CRITR/Arm Control gen1

Notes :

• Opencm9.04 Micro-controller is used.

Controller

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

 /* 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];
 int pos1 = 0;
 int pos9 = 0;
 int load9 = 0;
 int pos18 = 0;
 int load18 = 0;
 int posp = 0;
 char str[36];
 int state = 0;
 int cstate = 0;
 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.maxTorque(J_ID,1); // it has maxtorque for weak movement  
 //  Dxl.jointMode(J_ID); //jointMode() is to use position mode  
 }
 void usbInterrupt(byte* buffer, byte nCount)
 { 
  state =  buffer[0]-48;
   int n = 0;
   int id= 0;
   int number = 0;
   for(unsigned int i=5; i < nCount;i++){  //printf_SerialUSB_Buffer[N]_receive_Data
     number = (char)buffer[i] - '0';
     if (number > 39){
         number = number-39;
       }      
     if(n == 0){
       n = 1;
       pos[id] = number*36;
     }
     else{
       n=0;
       pos[id] = pos[id]+number;
       if(abs(pos[id]-prev[id])>2){
         Dxl.goalPosition(id+1, pos[id]);
         prev[id] = pos[id];
       }
       id++;
     }      
   }
 }
 
  
 void loop() {
     if(cstate != state){
     if(state == 1){ // fold
       if(cstate ==0){
       Dxl.goalSpeed(0xFE, 300);
       for (int i=1; i<21; i++){
         Dxl.goalPosition(i,512);
       }
       Dxl.goalPosition(4, 614);
       Dxl.goalPosition(13, 400);
       delay(1000);
       Dxl.goalPosition(19, 204);  
       delay(1000);
       Dxl.goalPosition(5, 204);
       Dxl.goalPosition(14, 817);
       delay(1000);
       Dxl.goalPosition(6, 817);
       Dxl.goalPosition(4, 0);
       Dxl.goalPosition(15, 204);
       Dxl.goalPosition(13, 1023);
       delay(1000);
       Dxl.goalPosition(2, 817);
       Dxl.goalPosition(11, 204);
       delay(1000);
       Dxl.goalPosition(5, 512);
       Dxl.goalPosition(14, 512);
       delay(1000);
       Dxl.goalPosition(19, 512);
       Dxl.goalPosition(2,900);
       Dxl.goalPosition(11,111);
       Dxl.goalPosition(6, 900);
       Dxl.goalPosition(15,111);
       delay(1000);
       }
       Dxl.goalPosition(21, 250);
       Dxl.goalPosition(20, 512);
       cstate = 1;
     }    
     else if(state == 0){
         Dxl.goalSpeed(0xFE, 300);
         Dxl.goalPosition(21, 512);
         delay(1000);
         Dxl.goalPosition(2,817);
         Dxl.goalPosition(11,204);
         Dxl.goalPosition(6, 817);
         Dxl.goalPosition(15,204);
         Dxl.goalPosition(19, 204);
         delay(1000);
         Dxl.goalPosition(5, 204);
         Dxl.goalPosition(14, 817);
         delay(1000);
         Dxl.goalPosition(2, 512);
         Dxl.goalPosition(11, 512);
        delay(1000);
         Dxl.goalPosition(4,614);
         Dxl.goalPosition(13,400);
         delay(300);
         Dxl.goalPosition(6, 512);
         Dxl.goalPosition(15, 512);
         delay(700);
         Dxl.goalPosition(5, 512);
         Dxl.goalPosition(14, 512);
         delay(1000);
         Dxl.goalPosition(19, 512);
         delay(1000);
         cstate = 0;
     }
     else if(state == 2){ 
       if(cstate == 0){// fold
       Dxl.goalSpeed(0xFE, 300);
       for (int i=1; i<21; i++){
         Dxl.goalPosition(i,512);
       }
       Dxl.goalPosition(4, 614);
       Dxl.goalPosition(13, 400);
       delay(1000);
       Dxl.goalPosition(19, 204);  
       delay(1000);
       Dxl.goalPosition(5, 204);
       Dxl.goalPosition(14, 817);
       delay(1000);
       Dxl.goalPosition(6, 817);
       Dxl.goalPosition(4, 0);
       Dxl.goalPosition(15, 204);
       Dxl.goalPosition(13, 1023);
       delay(1000);
       Dxl.goalPosition(2, 817);
       Dxl.goalPosition(11, 204);
       delay(1000);
       Dxl.goalPosition(5, 512);
       Dxl.goalPosition(14, 512);
       delay(1000);
       Dxl.goalPosition(19, 512);
       Dxl.goalPosition(2,900);
       Dxl.goalPosition(11,111);
       Dxl.goalPosition(6, 900);
       Dxl.goalPosition(15,111);
       delay(1000);
       }
       Dxl.goalPosition(21, 750);
       Dxl.goalPosition(20,820);
       cstate = 2;
     }    
   }
   delay(50);
   pos1 = Dxl.readWord(1, 37); 
   load9 = Dxl.readWord(9, 41);
   pos9 = Dxl.readWord(9, 37); 
   load18 = Dxl.readWord(18, 41);
   pos18 = Dxl.readWord(18, 37);   
   if(load9 >700){
     Dxl.goalPosition(9, pos9);
     pos[8] = pos9;
   }
   if(load18 >1700){
     Dxl.goalPosition(18, pos18);
     pos[17] = pos18;
   }
 }