Difference between revisions of "CRITR/Arm Control gen1"
From Lofaro Lab Wiki
(→Robot) |
|||
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) |
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; } }