   
    float speed = v;
    float torque = i;
-   Serial.print(position);
-   Serial.print("rad ");
-   Serial.print(speed);
-   Serial.print("rad/s ");
-   Serial.print(torque);
-   Serial.println("A");
+   Serial.print("position:"); Serial.print(position); Serial.print(", ");
+   Serial.print("speed:"); Serial.print(speed); Serial.print(", ");
+   Serial.print("intensity:"); Serial.print(torque); Serial.print(", ");
    }
  }
 

float Kd_MAX = 5;
float Test_Pos = 0.0;

 float pos = 0;
ServoInputPin<6> servo;
void setup() {
  Serial.begin(9600);
  //while (!Serial);
  delay(3000);

  Serial.println("CAN Sender");

  // start the CAN bus at 500 kbps
  if (!CAN.begin(1000E3)) {
    Serial.println("Starting CAN failed!");
    while (1);
  }

  setMotormode(0x01);
  setZero(0x01);
  //exitMotormode(0x01);
}

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)

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

