Fork from Dynamixel AX12 Servo for MX64 use and not-finishi now
Dependents: 2014-Mx64 2014-mx64-test
Fork of AX12 by
This Library was Fork From Chris Styles's AX12 Library .
Dynamixel MX64 Servo
MX64 is like a new generation Dynamixel Servo Use TTL 2.0 bus, Half-duplex Serial just like a pro-version AX12
Quote:
The MX-64T Dynamixel Robot Servo Actuator is the newest generation of Robotis Dynamixel actuator; equipped with an onboard 32bit 72mhz Cortex M3, a contact-less magnetic encoder with 4x the resolution over the AX/RX series, and up to 3mpbs using the new TTL 2.0 bus. Each servo has the ability to track its speed, temperature, shaft position, voltage, and load.
You need this Dependence Library SerialHalfDuplex library too
Import librarySerialHalfDuplex
Serial Half Duplex implementation
Revision 2:5ea99c37a2d7, committed 2011-04-10
- Comitter:
- chris
- Date:
- Sun Apr 10 20:58:21 2011 +0000
- Parent:
- 1:93ad80f5fde7
- Child:
- 3:ced71d1b2558
- Commit message:
- Revised read/write function code to prevent hangs when a response packet is missed.
Converted debug statements to #ifdef statements
Added additional parameter to constructor to pass in baud rate
Changed in this revision
AX12.cpp | Show annotated file Show diff for this revision Revisions of this file |
AX12.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/AX12.cpp Wed Mar 30 16:08:02 2011 +0000 +++ b/AX12.cpp Sun Apr 10 20:58:21 2011 +0000 @@ -24,11 +24,12 @@ #include "AX12.h" #include "mbed.h" -AX12::AX12(PinName tx, PinName rx, int ID) +AX12::AX12(PinName tx, PinName rx, int ID, int baud) : _ax12(tx,rx) { + _baud = baud; + _ID = ID; + _ax12.baud(_baud); - _ax12.baud(1000000); - _ID = ID; } // Set the mode of the servo @@ -64,9 +65,9 @@ // 1023 / 300 * degrees short goal = (1023 * degrees) / 300; - if (AX12_DEBUG) { - printf("SetGoal to 0x%x\n",goal); - } +#ifdef AX12_DEBUG + printf("SetGoal to 0x%x\n",goal); +#endif data[0] = goal & 0xff; // bottom 8 bits data[1] = goal >> 8; // top 8 bits @@ -109,13 +110,13 @@ int AX12::SetCWLimit (int degrees) { char data[2]; - + // 1023 / 300 * degrees short limit = (1023 * degrees) / 300; - if (AX12_DEBUG) { - printf("SetCWLimit to 0x%x\n",limit); - } +#ifdef AX12_DEBUG + printf("SetCWLimit to 0x%x\n",limit); +#endif data[0] = limit & 0xff; // bottom 8 bits data[1] = limit >> 8; // top 8 bits @@ -132,9 +133,9 @@ // 1023 / 300 * degrees short limit = (1023 * degrees) / 300; - if (AX12_DEBUG) { - printf("SetCCWLimit to 0x%x\n",limit); - } +#ifdef AX12_DEBUG + printf("SetCCWLimit to 0x%x\n",limit); +#endif data[0] = limit & 0xff; // bottom 8 bits data[1] = limit >> 8; // top 8 bits @@ -148,9 +149,11 @@ char data[1]; data[0] = NewID; - if (AX12_DEBUG) { - printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); - } + +#ifdef AX12_DEBUG + printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); +#endif + return (write(CurrentID, AX12_REG_ID, 1, data)); } @@ -170,14 +173,11 @@ char TxBuf[16]; char sum = 0; - if (AX12_TRIGGER_DEBUG) { - printf("\nTriggered\n"); - } - +#ifdef AX12_TRIGGER_DEBUG // Build the TxPacket first in RAM, then we'll send in one go - if (AX12_TRIGGER_DEBUG) { - printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); - } + printf("\nTriggered\n"); + printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); +#endif TxBuf[0] = 0xFF; TxBuf[1] = 0xFF; @@ -186,29 +186,31 @@ TxBuf[2] = 0xFE; sum += TxBuf[2]; - if (AX12_TRIGGER_DEBUG) { - printf(" ID : %d\n",TxBuf[2]); - } +#ifdef AX12_TRIGGER_DEBUG + printf(" ID : %d\n",TxBuf[2]); +#endif // Length TxBuf[3] = 0x02; sum += TxBuf[3]; - if (AX12_TRIGGER_DEBUG) { - printf(" Length %d\n",TxBuf[3]); - } + +#ifdef AX12_TRIGGER_DEBUG + printf(" Length %d\n",TxBuf[3]); +#endif // Instruction - ACTION TxBuf[4] = 0x04; sum += TxBuf[4]; - if (AX12_TRIGGER_DEBUG) { - printf(" Instruction 0x%X\n",TxBuf[5]); - } + +#ifdef AX12_TRIGGER_DEBUG + printf(" Instruction 0x%X\n",TxBuf[5]); +#endif // Checksum TxBuf[5] = 0xFF - sum; - if (AX12_TRIGGER_DEBUG) { - printf(" Checksum 0x%X\n",TxBuf[5]); - } +#ifdef AX12_TRIGGER_DEBUG + printf(" Checksum 0x%X\n",TxBuf[5]); +#endif // Transmit the packet in one burst with no pausing for (int i = 0; i < 6 ; i++) { @@ -216,16 +218,15 @@ } // This is a broadcast packet, so there will be no reply - return; } float AX12::GetPosition(void) { - if (AX12_DEBUG) { - printf("\nGetPosition(%d)",_ID); - } +#ifdef AX12_DEBUG + printf("\nGetPosition(%d)",_ID); +#endif char data[2]; @@ -239,9 +240,10 @@ float AX12::GetTemp (void) { - if (AX12_DEBUG) { - printf("\nGetTemp(%d)",_ID); - } +#ifdef AX12_DEBUG + printf("\nGetTemp(%d)",_ID); +#endif + char data[1]; int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data); float temp = data[0]; @@ -250,9 +252,11 @@ float AX12::GetVolts (void) { - if (AX12_DEBUG) { - printf("\nGetVolts(%d)",_ID); - } + +#ifdef AX12_DEBUG + printf("\nGetVolts(%d)",_ID); +#endif + char data[1]; int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data); float volts = data[0]/10.0; @@ -269,14 +273,14 @@ Status[4] = 0xFE; // return code - if (AX12_READ_DEBUG) { - printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); - } +#ifdef AX12_READ_DEBUG + printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); +#endif // Build the TxPacket first in RAM, then we'll send in one go - if (AX12_READ_DEBUG) { - printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); - } +#ifdef AX12_READ_DEBUG + printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); +#endif TxBuf[0] = 0xff; TxBuf[1] = 0xff; @@ -284,43 +288,48 @@ // ID TxBuf[2] = ID; sum += TxBuf[2]; - if (AX12_READ_DEBUG) { - printf(" ID : %d\n",TxBuf[2]); - } + +#ifdef AX12_READ_DEBUG + printf(" ID : %d\n",TxBuf[2]); +#endif // Packet Length TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes) sum += TxBuf[3]; // Accululate the packet sum - if (AX12_READ_DEBUG) { - printf(" Length : 0x%x\n",TxBuf[3]); - } + +#ifdef AX12_READ_DEBUG + printf(" Length : 0x%x\n",TxBuf[3]); +#endif // Instruction - Read TxBuf[4] = 0x2; sum += TxBuf[4]; - if (AX12_READ_DEBUG) { - printf(" Instruction : 0x%x\n",TxBuf[4]); - } + +#ifdef AX12_READ_DEBUG + printf(" Instruction : 0x%x\n",TxBuf[4]); +#endif // Start Address TxBuf[5] = start; sum += TxBuf[5]; - if (AX12_READ_DEBUG) { - printf(" Start Address : 0x%x\n",TxBuf[5]); - } + +#ifdef AX12_READ_DEBUG + printf(" Start Address : 0x%x\n",TxBuf[5]); +#endif // Bytes to read TxBuf[6] = bytes; sum += TxBuf[6]; - if (AX12_READ_DEBUG) { - printf(" No bytes : 0x%x\n",TxBuf[6]); - } + +#ifdef AX12_READ_DEBUG + printf(" No bytes : 0x%x\n",TxBuf[6]); +#endif // Checksum TxBuf[7] = 0xFF - sum; - if (AX12_READ_DEBUG) { - printf(" Checksum : 0x%x\n",TxBuf[7]); - } +#ifdef AX12_READ_DEBUG + printf(" Checksum : 0x%x\n",TxBuf[7]); +#endif // Transmit the packet in one burst with no pausing for (int i = 0; i<8 ; i++) { @@ -333,9 +342,30 @@ // Skip if the read was to the broadcast address if (_ID != 0xFE) { - // Receive the Status packet 6+ number of bytes read - for (int i=0; i<(6+bytes) ; i++) { - Status[i] = _ax12.getc(); + + + // response packet is always 6 + bytes + // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum + // timeout is a little more than the time to transmit + // the packet back, i.e. (6+bytes)*10 bit periods + + int timeout = 0; + int plen = 0; + while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) { + + if (_ax12.readable()) { + Status[plen] = _ax12.getc(); + plen++; + timeout = 0; + } + + // wait for the bit period + wait (1.0/_baud); + timeout++; + } + + if (timeout == ((6+bytes)*10) ) { + return(-1); } // Copy the data from Status into data for return @@ -343,20 +373,20 @@ data[i] = Status[5+i]; } - if (AX12_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]); +#ifdef AX12_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 < Status[3]-2 ; i++) { - printf(" Data : 0x%x\n",Status[5+i]); - } + for (int i=0; i < Status[3]-2 ; i++) { + printf(" Data : 0x%x\n",Status[5+i]); + } - printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]); - } + printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]); +#endif } // if (ID!=0xFE) @@ -364,21 +394,21 @@ } -int AX12:: write(int ID, int start, int bytes, char* data, int flag) { +int AX12::write(int ID, int start, int bytes, char* data, int flag) { // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum char TxBuf[16]; char sum = 0; char Status[6]; - if (AX12_WRITE_DEBUG) { - printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); - } +#ifdef AX12_WRITE_DEBUG + printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); +#endif // Build the TxPacket first in RAM, then we'll send in one go - if (AX12_WRITE_DEBUG) { - printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); - } +#ifdef AX12_WRITE_DEBUG + printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); +#endif TxBuf[0] = 0xff; TxBuf[1] = 0xff; @@ -387,17 +417,17 @@ TxBuf[2] = ID; sum += TxBuf[2]; - if (AX12_WRITE_DEBUG) { - printf(" ID : %d\n",TxBuf[2]); - } +#ifdef AX12_WRITE_DEBUG + printf(" ID : %d\n",TxBuf[2]); +#endif // packet Length TxBuf[3] = 3+bytes; sum += TxBuf[3]; - if (AX12_WRITE_DEBUG) { - printf(" Length : %d\n",TxBuf[3]); - } +#ifdef AX12_WRITE_DEBUG + printf(" Length : %d\n",TxBuf[3]); +#endif // Instruction if (flag == 1) { @@ -408,31 +438,35 @@ sum += TxBuf[4]; } - if (AX12_WRITE_DEBUG) { - printf(" Instruction : 0x%x\n",TxBuf[4]); - } +#ifdef AX12_WRITE_DEBUG + printf(" Instruction : 0x%x\n",TxBuf[4]); +#endif // Start Address TxBuf[5] = start; sum += TxBuf[5]; - if (AX12_WRITE_DEBUG) { - printf(" Start : 0x%x\n",TxBuf[5]); - } + +#ifdef AX12_WRITE_DEBUG + printf(" Start : 0x%x\n",TxBuf[5]); +#endif // data for (char i=0; i<bytes ; i++) { TxBuf[6+i] = data[i]; sum += TxBuf[6+i]; - if (AX12_WRITE_DEBUG) { - printf(" Data : 0x%x\n",TxBuf[6+i]); - } + +#ifdef AX12_WRITE_DEBUG + printf(" Data : 0x%x\n",TxBuf[6+i]); +#endif + } // checksum TxBuf[6+bytes] = 0xFF - sum; - if (AX12_WRITE_DEBUG) { - printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); - } + +#ifdef AX12_WRITE_DEBUG + printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); +#endif // Transmit the packet in one burst with no pausing for (int i = 0; i < (7 + bytes) ; i++) { @@ -448,20 +482,35 @@ // we'll only get a reply if it was not broadcast if (_ID!=0xFE) { - // response is always 6 bytes + + // response packet is always 6 bytes // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum - for (int i=0; i < 6 ; i++) { - Status[i] = _ax12.getc(); + // timeout is a little more than the time to transmit + // the packet back, i.e. 60 bit periods, round up to 100 + int timeout = 0; + int plen = 0; + while ((timeout < 100) && (plen<6)) { + + if (_ax12.readable()) { + Status[plen] = _ax12.getc(); + plen++; + timeout = 0; + } + + // wait for the bit period + wait (1.0/_baud); + timeout++; } + // Build the TxPacket first in RAM, then we'll send in one go - if (AX12_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]); - } +#ifdef AX12_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 }
--- a/AX12.h Wed Mar 30 16:08:02 2011 +0000 +++ b/AX12.h Sun Apr 10 20:58:21 2011 +0000 @@ -26,10 +26,10 @@ #include "mbed.h" -#define AX12_WRITE_DEBUG 0 -#define AX12_READ_DEBUG 0 -#define AX12_TRIGGER_DEBUG 0 -#define AX12_DEBUG 0 +//#define AX12_WRITE_DEBUG 0 +//#define AX12_READ_DEBUG 0 +//#define AX12_TRIGGER_DEBUG 0 +//#define AX12_DEBUG 0 #define AX12_REG_ID 0x3 #define AX12_REG_CW_LIMIT 0x06 @@ -77,7 +77,7 @@ * @param pin rx pin * @param int ID, the Bus ID of the servo 1-255 */ - AX12(PinName tx, PinName rx, int ID); + AX12(PinName tx, PinName rx, int ID, int baud=1000000); /** Set the mode of the servo * @param mode @@ -159,13 +159,15 @@ */ float GetVolts(void); + int read(int ID, int start, int length, char* data); + int write(int ID, int start, int length, char* data, int flag=0); + private : SerialHalfDuplex _ax12; int _ID; + int _baud; - int read(int ID, int start, int length, char* data); - int write(int ID, int start, int length, char* data, int flag=0); };