MRD Lab
Robotis Dynamixel MX-12W Servo Library
Dependents: SpindleBot_1_5b Utilisatio_MX12_V4
This is my attempt to adapt Chris Styles's AX12 library to work with my Dynamixel MX12 servos. This library is still very much a work in progress, and it may have some/many errors in it, but hopefully I will keep improving it to bring it up to snuff.
Dynamixel aficionados should also check out This MX28 library for a completely separate library that provides very similar functionality, and I wish I had known it existed before I started my work...
minimal example
#include "mbed.h"
#include "MX12.h"
int main() {
MX12 mymx12 (p9, p10, 1); // ID=1
while (1) {
mymx12.Set_Goal_Position(0); // go to 0 degrees
wait (2.0);
mymx12.Set_Goal_Position(300); // go to 300 degrees
wait (2.0);
}
}
Diff: MX12.cpp
- Revision:
- 5:4c118a827f11
- Parent:
- 4:6e320b7646ff
--- a/MX12.cpp Thu Jan 29 22:24:13 2015 +0000
+++ b/MX12.cpp Tue Feb 10 21:52:40 2015 +0000
@@ -8,13 +8,16 @@
MX12OD_Object MX12_OD[MX12_OD_SIZE];
bool MX12OD_Object_initalized;
-MX12::MX12(PinName tx, PinName rx, int ID, int baud_rate)
+MX12::MX12(PinName tx, PinName rx, PinName tx_enable_pin, PinName rx_enable_pin, int ID, int baud_rate)
: mx12_out(tx, NC),
mx12_in(NC, rx),
+ tx_enable(tx_enable_pin),
+ rx_enable(rx_enable_pin),
profileOut(LED4) {
mx12_out.baud(baud_rate);
mx12_in.baud(baud_rate);
_ID = ID;
+ _baud = baud_rate;
}
@@ -59,6 +62,31 @@
MX12OD_Object_initalized=true;
}
+// We shouldn't need to wait between setting pins,
+// since the switching time of the chip is ~6
+// nanoseconds, and a cpu cycle on the LPC1768
+// is ~10 nanoseconds
+void MX12::ChangeDir(MX12_Direction dir) {
+ if(dir==MX12_DIR_IN){
+ // Turn off transmit first
+ tx_enable=1;
+ // Then enable receive
+ rx_enable=0;
+ }else if(dir==MX12_DIR_OUT){
+ // Turn off receive first
+ rx_enable=1;
+ // Then enable transmit
+ tx_enable=0;
+ }else if(dir==MX12_DIR_NONE){
+ // Turn off both
+ tx_enable=1;
+ rx_enable=1;
+ }else{
+ // This shouldn't be possible!
+ printf("Error: Incorrect Direction choice!\n");
+ }
+}
+
// Set the mode of the servo
// 0 = Positional (0-300 degrees)
// 1 = Rotational -1 to 1 speed
@@ -124,11 +152,15 @@
if(scan_all_baud_rates){
int baud_rate[12]={3000000,2500000,2250000,1000000,500000,400000,250000,200000,115200,57600,19200,9600};
char ii;
- for(ii=0;ii<12;ii++){
- printf("baud rate=%d\n",baud_rate[ii]);
+ for(ii=0;ii<4;ii++){
ChangeUARTBaud(baud_rate[ii]);
for(ID_Num=0;ID_Num<=max_id;ID_Num++){
- printf("ID Num=%d\n",ID_Num);
+ //_ID=ID_Num;
+// char data[2];
+// data[1]=0;
+//
+// int ErrorCode = read(_ID, MX12_OD[MX12_REG_VERSION_OF_FIRMWARE].Address, MX12_OD[MX12_REG_VERSION_OF_FIRMWARE].Bytes, data);
+// if(ErrorCode!=0){
if(ping(ID_Num)){
printf("Found a servo at ID=%#02x, Baud=%d\n",ID_Num,baud_rate[ii]);
}
@@ -158,7 +190,7 @@
baud_int=252;
}else{
printf("Error! Invalid baud rate of %d!\n",target_baud);
- return 0;
+ return MX12_ERROR_RETURN;
}
#if MX12_DEBUG
@@ -195,6 +227,7 @@
// write the packet, return the error code
int rVal = write(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
+ if(rVal==MX12_ERROR_RETURN){return rVal;}
if(rVal>0){
if(CHECK_BIT(rVal, 0)){printf("Error! Input Voltage Error\n");}
if(CHECK_BIT(rVal, 1)){printf("Error! Angle Limit Error\n");}
@@ -211,8 +244,7 @@
short MX12:: read_short(MX12ODIndex OD)
{
- char data[2];
- data[1]=0;
+ char data[2]={0,0};
int ErrorCode = read(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
profileOut = (ErrorCode!=0);
@@ -233,7 +265,71 @@
+int MX12::read_raw(char* Status, int bytes) {
+ // Receive the Status packet 6+ number of bytes read
+ #if MX12_READ_DEBUG
+ printf(" Reading Byte:");
+ #endif
+ Timer serial_timeout;
+ serial_timeout.start ();
+
+ for (int i=0; i<(6+bytes) ; i++) {
+ serial_timeout.reset ();
+ while (!mx12_in.readable ())
+ {
+ //Just loop here until you either get a character, or we timeout
+ if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
+ {
+ //If we timeout, quit in a panic!
+ //printf("Error! Serial Timeout %d\n",i);
+ return(MX12_ERROR_RETURN);
+ }
+ }
+ Status[i] = mx12_in.getc();
+ }
+
+ if(Status[0]!=0xFF || Status[1]!=0xFF)
+ {
+ printf("Header Error!\n");
+ //printf("Unexpected header in serial response!\n");
+ //printf(" Header : 0x%x\n",Status[0]);
+ //printf(" Header : 0x%x\n",Status[1]);
+ return MX12_ERROR_RETURN;
+ }
+ if(Status[2]!=_ID){
+ printf("ID Error!\n");
+ return MX12_ERROR_RETURN;
+ }
+ if(Status[3]!=bytes+2){
+ printf("Length Error!\n");
+ return MX12_ERROR_RETURN;
+ }
+
+ char sum=Status[2]+Status[3]+Status[4]+Status[5];
+ if( bytes==2 )
+ sum+=Status[6];
+ if( Status[5+bytes]!=0xff-sum){
+ printf("Checksum Error!\n");
+ return MX12_ERROR_RETURN;
+ }
+
+ #if MX12_READ_DEBUG
+ printf("\nStatus Packet\n");
+ printf(" Header : 0x%x\n",Status[0]);
+ printf(" Header : 0x%x\n",Status[1]);
+ printf(" ID : 0x%x\n",Status[2]);
+ printf(" Length : 0x%x\n",Status[3]);
+ printf(" Error Code : 0x%x\n",Status[4]);
+ for (int i=0; i < bytes ; i++) {
+ printf(" Data : 0x%x\n",Status[5+i]);
+ }
+
+ printf(" Checksum : 0x%x\n",Status[5+bytes]);
+ #endif
+
+ return MX12_NORMAL_RETURN;
+}
int MX12::read(int ID, int start, int bytes, char* data) {
@@ -298,96 +394,45 @@
#endif
// Clear in input buffer first
- while (mx12_in.readable()) {
+ int buffer_purge_count=0;
+ while (mx12_in.readable() && buffer_purge_count < 16 ) {
mx12_in.getc();
- printf("Purging one character (read).\n");
+ buffer_purge_count++;
+ //printf("Purging one character (0x%x).\n",c);
}
+ if(buffer_purge_count > 1){ // One character is normal, I don't know why...
+ printf("Error: Purged %d characters from buffer.\n",buffer_purge_count);
+ return MX12_ERROR_RETURN;
+ }
+
+ // Change to output
+ ChangeDir(MX12_DIR_OUT);
+ wait_us(1); // Debounce
// Transmit the packet in one burst with no pausing
for (int i = 0; i<8 ; i++) {
mx12_out.putc(TxBuf[i]);
}
- // Read
- for (int i = 0; i<8 ; i++) {
- //profileOut=i%2;
- mx12_in.getc();
+ // Wait for the bytes to be transmitted
+ // This shouldn't overflow with 32 bit ints, but be aware
+ // us/s bytes b/byte baud
+ wait_us(1000000 * 8 * 8 / _baud);
+
+ // Change to input
+ ChangeDir(MX12_DIR_IN);
+ wait_us(1); // Debounce
+
+ if(ID!=0xFE){
+ if(read_raw(Status,bytes)!=MX12_NORMAL_RETURN){
+ return MX12_ERROR_RETURN;
+ }
}
- // Wait for the bytes to be transmitted
- //wait (0.00002);
-
- // Skip if the read was to the broadcast address
- if (_ID != 0xFE) {
-
- // Receive the Status packet 6+ number of bytes read
- #if MX12_READ_DEBUG
- printf(" Reading Byte:");
- #endif
- Timer serial_timeout;
- serial_timeout.start ();
-
- for (int i=0; i<(6+bytes) ; i++) {
- serial_timeout.reset ();
- while (!mx12_in.readable ())
- {
- //Just loop here until you either get a character, or we timeout
- if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
- {
- //If we timeout, quit in a panic!
- printf("Error! Serial Timeout %d\n",i);
- return(0x00);
- }
- }
- Status[i] = mx12_in.getc();
- }
-
- if(Status[0]!=0xFF || Status[1]!=0xFF)
- {
- printf("Header Error!\n");
- //printf("Unexpected header in serial response!\n");
- //printf(" Header : 0x%x\n",Status[0]);
- //printf(" Header : 0x%x\n",Status[1]);
- return 255;
- }
- if(Status[2]!=_ID){
- printf("ID Error!\n");
- return 255;
- }
- if(Status[3]!=bytes+2){
- printf("Length Error!\n");
- return 255;
- }
-
- char sum=Status[2]+Status[3]+Status[4]+Status[5];
- if( bytes==2 )
- sum+=Status[6];
- if( Status[5+bytes]!=0xff-sum){
- printf("Checksum Error!\n");
- return 255;
- }
-
- // Copy the data from Status into data for return
- for (int i=0; i < bytes ; i++) {
- data[i] = Status[5+i];
- }
-
- #if MX12_READ_DEBUG
- printf("\nStatus Packet\n");
- printf(" Header : 0x%x\n",Status[0]);
- printf(" Header : 0x%x\n",Status[1]);
- printf(" ID : 0x%x\n",Status[2]);
- printf(" Length : 0x%x\n",Status[3]);
- printf(" Error Code : 0x%x\n",Status[4]);
-
- for (int i=0; i < bytes ; i++) {
- printf(" Data : 0x%x\n",Status[5+i]);
- }
-
- printf(" Checksum : 0x%x\n",Status[5+bytes]);
- #endif
-
- } // if (ID!=0xFE)
+ // Copy the data from Status into data for return
+ for (int i=0; i < bytes ; i++) {
+ data[i] = Status[5+i];
+ }
return(Status[4]);
}
@@ -469,63 +514,33 @@
mx12_in.getc();
printf("Purging one character (write).\n");
}
-
+
+ // Change to output
+ ChangeDir(MX12_DIR_OUT);
+ wait_us(1);
+
// Transmit the packet in one burst with no pausing
- for (int i = 0; i < (7 + bytes) ; i++) {
+ for (int i = 0; i< (7 + bytes) ; i++) {
mx12_out.putc(TxBuf[i]);
- mx12_in.getc();
- //printf("Echo: 0x%02x\n",mx12_in.getc());
}
+
+ // Wait for the bytes to be transmitted
+ // This shouldn't overflow with 32 bit ints, but be aware
+ // us/s bytes b/byte baud fudge
+ wait_us(1000000 * (7 + bytes) * 8 / _baud + 1);
+
+ // Change to input
+ ChangeDir(MX12_DIR_IN);
+ wait_us(1);
- // make sure we have a valid return
- Status[4]=0x00;
+ // make sure we have an invalid return
+ Status[4]=MX12_ERROR_RETURN;
// we'll only get a reply if it was not broadcast
- if (_ID!=0xFE) {
-
- // response is always 6 bytes
- // 0xFF, 0xFF, ID, Length, Error, Checksum
- Timer serial_timeout;
- serial_timeout.start ();
-
- for (int i=0; i < 6 ; i++) {
- serial_timeout.reset ();
- while (!mx12_in.readable ())
- {
- //Just loop here until you either get a character, or we timeout
- if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
- {
- //If we timeout, quit in a panic!
- printf("Error! Serial Timeout %d\n",i);
- return(0x00);
- }
- }
- Status[i] = mx12_in.getc();
+ if(ID!=0xFE){
+ if(read_raw(Status,0)!=MX12_NORMAL_RETURN){
+ return MX12_ERROR_RETURN;
}
-
- if(Status[0]!=0xFF || Status[1]!=0xFF)
- {
- printf("Unexpected header in serial response!\n");
- printf(" Header : 0x%x\n",Status[0]);
- printf(" Header : 0x%x\n",Status[1]);
- return 255;
- }
- if( Status[5]!=0xff-(Status[2]+Status[3]+Status[4])){
- printf("Checksum Error!\n");
- return 255;
- }
-
-
- // Build the TxPacket first in RAM, then we'll send in one go
- #if MX12_WRITE_DEBUG
- printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
- printf(" ID : %d\n",Status[2]);
- printf(" Length : %d\n",Status[3]);
- printf(" Error : 0x%x\n",Status[4]);
- printf(" Checksum : 0x%x\n",Status[5]);
- #endif
-
-
}
return(Status[4]); // return error code
@@ -644,16 +659,21 @@
// Transmit the packet in one burst with no pausing
offset=7+NumDevices*(DataLength+0x1)+0x1; // one more for the checksum
- for (ii = 0; ii<offset ; ii++) {
- mx12_out.putc(TxBuf[ii]);
- mx12_in.getc();
- //printf("0x%x",TxBuf[ii]);
+
+
+ // Change to output
+ ChangeDir(MX12_DIR_OUT);
+ wait_us(1);
+
+ // Transmit the packet in one burst with no pausing
+ for (int i = 0; i< offset ; i++) {
+ mx12_out.putc(TxBuf[i]);
}
- // Read
- for (ii = 0; ii<offset ; ii++) {
- //profileOut=ii%2;
- }
+ // Wait for the bytes to be transmitted
+ // This shouldn't overflow with 32 bit ints, but be aware
+ // us/s bytes b/byte baud
+ wait_us(1000000 * offset * 8 / _baud);
}
void MX12::trigger(void) {
@@ -766,18 +786,31 @@
#if MX12_TRIGGER_DEBUG
printf(" Checksum 0x%X\n",TxBuf[5]);
#endif
+
+
+ // Clear in input buffer first
+ while (mx12_in.readable()) {
+ mx12_in.getc();
+ printf("Purging one character (ping).\n");
+ }
+ // Change to output
+ ChangeDir(MX12_DIR_OUT);
+ wait_us(1);
+
// Transmit the packet in one burst with no pausing
- for (int i = 0; i < 6 ; i++) {
+ for (int i = 0; i<6 ; i++) {
mx12_out.putc(TxBuf[i]);
}
- // Read
- for (int i = 0; i < 6 ; i++) {
- //profileOut=i%2;
- mx12_in.getc();
- }
-
+ // Wait for the bytes to be transmitted
+ // This shouldn't overflow with 32 bit ints, but be aware
+ // us/s bytes b/byte baud fudge
+ wait_us(1000000 * 6 * 8 / _baud + 1);
+
+ // Change to input
+ ChangeDir(MX12_DIR_IN);
+ wait_us(1);
// response is always 6 bytes
// 0xFF, 0xFF, ID, Length, Error, Checksum