Diff: main.cpp

Revision:

5:bcf70ca27a55

Parent:

4:2fb782334564

Child:

6:eccd0b81ba62

--- a/main.cpp	Tue Aug 22 02:10:11 2017 +0000
+++ b/main.cpp	Fri Sep 01 03:17:31 2017 +0000
@@ -15,12 +15,14 @@
 DigitalOut RSControl(RS485_CS);
 DigitalOut addrChecked(LED_0);
 DigitalOut headerRecieved(LED_1);
+BusOut debugLED(LED_2,LED_3);
 Timeout timeout;
 uint8_t pointedMotor;
 bool estop =false,doubleHeader = false;
-//BusIn addr(DIP_0,DIP_1,DIP_2);
-uint8_t addr = 2;
+BusIn addr(DIP_0,DIP_1,DIP_2);
+//uint8_t addr = 2;
 DigitalOut beep(BUZER);
+bool brakeing[4] = {false};
 
 int mode[4] = {0};
 
@@ -58,11 +60,12 @@
         {
             addrChecked =true;
             pointedMotor = data%4;
-            mode[pointedMotor] = (data>>4)%2;   
+            mode[pointedMotor] = ((data>>4)%2);
+            motor[pointedMotor].braking = (data>>3)%2;   
             //motor[pointedMotor].setMode(false);
-            RSControl = 1;
+            //RSControl = 1;
             //rs485.putc(addr);
-            RSControl = 0;
+            //RSControl = 0;
         }else{
             headerRecieved = false;
             addrChecked = false;
@@ -70,9 +73,15 @@
         return;
     }else if(headerRecieved && addrChecked && !estop)
     {
+        //serial.printf("data %d\n\r",data);
         motor[pointedMotor].setMode(mode[pointedMotor]);
         doubleHeader = false;
-        motor[pointedMotor].setMotorSpeed(data/126.0-1.0);
+        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);     
@@ -82,22 +91,24 @@
         headerRecieved =false;
         addrChecked = false;
     }
-       
 }
 
 int main() {
-    beep = 0;
+    beep = true;
     addrChecked=false,headerRecieved=false;
     //motor[0].setMotorSpeed(0);
-    rs485.baud(115200);
+    rs485.baud(38400);
     serial.baud(115200);
-   // addr.mode(PullUp);
+    addr.mode(PullUp);
     RSControl = 0;
     wait(0.5);
     rs485.putc((1<<addr));
     rs485.attach(&callback);
+    beep = false;
+    while(1) {
+        debugLED = addr; 
 
-    while(1) {
+        //serial.printf("addr: %d",addr);
         if(estop)
             forceStop();
         beep = estop;