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


void loop() {
  float angle = servo.getAngle();  // get angle of servo (0 - 180)
  

  order = mapfloat(angle, 0, 180, P_MIN, P_MAX);
  Serial.print("order:"); Serial.print(order); Serial.print(", ");
  setpoint = setpoint + constrain(order-setpoint, -slew_rate * dt_ms/1000., slew_rate * dt_ms/1000.);
  Serial.print("setpoint:"); Serial.print(setpoint); Serial.print(", ");
  setCommand(0x01, setpoint, 0.0, 25, 5, 0*setpoint);
  readCAN();
  Serial.println("");
  delay(dt_ms);
  //

  float i = uint_to_float(i_int, -T_MAX, T_MAX, 12);
  if (id==1)
  {
   position = p;
  
   float speed = v;
   float torque = i;
