Diff: main.cpp

Revision:

0:4791d1cb39f8

Child:

1:d802a9793080

Child:

2:de85eb142f32

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 10 04:53:24 2017 +0000
@@ -0,0 +1,56 @@
+#include "mbed.h"
+#include "motorsmlap.h"
+motorSmLap motor[]={
+    motorSmLap(PB_0,PB_1)
+    };
+Serial serial(PB_10,PB_11); 
+DigitalOut serialControl(PA_8);
+Timeout timeout;
+uint8_t pointedMotor;
+bool addrChecked=false,headerRecieved=false;
+BusIn addr(PB_4,PB_5,PB_6);
+DigitalOut beep(PB_7);
+int mode = CS;
+
+void forceStop()
+{
+    for(int i = 0;i < 4; i++)
+    {
+        motor[i].setMotorSpeed(0);
+    }    
+}
+void callback(){
+    uint8_t data;
+    data = serial.getc();
+    if(data == 255) //header
+    {
+        addrChecked = false;
+        headerRecieved = true;
+    }else
+    if(headerRecieved && !addrChecked)
+    {
+        if((data>>4) == addr.read())
+        {   
+            addrChecked =true;
+            pointedMotor = data%4;
+            serialControl = 1;
+            serial.putc(addr.read());
+            serialControl = 0;
+        }
+    }else if(headerRecieved && addrChecked)
+    {
+        motor[pointedMotor].setMotorSpeed(data/126.0-1.0);
+        addrChecked = false;
+        headerRecieved = false;
+        timeout.attach(&forceStop,0.1);
+    }
+           
+}
+
+int main() {
+    serialControl = 0;
+    serial.attach(&callback);
+       
+    while(1) {
+    }
+}