Diff: communication.cpp

Revision:

8:49a5e7d12c82

Parent:

6:2ed7945594db

--- a/communication.cpp	Sun Jan 24 09:03:54 2016 +0000
+++ b/communication.cpp	Wed Jan 27 12:52:04 2016 +0000
@@ -18,14 +18,14 @@
     timer.start();
 
     while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) {
-        
+
         if( serialCom->writeable()) {
 
             switch(encoderState) {
                 case WAIT_ON_HEADER_0: {
 #ifdef ANDANTE_DEBUG
                     pc->printf("Write: 0x%02X ", ANDANTE_PROTOCOL_HEADER_0);
-#endif               
+#endif
 
                     serialCom->putc(ANDANTE_PROTOCOL_HEADER_0);
                     //   dirc_rs485=0;
@@ -41,7 +41,7 @@
                     pc->printf("0x%02X ", ANDANTE_PROTOCOL_HEADER_1);
 #endif
 
-            
+
 
                     serialCom->putc(ANDANTE_PROTOCOL_HEADER_1);
                     //      dirc_rs485=0;
@@ -58,7 +58,7 @@
 
 
                     serialCom->putc(packet->robotId);
-                 
+
 
                     encoderState = WAIT_ON_LENGTH;
 
@@ -68,9 +68,9 @@
 #ifdef ANDANTE_DEBUG
                     pc->printf("0x%02X ", packet->length);
 #endif
-                
+
                     serialCom->putc(packet->length);
-                  
+
 
                     encoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
 
@@ -80,7 +80,7 @@
 #ifdef ANDANTE_DEBUG
                     pc->printf("0x%02X ", packet->instructionErrorId);
 #endif
-               
+
 
                     serialCom->putc(packet->instructionErrorId);
 
@@ -137,13 +137,13 @@
 
         return ANDANTE_ERRBIT_WRITE_TIMEOUT;
     }
-/*
-    if( packet->servoId == ANDANTE_PROTOCOL_BROADCAST_ID ||
-            packet->instructionErrorId == ANDANTE_ACTION ||
-            packet->instructionErrorId == ANDANTE_SYNC_WRITE)
-*/
-        return ANDANTE_ERRBIT_NONE;
-        
+    /*
+        if( packet->servoId == ANDANTE_PROTOCOL_BROADCAST_ID ||
+                packet->instructionErrorId == ANDANTE_ACTION ||
+                packet->instructionErrorId == ANDANTE_SYNC_WRITE)
+    */
+    return ANDANTE_ERRBIT_NONE;
+
 }
 
 uint8_t COMMUNICATION::receiveCommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet)
@@ -168,10 +168,8 @@
 #ifdef ANDANTE_DEBUG
                     pc->printf("Read: 0x%02X ", mx28ProtocolHeader0);
 #endif
-                    if(mx28ProtocolHeader0 == 0xFF)
+                    if(mx28ProtocolHeader0 == ANDANTE_PROTOCOL_HEADER_0)
                         decoderState = WAIT_ON_HEADER_1;
-                    else
-                        isWholePacket = false;
 
                     break;
                 }
@@ -182,8 +180,8 @@
 #ifdef ANDANTE_DEBUG
                     pc->printf("0x%02X ", mx28ProtocolHeader1);
 #endif
-
-                    decoderState = WAIT_ON_ROBOT_ID;
+                    //if(mx28ProtocolHeader1 == ANDANTE_PROTOCOL_HEADER_1)
+                        decoderState = WAIT_ON_ROBOT_ID;
 
                     break;
                 }