  float slew_rate = 20;
  float torque_slew_rate = 30;
  float position = 0;
+ float torque = 0;
+ float speed_order = 0;
 ServoInputPin<6> servo;
 void setup() {
   Serial.begin(9600);

          
      }
    }
    setCommand(0x01, setpoint, 0.0, 25*kp_extinction_ratio, 1, torque_setpoint*torque_extinction_ratio);
  }
  Serial.print("setpoint:"); Serial.print(setpoint); Serial.print(", ");
  Serial.print("torque_setpoint:"); Serial.print(torque_setpoint*torque_extinction_ratio); Serial.print(", ");

   position = p;
  
   float speed = v;
   torque = i;
   Serial.print("position:"); Serial.print(position); Serial.print(", ");
   Serial.print("speed:"); Serial.print(speed); Serial.print(", ");
   Serial.print("intensity:"); Serial.print(torque); Serial.print(", ");
