Diff: communication.cpp

Revision:

0:0c88691e3904

Child:

1:1f6864549b92

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/communication.cpp	Sun Mar 01 06:28:01 2015 +0000
@@ -0,0 +1,157 @@
+#include "mbed.h"
+#include "communication.h"
+
+uint8_t COMMUNICATION::CommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet)
+{
+    uint8_t currentParameter = 0;
+    bool isWholePacket = false;
+
+    currentParameter = 0;
+    isWholePacket = false;
+    uint8_t decoderState = WAIT_ON_HEADER_0;
+
+    Timer timer;
+    timer.start();
+
+    while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) {
+        if(serialCom->readable()) {
+            switch(decoderState) {
+                case WAIT_ON_HEADER_0: {
+
+                    uint8_t mx28ProtocolHeader0 = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("Read: 0x%02X ", mx28ProtocolHeader0);
+#endif
+
+                    decoderState = WAIT_ON_HEADER_1;
+
+                    break;
+                }
+                case WAIT_ON_HEADER_1: {
+
+                    uint8_t mx28ProtocolHeader1 = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", mx28ProtocolHeader1);
+#endif
+
+                    decoderState = WAIT_ON_ROBOT_ID;
+
+                    break;
+                }
+                case WAIT_ON_ROBOT_ID: {
+
+
+                    packet->robotId = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->servoId);
+#endif
+
+                    decoderState = WAIT_ON_LENGTH;
+
+                    break;
+                }
+                case WAIT_ON_LENGTH: {
+
+                    packet->length = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->length);
+#endif
+
+                    decoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
+
+                    break;
+                }
+                case WAIT_ON_INSTRUCTION_ERROR_ID: {
+
+                    packet->instructionErrorId = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->instructionErrorId);
+#endif
+
+                    if(packet->length > 2)
+                        decoderState = WAIT_ON_PARAMETER;
+                    else
+                        decoderState = WAIT_ON_CHECK_SUM;
+
+                    break;
+                }
+                case WAIT_ON_PARAMETER: {
+
+                    uint8_t parameter = serialCom->getc();
+                    packet->parameter[currentParameter] = parameter;
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", parameter);
+#endif
+
+                    if(++currentParameter == packet->length - 2)
+                        decoderState = WAIT_ON_CHECK_SUM;
+
+                    break;
+                }
+                case WAIT_ON_CHECK_SUM: {
+
+                    packet->checkSum = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("sum =0x%02X\r\n", packet->checkSum);
+#endif
+
+                    decoderState = 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: Read response timeout\r\n");
+#endif
+
+        return ANDANTE_ERRBIT_READ_TIMEOUT;
+    }
+
+    timer.reset();
+    timer.start();
+
+}
+
+COMMUNICATION::COMMUNICATION(PinName tx, PinName rx, uint32_t baudRate, uint16_t tx_buff, uint16_t rx_buff )
+{
+
+
+#ifdef ANDANTE_DEBUG
+    pc = new Serial(USBTX, USBRX);
+    pc->baud(115200);
+    pc->printf("\033[2J");
+#endif
+
+    serialCom = new iSerial(tx, rx,NULL,tx_buff,rx_buff);
+    serialCom->baud(baudRate);
+
+}
+
+COMMUNICATION::~COMMUNICATION()
+{
+#ifdef ANDANTE_DEBUG
+    if(pc != NULL)
+        delete pc;
+#endif
+
+    if(serialCom != NULL)
+        delete serialCom;
+}
\ No newline at end of file