     setpoint = setpoint + constrain(order-setpoint, -slew_rate * dt_ms/1000., slew_rate * dt_ms/1000.);
     
     setCommand(0x01, setpoint, 0.0, 25, 5, 0*setpoint);
+    setCommand(0x02, setpoint, 0.0, 25, 5, 0*setpoint);
   }
   else // torque control
   {

  return (int) ((x-x_min)*((float)((1<<bits)-1)/span));
}

void readCAN()
{
  int packetSize = CAN.parsePacket();

  if (packetSize)

  float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
  float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
  float i = uint_to_float(i_int, -T_MAX, T_MAX, 12);
//if (id==1)
  {
   position = p;
  
   float speed = v;
   torque = i;
   Serial.print("position");Serial.print(id);Serial.print(":"); Serial.print(position); Serial.print(", ");
   Serial.print("speed");Serial.print(id);Serial.print(":"); Serial.print(speed); Serial.print(", ");
   Serial.print("intensity");Serial.print(id);Serial.print(":"); Serial.print(torque); Serial.print(", ");
   }
 }

