   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
   //  // Z maps to joint 2
-  target[1]= iPwmValue;
-
-  // 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_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!
+    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!
   }
   else if(g_fZ >= 0.3 && g_fZ< 0.6){
-    iPwmValue = mapfloat(g_fZ, 0.3, 0.6, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax);  ///////<-hardcoded!
+    iPwmValue = mapfloat(g_fZ, 0.3, 0.6, g_poDataMonster->m_apJoinArray[2]->m_fPwmMax, g_poDataMonster->m_apJoinArray[2]->m_fPwmMin);  ///////<-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[3]= iPwmValue;
  }
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  target[3]= iPwmValue;

  // Y maps to joint 4
  if(g_fZ >= 0 && g_fZ < 0.1){
    iPwmValue = mapfloat(g_fY, 0, (.1), g_poDataMonster->m_apJoinArray[4]->m_fPwmMin, g_poDataMonster->m_apJoinArray[4]->m_fPwmMax-50);  ///////<-hardcoded! 
  }
  else if(g_fZ >= 0.1 && g_fZ< 0.2){
    iPwmValue = mapfloat(g_fY, 0.1, 0.2, g_poDataMonster->m_apJoinArray[4]->m_fPwmMin + 50, g_poDataMonster->m_apJoinArray[4]->m_fPwmMax);  ///////<-hardcoded!
  }
  else if(g_fZ >= 0.2 && g_fZ< 0.3){
    iPwmValue = mapfloat(g_fY, 0.1, 0.2, g_poDataMonster->m_apJoinArray[4]->m_fPwmMin + 30, g_poDataMonster->m_apJoinArray[4]->m_fPwmMax);  ///////<-hardcoded!
  }
  else if(g_fZ >= 0.3 && g_fZ< 0.6){
    iPwmValue = mapfloat(g_fY, 0.3, 0.6, g_poDataMonster->m_apJoinArray[4]->m_fPwmMin, g_poDataMonster->m_apJoinArray[4]->m_fPwmMax);  ///////<-hardcoded!
  }
  else{
    iPwmValue = mapfloat(g_fY, SENSOR_X_MIN, SENSOR_X_MAX, g_poDataMonster->m_apJoinArray[4]->m_fPwmMin, g_poDataMonster->m_apJoinArray[4]->m_fPwmMax);

  }

  if (monsterTimer <= 90){

  else {
    target[4]= g_poDataMonster->m_apJoinArray[4]->m_fPwmMax-2;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  //Dampening code

  for (int i=0; i<5; i++){
    magnitude[i] = target[i] - location[i];
  }  
