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]);