CRITR/Arm Control gen1

Notes :

• Opencm9.04 Micro-controller is used.

Controller

 /* Dynamixel ID Change Example
  */
  /* Serial device defines for dxl bus */
 #define DXL_BUS_SERIAL1 1  //Dynamixel on Serial1(USART1)  <-OpenCM9.04
 #define DXL_BUS_SERIAL2 2  //Dynamixel on Serial2(USART2)  <-LN101,BT210
 #define DXL_BUS_SERIAL3 3  //Dynamixel on Serial3(USART3)  <-OpenCM 485EXP
 /* Dynamixel ID defines */
 #define J_ID 1
 #define PRESENT_POS 54
 Dynamixel Dxl(DXL_BUS_SERIAL1); 
 int sum;
 int num;
 int pos[20], prev[20];
 char line;
 void setup() {
   // Initialize the dynamixel bus:
   // Dynamixel 2.0 Baudrate -> 0: 9600, 1: 57600, 2: 115200, 3: 1Mbps  
   SerialUSB.attachInterrupt(usbInterrupt);
    Dxl.begin(3);
 // Dxl.writeWord(0xFE, 24, 1);
 // Dxl.goalTorque(0xFE, 200);
 //  Dxl.maxTorque(J_ID,1); // it has maxtorque for weak movement  
 //  Dxl.jointMode(J_ID); //jointMode() is to use position mode    Dxl.jointMode(J_ID); //jointMode() is to use position mode  
 }
 void usbInterrupt(byte* buffer, byte nCount)
 {  
     num = (int)buffer[0];
     sum = 0;
     for(int i=0; i<num; i++){
       SerialUSB.print(pos[i]);
       SerialUSB.print(" ");
       sum +=pos[i];
     }
      SerialUSB.println(sum);
 } 
 void loop() {
   // Wait for 20 milliseconds
   for(int i=0; i<num; i++){
     pos[i] = Dxl.readWord(i+1, PRESENT_POS);
     if (abs(pos[i] - prev[i]) < 1 ){
       pos[i] = prev[i];
     }
     if (pos[i] > 1023){
       pos[i] = prev[i];
     }
     prev[i] = pos[i];
   }
 }

Robot

code