Diff: main.cpp

Revision:

0:9969c88facec

Child:

1:be4238d819bb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Feb 24 13:34:41 2019 +0000
@@ -0,0 +1,152 @@
+#include "mbed.h"
+
+int val = 0;
+
+Serial pc( USBTX, USBRX);
+CAN can( PA_11, PA_12);
+CANMessage msg;
+DigitalOut led (PA_5);
+DigitalIn start(PB_13);
+
+bool finishFlags[4] = {false, false, false, false};
+/*
+void inter(){
+    can.read(msg);
+    if(msg.id >= 4 && msg.id <= 7){
+        if(msg.data[0] == 0){
+            finishFlags[msg.id - 4] = false;
+        }else{
+            finishFlags[msg.id - 4] = true;
+        }
+    }
+}
+*/
+void checkFlags(int id){
+    char data = 10;
+    if(!finishFlags[id]){
+        can.write( CANMessage((id + 4),&data,1) );
+        while(1){
+            if(can.read(msg)){
+                break;
+            }
+        }
+        if(msg.id == id+4){
+            if(msg.data[0] == 0){
+                finishFlags[msg.id - 4] = false;
+            }else{
+                finishFlags[msg.id - 4] = true;
+            }
+        }
+    }
+}
+
+
+void waitReach(int id){
+    while(1){
+        checkFlags(id);
+        pc.printf("%d \t",id);
+        
+        pc.printf("%d \n",finishFlags[id]);
+        //bool flag = true;
+        /*
+        for(int i=0;i<4;i++){
+            flag = flag & finishFlags[i];
+            pc.printf("%d \t",finishFlags[i]);
+        }
+        */
+        //pc.printf("\n");
+        //pc.printf("%d \n",start.read());
+       // pc.printf("%d \n",finishFlags[id]);
+        if(finishFlags[id] == true){
+            break;
+        }
+    }
+}
+
+void allWaitReach(){
+    for(int i=0;i<4;i++){
+        waitReach(i);
+    }
+    //pc.printf(" \n");
+}
+
+void step(int id, int y, bool z){
+    char data[2] = {y,z};
+    can.write(CANMessage(id,data,2) );
+    finishFlags[id] = false;
+    wait_ms(2);
+}
+
+int main() {
+    wait(1);
+    start.mode(PullUp);
+    can.frequency(100000);
+    //can.attach(&inter);
+    led.write(0);
+    int count = 0;
+    while(1){
+        if(start.read() == 0){
+            count++;
+        }
+        if(count>= 100){
+            break;
+        }
+    }
+    
+    step(0,10,1);
+    step(1,10,0);
+    step(2,10,0);
+    step(3,10,1);
+    allWaitReach();
+    
+    wait(2.0);
+    while (true) {
+        /*
+        //led.write(1);
+        step(2,10,1);
+        allWaitReach();
+        //pc.printf("ok \n");
+        //wait(3.0);
+        step(2,200,0);
+        allWaitReach();
+        //pc.printf("ok \n");
+        */
+        
+        step(1,200,0);
+        step(2,200,0);
+        waitReach(1);
+        waitReach(2);
+        
+        step(0,10,0);
+        step(1,200,1);
+        step(2,200,1);
+        step(3,10,0);
+        allWaitReach();
+        
+        step(1,10,1);
+        step(2,10,1);
+        waitReach(1);
+        waitReach(2);
+        
+        step(0,200,0);
+        step(3,200,0);
+        waitReach(0);
+        waitReach(3);
+        
+        step(0,200,1);
+        step(1,10,0);
+        step(2,10,0);
+        step(3,200,1);
+        allWaitReach();
+        
+        step(0,10,1);
+        step(3,10,1);
+        waitReach(0);
+        waitReach(3);
+        
+        
+        //wait(3.0);
+        //pc.printf("%d \t",val);
+        
+    }
+}
\ No newline at end of file