   
  ///// Set Dampening couter/reset
 dampening = (dampening - 0.0005);
-if (dampening < 0.6){
-  dampening = 0.6;
+if (dampening < 0.8){
+  dampening = 0.8;
 }
 if (g_bGotTweet == true){
- dampening = 1.0; 
+ dampening = .95; 
 }
 
 
   // Orient the robot towards the object
+  if(monsterTimer <=190){
   g_poDataMonster->setPosture(location[0],  location[1],  location[2], location[3], location [4], g_bGotTweet);
-
+  }
+  
 #endif
 
 }


}

void loop() {

  // RUN JOINT CALIBRATION ROUTINE
#ifdef CALIBRATING 

  /////////////////////////////////////////////
  // 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!

  } 
  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{
    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! 
  }
  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!
  }
  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!
  }
  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!
  }
  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
  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!
  }
  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!
  }
  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){
  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! 
  }
  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!
  }
  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!
  }
  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!
