Diff: main.cpp

Revision:

4:2fb782334564

Parent:

3:22c5eb74817f

Child:

5:bcf70ca27a55

--- a/main.cpp	Sat Aug 12 02:15:28 2017 +0000
+++ b/main.cpp	Tue Aug 22 02:10:11 2017 +0000
@@ -1,22 +1,28 @@
 #include "mbed.h"
 #include "motorsmlap.h"
+#include "MDC3_0pinConfig.h"
+
+using namespace pinConfig;
 motorSmLap motor[]={
-    motorSmLap(PA_2,PB_15),
-//    motorSmLap(,NULL),
-//    motorSmLap(NULL,NULL),
-//    motorSmLap(NULL,NULL)
-    };
-Serial serial(PB_10,PB_11); 
-DigitalOut serialControl(PA_8);
-DigitalOut addrChecked(PB_13);
-DigitalOut headerRecieved(PB_14);
+    motorSmLap(DIR_H_0,DIR_L_0,PWM0),
+    motorSmLap(DIR_H_1,DIR_L_1,PWM1),
+    motorSmLap(DIR_H_2,DIR_L_2,PWM2),
+    motorSmLap(DIR_H_3,DIR_L_3,PWM3)
+};
+Serial rs485(RS485_TX,RS485_RX);
+Serial serial(UART1_TX,UART1_RX);
+
+DigitalOut RSControl(RS485_CS);
+DigitalOut addrChecked(LED_0);
+DigitalOut headerRecieved(LED_1);
 Timeout timeout;
 uint8_t pointedMotor;
 bool estop =false,doubleHeader = false;
-BusIn addr(PB_4,PB_5,PB_6);
-DigitalOut beep(PB_7);
+//BusIn addr(DIP_0,DIP_1,DIP_2);
+uint8_t addr = 2;
+DigitalOut beep(BUZER);
 
-int mode = SM;
+int mode[4] = {0};
 
 void forceStop()
 {
@@ -27,12 +33,14 @@
 }
 
 void callback(){
-    uint8_t data;
-    data = serial.getc();
+    const uint8_t data = rs485.getc();
+    serial.putc(data);
     if(data == 255) //header
     {
-        if(headerRecieved)
+        if(headerRecieved){
             doubleHeader =true;
+            return;
+        }
         else if (doubleHeader)
         {
             estop = true;
@@ -40,42 +48,59 @@
         }     
         addrChecked = false;
         headerRecieved = true;
+        return;
     }else
     if(headerRecieved && !addrChecked)
     {
+        //serial.printf("data :%d",data);
         doubleHeader=false;
-        if((data>>4) == addr.read())
-        {   
+        if((data>>5) == addr)
+        {
             addrChecked =true;
             pointedMotor = data%4;
-            serialControl = 1;
-            serial.putc(addr.read());
-            serialControl = 0;
+            mode[pointedMotor] = (data>>4)%2;   
+            //motor[pointedMotor].setMode(false);
+            RSControl = 1;
+            //rs485.putc(addr);
+            RSControl = 0;
+        }else{
+            headerRecieved = false;
+            addrChecked = false;
         }
+        return;
     }else if(headerRecieved && addrChecked && !estop)
     {
+        motor[pointedMotor].setMode(mode[pointedMotor]);
         doubleHeader = false;
         motor[pointedMotor].setMotorSpeed(data/126.0-1.0);
         addrChecked = false;
         headerRecieved = false;
+        serial.putc(pointedMotor);     
         //timeout.attach(&forceStop,0.1);
+    }else{
+        doubleHeader = false;
+        headerRecieved =false;
+        addrChecked = false;
     }
-           
+       
 }
 
 int main() {
     beep = 0;
     addrChecked=false,headerRecieved=false;
-    motor[0].setMotorSpeed(0);
+    //motor[0].setMotorSpeed(0);
+    rs485.baud(115200);
     serial.baud(115200);
-    addr.mode(PullUp);
-    serialControl = 0;
-    serial.attach(&callback);
-    
+   // addr.mode(PullUp);
+    RSControl = 0;
+    wait(0.5);
+    rs485.putc((1<<addr));
+    rs485.attach(&callback);
+
     while(1) {
         if(estop)
             forceStop();
         beep = estop;
-        //led = !led
+        wait(0.1);
     }
 }