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); } }
Revision 5:4c118a827f11, committed 2015-02-10
- Comitter:
- labmrd
- Date:
- Tue Feb 10 21:52:40 2015 +0000
- Parent:
- 4:6e320b7646ff
- Commit message:
- Now with buffer chip!
Changed in this revision
MX12.cpp | Show annotated file Show diff for this revision Revisions of this file |
MX12.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 6e320b7646ff -r 4c118a827f11 MX12.cpp --- 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
diff -r 6e320b7646ff -r 4c118a827f11 MX12.h --- a/MX12.h Thu Jan 29 22:24:13 2015 +0000 +++ b/MX12.h Tue Feb 10 21:52:40 2015 +0000 @@ -68,17 +68,28 @@ #define MX12_MODE_POSITION 0 #define MX12_MODE_ROTATION 1 +// For now we don't have a both option, since no +// one should use that anyways. +enum MX12_Direction { + MX12_DIR_IN, + MX12_DIR_OUT, + MX12_DIR_NONE +}; + #define MX12_CW 1 #define MX12_CCW 0 #define MX12_INSTRUCTION_HEADER 0xff +#define MX12_ERROR_RETURN 255 +#define MX12_NORMAL_RETURN 1 + // The max delay should be 508us according to: // http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_05 // And a max character should be 833 us according to: // (1 byte) / (9600 (bits per second)) = 833 microseconds // So 1000 should be a decent value, because 9600 bps is for chumps. -#define MAX_DELAY_BETWEEN_CHARCTERS_IN_US 500 +#define MAX_DELAY_BETWEEN_CHARCTERS_IN_US 1500 #ifndef M_PI #define M_PI 3.14159265358979323846 /* pi */ @@ -119,9 +130,11 @@ * @param pin rx pin * @param int ID, the Bus ID of the servo 1-255 */ - MX12(PinName tx, PinName rx, int ID, int baud_rate=1000000); + MX12(PinName tx, PinName rx, PinName tx_enable_pin, PinName rx_enable_pin, int ID, int baud_rate=1000000); void Init(void); + + void ChangeDir(MX12_Direction dir); /** clockwise Angle Limit @retval CW Angle Limit in Degrees */ float Get_CW_Angle_Limit(void){return 0.087891*read_short(MX12_REG_CW_ANGLE_LIMIT);} /** counterclockwise Angle Limit @retval CCW Angle Limit in Degrees */ float Get_CCW_Angle_Limit(void){return 0.087891*read_short(MX12_REG_CCW_ANGLE_LIMIT);} @@ -289,10 +302,14 @@ //// Variables Serial mx12_out; Serial mx12_in; + DigitalOut tx_enable; + DigitalOut rx_enable; DigitalOut profileOut; int _ID; + int _baud; //// Functions + int read_raw(char* Status, int bytes=0); int read(int ID, int start, int length, char* data); int write(int ID, int start, int length, char* data);