     if (m1_des_angle < -90) m1_des_angle = -90;
 
     m1_pid();
+    moveFootServo(rightFootServo, 90+m1_des_angle);
   }
   else {
     set_motor_speed(0);

// TODO Rea:
// Write code for reading current sensor (output Amp) : [done]
// Add code for foot servo: [done]
// Work on ways of adding the current reading to over current protection: [done]
// Work on ways of adding the current reading to the control loop (Motor): [....TODO...]

  


#define PC_COMM_DEBUG
//#define MOTOR_DEBUG
//#define SERVO_DEBUG
//#define ENCODER_DEBUG
//#define INNER_LOOP_DEBUG
#define CUR_SENSE_DEBUG
