   /////////////////////////////////////////////
   // Set Robot
   /////////////////////////////////////////////
-  monsterTimer = monsterTimer + 1;
-  
   //////////////////HARDCODING VALUES TO JOINTS BASED ON COMMON SENSOR VALUES BELOW////////////////////////
   // X maps to joint 0
-  if(g_fX > -.1){
-    iPwmValue = mapfloat(g_fX, -.1, -0.6, g_poDataMonster->m_apJoinArray[0]->m_fPwmMin, g_poDataMonster->m_apJoinArray[0]->m_fPwmMin + 5);  ///////<-hardcoded!
+  if(g_fX < -.1){
+    iPwmValue = mapfloat(g_fX, -.1, -0.6, g_poDataMonster->m_apJoinArray[0]->m_fPwmMin, g_poDataMonster->m_apJoinArray[0]->m_fPwmMin + 50);  ///////<-hardcoded!
 
   } 
-  else if(g_fX < 0.1){
-    iPwmValue = mapfloat(g_fX, 0.1, 0.6, g_poDataMonster->m_apJoinArray[0]->m_fPwmMax , g_poDataMonster->m_apJoinArray[0]->m_fPwmMax - 5); ///////<-hardcoded!
+  else if(g_fX > 0.1){
+    iPwmValue = mapfloat(g_fX, 0.1, 0.6, g_poDataMonster->m_apJoinArray[0]->m_fPwmMax , g_poDataMonster->m_apJoinArray[0]->m_fPwmMax - 50); ///////<-hardcoded!
   }
   else{
     iPwmValue = mapfloat(g_fX, SENSOR_X_MIN, SENSOR_X_MAX, g_poDataMonster->m_apJoinArray[0]->m_fPwmMin, g_poDataMonster->m_apJoinArray[0]->m_fPwmMax);
   }
- /////////////////////////////////////////////////// 
-  ////what to do
-  if (monsterTimer <= 160){
   target[0]= iPwmValue;
- // Serial.println(monsterTimer);
-  }
-    if ((target[0] - iPwmValue > 2.5) || (target[0] - iPwmValue < -2.5 )){ 
- monsterTimer =0;
-    }
-// Serial.println(monsterTimer);
-  
-  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 
   // Z maps to joint 1
   if(g_fZ >= 0 && g_fZ < 0.1){
-    iPwmValue = mapfloat(g_fX, 0, (.1), g_poDataMonster->m_apJoinArray[1]->m_fPwmMin, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax-10);  ///////<-hardcoded! 
+    iPwmValue = mapfloat(g_fX, 0, (.1), g_poDataMonster->m_apJoinArray[1]->m_fPwmMin, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax-50);  ///////<-hardcoded! 
   }
   else if(g_fZ >= 0.1 && g_fZ< 0.2){
-    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[1]->m_fPwmMin, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax-10);  ///////<-hardcoded!
+    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[1]->m_fPwmMin + 50, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax);  ///////<-hardcoded!
   }
   else if(g_fZ >= 0.2 && g_fZ< 0.3){
-    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[1]->m_fPwmMin, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax-5);  ///////<-hardcoded!
+    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[1]->m_fPwmMin + 30, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax);  ///////<-hardcoded!
   }
   else if(g_fZ >= 0.3 && g_fZ< 0.6){
-    iPwmValue = mapfloat(g_fZ, 0.3, 0.6, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax-10, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax);  ///////<-hardcoded!
+    iPwmValue = mapfloat(g_fZ, 0.3, 0.6, g_poDataMonster->m_apJoinArray[1]->m_fPwmMin, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax);  ///////<-hardcoded!
   }
   else{
     iPwmValue = mapfloat(g_fZ, SENSOR_Z_MIN, SENSOR_Z_MAX, g_poDataMonster->m_apJoinArray[1]->m_fPwmMin, g_poDataMonster->m_apJoinArray[1]->m_fPwmMax);
   }
-  
-  //////////////////what to do
-   if (monsterTimer <= 105){
   target[1]= iPwmValue;
-  }
-  else if (monsterTimer <= 165){
-    target[1] = g_poDataMonster->m_apJoinArray[1]->m_fPwmMin;
-  }
-else{
- target[1] = g_poDataMonster->m_apJoinArray[1]->m_fPwmMax; 
-}
-    
-
-  
-
-
-
-  
-  
-  
-  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-//  // Z maps to joint 2
+
+  // Z maps to joint 2
   if(g_fZ >= 0 && g_fZ < 0.1){
-    iPwmValue = mapfloat(g_fZ, 0, (.1), g_poDataMonster->m_apJoinArray[2]->m_fPwmMax-10, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin);  ///////<-hardcoded! 
-  }
-  else if(g_fZ >= 0.1 && g_fZ< 0.3){
-    iPwmValue = mapfloat(g_fZ, 0.1, 0.3, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax-5, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin);  ///////<-hardcoded!
+    iPwmValue = mapfloat(g_fZ, 0, (.1), g_poDataMonster->m_apJoinArray[2]->m_fPwmMin, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax-50);  ///////<-hardcoded! 
+  }
+  else if(g_fZ >= 0.1 && g_fZ< 0.2){
+    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin + 50, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax);  ///////<-hardcoded!
+  }
+  else if(g_fZ >= 0.2 && g_fZ< 0.3){
+    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin + 30, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax);  ///////<-hardcoded!
   }
   else if(g_fZ >= 0.3 && g_fZ< 0.6){
-    iPwmValue = mapfloat(g_fZ, 0.3, 0.6, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin);  ///////<-hardcoded!
+    iPwmValue = mapfloat(g_fZ, 0.3, 0.6, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax);  ///////<-hardcoded!
   }
   else{
-    iPwmValue = mapfloat(g_fZ, SENSOR_Z_MIN, SENSOR_Z_MAX, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin+5);
-  }
-  if (target[1] < g_poDataMonster->m_apJoinArray[1]->m_fPwmMax-10)
-  target[2] = target[1]; 
-
-  
-   if (monsterTimer <= 120){
+    iPwmValue = mapfloat(g_fZ, SENSOR_Z_MIN, SENSOR_Z_MAX, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax);
+
+  }
   target[2]= iPwmValue;
-  }
-  else {
-    target[2] = g_poDataMonster->m_apJoinArray[1]->m_fPwmMin;
-  }
- 
-  
-  
-
-  
-  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
   // Z maps to joint 3
   if(g_fZ >= 0 && g_fZ < 0.1){
-    iPwmValue = mapfloat(g_fZ, 0, (.1), g_poDataMonster->m_apJoinArray[3]->m_fPwmMax, g_poDataMonster->m_apJoinArray[3]->m_fPwmMax-10);  ///////<-hardcoded! 
+    iPwmValue = mapfloat(g_fZ, 0, (.1), g_poDataMonster->m_apJoinArray[3]->m_fPwmMin, g_poDataMonster->m_apJoinArray[3]->m_fPwmMax);  ///////<-hardcoded! 
   }
   else if(g_fZ >= 0.1 && g_fZ< 0.2){
-    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[3]->m_fPwmMax, g_poDataMonster->m_apJoinArray[3]->m_fPwmMin+10);  ///////<-hardcoded!
+    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[3]->m_fPwmMin, g_poDataMonster->m_apJoinArray[3]->m_fPwmMax);  ///////<-hardcoded!
   }
   else if(g_fZ >= 0.2 && g_fZ< 0.3){
-    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[3]->m_fPwmMax, g_poDataMonster->m_apJoinArray[3]->m_fPwmMax-10);  ///////<-hardcoded!
+    iPwmValue = mapfloat(g_fZ, 0.1, 0.2, g_poDataMonster->m_apJoinArray[3]->m_fPwmMin, g_poDataMonster->m_apJoinArray[3]->m_fPwmMax);  ///////<-hardcoded!
   }
   else if(g_fZ >= 0.3 && g_fZ< 0.6){
     iPwmValue = mapfloat(g_fZ, 0.3, 0.6, g_poDataMonster->m_apJoinArray[3]->m_fPwmMin, g_poDataMonster->m_apJoinArray[3]->m_fPwmMax);  ///////<-hardcoded!


}

void loop(){ 

  // RUN JOINT CALIBRATION ROUTINE
#ifdef CALIBRATING 

  ////////////////////////////dampening value (1= no dampening, 0 = full dampening (no movement)) Usually .9 or so.
  dampening = 0.9;
  //////////////////////////////////////////////////////////////////////

  for (int i=0; i<5; i++){
    target[i] = 0.1;
