Diff: main.cpp

Revision:

7:5ebe1058ca5b

Parent:

6:eccd0b81ba62

Child:

9:1579e5cfa499

--- a/main.cpp	Sat Sep 30 04:31:19 2017 +0000
+++ b/main.cpp	Sat Nov 11 09:41:10 2017 +0900
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "motorsmlap.h"
 #include "MDC3_0pinConfig.h"
+#include <vector>
 #define TIMEOUT 0.5
 using namespace pinConfig;
 motorSmLap motor[]={
@@ -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 LED0(LED_0);
+DigitalOut LED1(LED_1);
 BusOut debugLED(LED_2,LED_3);
 Timeout timeout;
 uint8_t pointedMotor;
@@ -26,6 +29,10 @@
 
 int mode[4] = {0};
 
+std::vector<unsigned char> buf;
+
+unsigned char buf[100];
+
 void forceStop()
 {
     for(int i= 0; i< 4; i++)
@@ -34,61 +41,31 @@
 }
 
 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)
+    LED0 = != LED0;
+    buf.push_back(rs485.getc());
+}
+
+void processData(const unsigned char[] data)
+{
+    if(data[0]^data[1] == data[2])
+    if((data[0]>>5) == addr)
     {
-        //serial.printf("data :%d",data);
-        doubleHeader=false;
-        if((data>>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{
-            headerRecieved = false;
-            addrChecked = false;
-        }
-        return;
-    }else if(headerRecieved && addrChecked && !estop)
-    {
-        //serial.printf("data %d\n\r",data);
+        addrChecked =true;
+        pointedMotor = data[0]%4;
+        mode[pointedMotor] = ((data[0]>>4)%2);
+        motor[pointedMotor].braking = (data[0]>>3)%2;
+
         motor[pointedMotor].setMode(mode[pointedMotor]);
-        doubleHeader = false;
-        if(data == 126){
+        if(data[1] == 126){
             motor[pointedMotor].setMotorSpeed(0);
             //serial.printf("STOP");
         }else{
-            motor[pointedMotor].setMotorSpeed((data-126.0)/126.0);
+            motor[pointedMotor].setMotorSpeed((data[1]-126.0)/126.0);
         }
         addrChecked = false;
         headerRecieved = false;
-        serial.putc(pointedMotor);     
+        serial.putc(pointedMotor);
         timeout.attach(&forceStop,TIMEOUT);
-    }else{
-        doubleHeader = false;
-        headerRecieved =false;
-        addrChecked = false;
     }
 }
 
@@ -96,6 +73,7 @@
     for(int i= 0; i< 4; i++)
         motor[i].setMotorSpeed(0);
     beep = true;
+    unsigned char tmp[3] = {0};
     addrChecked=false,headerRecieved=false;
     //motor[0].setMotorSpeed(0);
     rs485.baud(38400);
@@ -107,9 +85,20 @@
     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;