Diff: main.cpp

Revision:

9:1579e5cfa499

Parent:

8:fd749c243a8f

Parent:

7:5ebe1058ca5b

Child:

10:5f932239b8a5

--- a/main.cpp	Sat Nov 11 00:43:54 2017 +0000
+++ b/main.cpp	Sat Nov 11 00:52:25 2017 +0000
@@ -1,9 +1,10 @@
 #include "mbed.h"
 #include "motorsmlap.h"
 #include "MDC3_0pinConfig.h"
+#include <vector>
 #define TIMEOUT 0.5
 using namespace pinConfig;
-motorSmLap motor[]={
+motorSmLap motor[]= {
     motorSmLap(DIR_H_0,DIR_L_0,PWM0),
     motorSmLap(DIR_H_1,DIR_L_1,PWM1),
     motorSmLap(DIR_H_2,DIR_L_2,PWM2),
@@ -13,8 +14,10 @@
 Serial serial(UART1_TX,UART1_RX);
 
 DigitalOut RSControl(RS485_CS);
-DigitalOut addrChecked(LED_0);
-DigitalOut headerRecieved(LED_1);
+bool addrChecked;
+bool headerRecieved;
+DigitalOut debugLED0(LED_0);
+DigitalOut debugLED1(LED_1);
 BusOut debugLED(LED_2,LED_3);
 Timeout timeout;
 uint8_t pointedMotor;
@@ -26,6 +29,9 @@
 
 int mode[4] = {0};
 
+std::vector<unsigned char> buf;
+
+
 void forceStop()
 {
     for(int i= 0; i< 4; i++)
@@ -33,81 +39,54 @@
     estop = true;
 }
 
-void callback(){
-    const uint8_t data = rs485.getc();
-    serial.putc(data);
-    if(data == 255) //header
-    {
-        if(headerRecieved){
-            doubleHeader =true;
-            return;
-        }
-        else if (doubleHeader)
-        {
-            estop = true;
-            return;      
-        }     
-        addrChecked = false;
-        headerRecieved = true;
-        return;
-    }else
-    if(headerRecieved && !addrChecked)
-    {
-        //serial.printf("data :%d",data);
-        doubleHeader=false;
-        if((data>>5) == addr)
-        {
+void callback()
+{
+    debugLED0 != debugLED0;
+    buf.push_back(rs485.getc());
+}
+
+void processData(unsigned char data[])
+{
+    if(data[0]^data[1] == data[2]) {
+        if((data[0]>>5) == addr) {
             addrChecked =true;
-            pointedMotor = data%4;
-            mode[pointedMotor] = ((data>>4)%2);
-            motor[pointedMotor].braking = (data>>3)%2;   
-            //motor[pointedMotor].setMode(false);
-            //RSControl = 1;
-            //rs485.putc(addr);
-            //RSControl = 0;
-        }else{
+            pointedMotor = data[0]%4;
+            mode[pointedMotor] = ((data[0]>>4)%2);
+            motor[pointedMotor].braking = (data[0]>>3)%2;
+
+            motor[pointedMotor].setMode(mode[pointedMotor]);
+            if(data[1] == 126) {
+                motor[pointedMotor].setMotorSpeed(0);
+                //serial.printf("STOP");
+            } else {
+                motor[pointedMotor].setMotorSpeed((data[1]-126.0)/126.0);
+            }
+            addrChecked = false;
             headerRecieved = false;
-            addrChecked = false;
+            serial.putc(pointedMotor);
+            timeout.attach(&forceStop,TIMEOUT);
         }
-        return;
-    }else if(headerRecieved && addrChecked)
-    {
-        //serial.printf("data %d\n\r",data);
-        motor[pointedMotor].setMode(mode[pointedMotor]);
-        doubleHeader = false;
-        estop = false;
-        if(data == 126){
-            motor[pointedMotor].setMotorSpeed(0);
-            //serial.printf("STOP");
-        }else{
-            motor[pointedMotor].setMotorSpeed((data-126.0)/126.0);
-        }
-        addrChecked = false;
-        headerRecieved = false;
-        serial.putc(pointedMotor);     
-        timeout.attach(&forceStop,TIMEOUT);
-    }else{
-        doubleHeader = false;
-        headerRecieved =false;
-        addrChecked = false;
     }
 }
 
-int main() {
+
+
+int main()
+{
     for(int i= 0; i< 4; i++)
         motor[i].setMotorSpeed(0);
     beep = true;
     wait(1);
     beep = false;
     wait(0.5);
-    for(int i = 0; i<=addr;i++)
-    {
+    for(int i = 0; i<=addr; i++) {
         beep = true;
         wait(0.1);
         beep = false;
         wait(0.1);
     } // beep addr times
-    
+
+    unsigned char tmp[3] = {0};
     addrChecked=false,headerRecieved=false;
     //motor[0].setMotorSpeed(0);
     rs485.baud(38400);
@@ -118,9 +97,18 @@
     rs485.attach(&callback);
     beep = false;
     while(1) {
-        debugLED = addr; 
-
-        //serial.printf("addr: %d",addr);
+        debugLED = addr;
+        if(buf.size() > 4) {
+            if (buf[0] == 255) {
+                tmp[0] = buf[1];
+                tmp[1] = buf[2];
+                tmp[2] = buf[3];
+                processData(tmp);
+                buf.erase(buf.begin(),buf.begin()+3);
+            } else {
+                buf.erase(buf.begin());
+            }
+        }
         if(estop)
             forceStop();
         beep = estop;