Difference between revisions of "CRITR/Arm Control gen1"

From Lofaro Lab Wiki
Jump to: navigation, search
(→‎Controller)
(→‎Robot)
Line 53: Line 53:
  
 
==Robot==
 
==Robot==
−
  code
+
  /* 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;
 +
    }
 +
  }

Revision as of 18:06, 8 December 2015

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

 /* 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;
   }
 }
Retrieved from "http://wiki.lofarolabs.com/index.php?title=CRITR/Arm_Control_gen1&oldid=1815"