Difference between revisions of "CRITR/Arm Control gen1"
From Lofaro Lab Wiki
(Created page with "Notes : *Opencm9.04 Micro-controller is used. ==Controller== code ==Robot== code") |
(→Controller) |
||
Line 3: | Line 3: | ||
==Controller== | ==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== | ==Robot== | ||
code | code |
Revision as of 18:05, 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
code