Diff: communication.cpp

Revision:

2:56c1de1612dd

Parent:

1:1f6864549b92

Child:

3:2f334f0cee75

diff -r 1f6864549b92 -r 56c1de1612dd communication.cpp
--- a/communication.cpp	Sun Mar 01 08:48:23 2015 +0000
+++ b/communication.cpp	Sun Mar 01 15:40:51 2015 +0000
@@ -1,5 +1,149 @@
 #include "mbed.h"
 #include "communication.h"
+//#include "protocol.h"
+//#include "Utilities.h"
+//#include "iSerial.h"
+
+
+uint8_t COMMUNICATION::sendCommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet)
+{
+    uint8_t currentParameter = 0;
+    bool isWholePacket = false;
+    uint8_t encoderState = WAIT_ON_HEADER_0;
+
+    packet->checkSum = Utilities::GetCheckSum((uint8_t*)&(packet->robotId), packet->length + 1);
+
+    Timer timer;
+    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               
+
+                    serialCom->putc(ANDANTE_PROTOCOL_HEADER_0);
+                    //   dirc_rs485=0;
+                    //    wait_us(DELAY_DIR);
+
+
+                    encoderState = WAIT_ON_HEADER_1;
+
+                    break;
+                }
+                case WAIT_ON_HEADER_1: {
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", ANDANTE_PROTOCOL_HEADER_1);
+#endif
+
+            
+
+                    serialCom->putc(ANDANTE_PROTOCOL_HEADER_1);
+                    //      dirc_rs485=0;
+                    //      wait_us(DELAY_DIR);
+
+                    encoderState = WAIT_ON_ROBOT_ID;
+
+                    break;
+                }
+                case WAIT_ON_ROBOT_ID: {
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->robotId);
+#endif
+
+
+                    serialCom->putc(packet->robotId);
+                 
+
+                    encoderState = WAIT_ON_LENGTH;
+
+                    break;
+                }
+                case WAIT_ON_LENGTH: {
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->length);
+#endif
+                
+                    serialCom->putc(packet->length);
+                  
+
+                    encoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
+
+                    break;
+                }
+                case WAIT_ON_INSTRUCTION_ERROR_ID: {
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->instructionErrorId);
+#endif
+               
+
+                    serialCom->putc(packet->instructionErrorId);
+
+                    if(packet->length > 2)
+                        encoderState = WAIT_ON_PARAMETER;
+                    else
+                        encoderState = WAIT_ON_CHECK_SUM;
+
+                    break;
+                }
+                case WAIT_ON_PARAMETER: {
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->parameter[currentParameter]);
+#endif
+
+                    serialCom->putc(packet->parameter[currentParameter]);
+
+                    if(++currentParameter == packet->length - 2)
+                        encoderState = WAIT_ON_CHECK_SUM;
+
+                    break;
+                }
+                case WAIT_ON_CHECK_SUM: {
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X\r\n", packet->checkSum);
+#endif
+
+
+                    serialCom->putc(packet->checkSum);
+
+
+                    encoderState = WAIT_ON_HEADER_0;
+                    isWholePacket = true;
+
+                    break;
+                }
+            }
+        }
+    }
+
+
+#ifdef ANDANTE_DEBUG
+    pc->printf("Timer: %d ms\r\n", timer.read_ms());
+#endif
+
+    timer.stop();
+
+
+
+    if(!isWholePacket) {
+#ifdef ANDANTE_DEBUG
+        pc->printf("Error: Write response timeout.\r\n");
+#endif
+
+        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;
+        
+}
 
 uint8_t COMMUNICATION::receiveCommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet)
 {
@@ -46,7 +190,7 @@
                     packet->robotId = serialCom->getc();
 
 #ifdef ANDANTE_DEBUG
-                    pc->printf("0x%02X ", packet->servoId);
+                    pc->printf("0x%02X ", packet->robotId);
 #endif
 
                     decoderState = WAIT_ON_LENGTH;