Dyanamixel
Dependents: YOZAKURA_ARM YOZAKURA_ARM_USB YOZAKURA_ARM_USB_Keyboard YOZAKURA_ARM_Keyboard0424 ... more
Diff: MX28.cpp
- Revision:
- 12:7affd17fcb66
- Parent:
- 11:cb0d11254b22
--- a/MX28.cpp Sat Apr 25 06:07:02 2015 +0000 +++ b/MX28.cpp Sun May 03 16:17:31 2015 +0000 @@ -21,6 +21,8 @@ * THE SOFTWARE. */ +int mx28timer=0; + #include "MX28.h" #include "mbed.h" //#include "SerialHalfDuplex.h" @@ -372,12 +374,45 @@ // 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] = _mx28.getc(); + if(mx28timer==0){ + // Receive the Status packet 6+ number of bytes read + for (int i=0; i<(6+bytes) ; i++) { + Status[i] = _mx28.getc(); + } + + // Copy the data from Status into data for return + for (int i=0; i < Status[3]-2 ; i++) { + data[i] = Status[5+i]; + } } - + 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. (6+bytes)*10 bit periods + + int timeout = 0; + int plen = 0; + while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) { + + if (_mx28.readable()) { + Status[plen] = _mx28.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]; @@ -488,13 +523,35 @@ // we'll only get a reply if it was not broadcast if (_ID!=0xFE) { - - // response is always 6 bytes - // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum - for (int i=0; i < 6 ; i++) { - Status[i] = _mx28.getc(); + if(mx28timer==0){ + // response is always 6 bytes + // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum + for (int i=0; i < 6 ; i++) { + Status[i] = _mx28.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. (6+bytes)*10 bit periods + int timeout = 0; + int plen = 0; + while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) { + + if (_mx28.readable()) { + Status[plen] = _mx28.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 (MX28_WRITE_DEBUG) { printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);