a
Fork of AX12 by
Diff: AX12.cpp
- Revision:
- 2:5ea99c37a2d7
- Parent:
- 1:93ad80f5fde7
- Child:
- 3:ced71d1b2558
diff -r 93ad80f5fde7 -r 5ea99c37a2d7 AX12.cpp --- 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 }