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

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

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

  if (packetSize)


void loop() {
  float angle = servo.getAngle();  // get angle of servo (0 - 180)
  readCAN(0x01);
  readCAN(0x02);
  bool positionControl = true;
  float torque_extinction_ratio = 1;
  if (positionControl)
  {
