Difference between revisions of "CRITR/Arm Control gen1"
From Lofaro Lab Wiki
(→Controller) |
|||
(One intermediate revision by the same user not shown) | |||
Line 3: | Line 3: | ||
==Controller== | ==Controller== | ||
− | |||
− | |||
/* Serial device defines for dxl bus */ | /* Serial device defines for dxl bus */ | ||
#define DXL_BUS_SERIAL1 1 //Dynamixel on Serial1(USART1) <-OpenCM9.04 | #define DXL_BUS_SERIAL1 1 //Dynamixel on Serial1(USART1) <-OpenCM9.04 | ||
Line 10: | Line 8: | ||
#define DXL_BUS_SERIAL3 3 //Dynamixel on Serial3(USART3) <-OpenCM 485EXP | #define DXL_BUS_SERIAL3 3 //Dynamixel on Serial3(USART3) <-OpenCM 485EXP | ||
/* Dynamixel ID defines */ | /* Dynamixel ID defines */ | ||
− | |||
− | |||
Dynamixel Dxl(DXL_BUS_SERIAL1); | Dynamixel Dxl(DXL_BUS_SERIAL1); | ||
int sum; | int sum; | ||
Line 22: | Line 18: | ||
SerialUSB.attachInterrupt(usbInterrupt); | SerialUSB.attachInterrupt(usbInterrupt); | ||
Dxl.begin(3); | Dxl.begin(3); | ||
− | |||
− | |||
− | |||
− | |||
} | } | ||
void usbInterrupt(byte* buffer, byte nCount) | void usbInterrupt(byte* buffer, byte nCount) | ||
Line 53: | Line 45: | ||
==Robot== | ==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; | ||
+ | } | ||
+ | } |
Latest revision as of 18:07, 8 December 2015
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; } }