Dyanamixel
Dependents: YOZAKURA_ARM YOZAKURA_ARM_USB YOZAKURA_ARM_USB_Keyboard YOZAKURA_ARM_Keyboard0424 ... more
Diff: AX12.cpp
- Revision:
- 12:7affd17fcb66
- Parent:
- 9:0927a7b74806
diff -r cb0d11254b22 -r 7affd17fcb66 AX12.cpp --- a/AX12.cpp Sat Apr 25 06:07:02 2015 +0000 +++ b/AX12.cpp Sun May 03 16:17:31 2015 +0000 @@ -21,6 +21,8 @@ * THE SOFTWARE. */ +int timer=0; + #include "AX12.h" #include "mbed.h" //#include "SerialHalfDuplex.h" @@ -358,17 +360,44 @@ // 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(); + if(timer==0){ + // Receive the Status packet 6+ number of bytes read + for (int i=0; i<(6+bytes) ; i++) { + Status[i] = _ax12.getc(); + } } - + else{ + // 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. 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 + // int baudrate = 1000000; + // wait (1.0/baudrate); + wait(0.01); + timeout++; + } + + if (timeout == ((6+bytes)*10) ) { + return(-1); + } + } + // Copy the data from Status into data for return for (int i=0; i < Status[3]-2 ; i++) { data[i] = Status[5+i]; } - + if (AX12_READ_DEBUG) { printf("\nStatus Packet\n"); printf(" Header : 0x%x\n",Status[0]); @@ -475,13 +504,38 @@ // we'll only get a reply if it was not broadcast if (_ID!=0xFE) { + if(timer==0){ + // response is always 6 bytes + // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum + for (int i=0; i < 6 ; i++) { + Status[i] = _ax12.getc(); + } + } + else{ - // response is always 6 bytes - // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum - for (int i=0; i < 6 ; 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 + // int baudrate = 1000000; + // wait (1.0/baudrate); + wait(0.01); + 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]);