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
