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

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


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

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

  if (packetSize)
