Diff: main.cpp

Revision:

0:0747159bf07d

Child:

1:28e137ae5af7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 16 20:51:22 2021 +0000
@@ -0,0 +1,126 @@
+//**************************
+// Mbed to Raspberry Pi
+//**************************
+// 0 - pass starting line
+// 1 - pass checkpoint 1
+// 2 - pass checkpoint 2
+// ...
+// 9 - end race
+
+#include "mbed.h"
+#include "ultrasonic.h"
+
+Serial      pc(USBTX,USBRX);
+DigitalOut  led0(p5);
+DigitalOut  led1(p6);
+DigitalOut  led2(p7);
+
+int width[3];          //width of each checkpoint, mm
+int sensitivity = 5;   //sensitivity, adjust with testing, mm
+int total_laps = 3;
+int current_lap = 0;
+
+int setup[3] = {1,1,1};
+
+//state machine, tracks current position in track
+enum state_type {start,past_start,past_cp1,past_cp2};
+state_type track_state = start;
+
+//executed when crossing starting line
+void start_line(int distance)
+{
+    if (setup[0]) {
+        pc.printf("Setup\n");
+        pc.printf("%d",distance);
+        width[0] = distance;
+        setup[0] = !setup[0];   
+    }
+    
+    if (width[0] - distance >= sensitivity) {
+        pc.printf("Past start line\n");
+        led0 = 1;
+        led1 = 0;
+        led2 = 0;
+        current_lap++;
+        pc.printf("%d\n",current_lap);
+        track_state = past_start;
+        pc.putc('0');
+    }
+}
+
+//executed when passing checkpoint 1
+void checkpoint1(int distance)
+{
+    if (setup[1]) {
+        pc.printf("Setup\n");
+        pc.printf("%d",distance);
+        width[1] = distance;
+        setup[1] = !setup[1];   
+    }
+    
+    pc.printf("Past Checkpoint 1\n");
+    if (width[1] - distance >= sensitivity) {
+        led1 = 1;
+        track_state = past_cp1;
+        pc.putc('1');
+    }
+}
+
+//executed when passing checkpoint 2
+void checkpoint2(int distance)
+{
+    if (setup[2]) {
+        pc.printf("Setup\n");
+        pc.printf("%d",distance);
+        width[2] = distance;
+        setup[2] = !setup[2];   
+    }
+    
+    pc.printf("Past Checkpoint 2\n");
+    if (width[2] - distance >= sensitivity) {
+        led2 = 1;
+        track_state = past_cp2;
+        pc.putc('2');
+    }
+}
+
+ultrasonic  sonar0(p15,p16,0.1,0.1,&start_line);      //starting line
+ultrasonic  sonar1(p17,p18,0.1,0.1,&checkpoint1);     //checkpoint 1
+ultrasonic  sonar2(p21,p22,0.1,0.1,&checkpoint2);     //checkpoint 2
+
+int main() 
+{
+    sonar0.startUpdates();
+    sonar1.startUpdates();
+    sonar2.startUpdates();
+    
+    sonar0.checkDistance();
+    sonar1.checkDistance();
+    sonar2.checkDistance();
+
+    while(1) 
+    {
+        switch (track_state) {
+            case start:
+                sonar0.checkDistance();
+                break;
+            case past_start:
+                sonar1.checkDistance();
+                break;
+            case past_cp1:
+                sonar2.checkDistance();
+                break;
+            case past_cp2:
+                sonar0.checkDistance();
+                break;
+        }
+        if (current_lap > total_laps) {
+            pc.putc('9');
+            while(1) {
+                pc.printf("Race Over");
+                wait(0.05);    
+            }
+        }
+        wait(0.05);
+    }
+}
\ No newline at end of file