   }
 }
 
+// Robot calibration interface
+void carlosControlRobot()
+{
+  //print("Calibrating Robot\n");
+
+  // Update the joint position every 100 draw cycles
+  g_iJointUpdateTimerCounter++;
+  if ( (g_iJointUpdateTimerCounter%JOINT_UPDATE_TIMER_COUNTER_LIMIT) == 0) {
+
+    // Move the joint to the next PWM value
+    g_poDataMonster->moveJoint(g_iJointSelect, g_iPwmValue);
+
+    Serial.print("Controlingd | Joint: ");
+    Serial.print(g_iJointSelect);
+    Serial.print(" | Limit: LOW | PWM Value: ");
+    Serial.println(g_iPwmValue);
+  }
+}
+
 void getSerialCommand() {
   if (Serial.available() > 0) {
     // get incoming byte:
     g_iByte = Serial.read();
+    //Serial.println(g_iByte);
 
     if (g_iByte == UP) {
       g_iPwmValue += 1;
+      //g_fAngle += g_fOneDegInRad;
     } 
     else if (g_iByte == DOWN) {
       g_iPwmValue -= 1;
+      //g_fAngle -= g_fOneDegInRad;
     }
     else if (g_iByte == LEFT) {
+
       g_iPwmValue = 5;
+      //g_fAngle = 0;
       g_iJointCounter--;
     } 
     else if (g_iByte == RIGHT) {
+
       g_iPwmValue = 5;
+      //g_fAngle = 0;
       g_iJointCounter++;
     }
   }

    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);
  }

  g_poDataMonster->setPosture(location[0],  location[1],  location[2], location[3], location [4], g_bGotTweet);

#endif

  if(monsterTimer > 400)
    monsterTimer = 400;
  
}

///////////////////////////////
// Robot calibration interface
//////////////////////////////
void calibRobot()
{
  // Update the joint position every 100 draw cycles

  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];
  }  
