Remote Control Car

Diff: main.cpp

Revision:

3:4956cc0efdf3

Parent:

2:5c9526ccb055

Child:

4:29839de66eae

--- a/main.cpp	Tue Nov 17 18:27:13 2020 +0000
+++ b/main.cpp	Thu Nov 19 17:23:54 2020 +0000
@@ -11,14 +11,84 @@
 #define VL53L0_I2C_SDA   p28
 #define VL53L0_I2C_SCL   p27
 
+#define AUTOPILOT 10
+#define FORWARD 1
+#define REVERSE -1
+#define STOP 0
+
 static XNucleo53L0A1 *board=NULL;
 Motor M(p21, p22, p23, 1); // pwm, fwd, rev, can brake 
 Servo S(p24);
+int state = 0; //global variable stop state 
+int status;
+uint32_t distanceCenter;
+uint32_t distanceLeft;
+uint32_t distanceRight;
+
+void getNewState() {
+    //Logic for AdaFruit App
+    char bnum=0;
+    char bhit=0;
+    if (blue.readable()) {
+    if (blue.getc()=='!') {
+        if (blue.getc()=='B') { //button data packet
+            bnum = blue.getc(); //button number
+            bhit = blue.getc(); //1=hit, 0=release
+            if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
+                myled = bnum - '0'; //current button number will appear on LEDs
+                switch (bnum) {
+                    case '1': //AutoPilot Mode
+                        if (bhit=='1') {
+                            state = 10; //autopilot state
+                        }
+                        break;
+                    case '5': //forward
+                        if (bhit=='1') {
+                            state = 1; //forward state
+                        } else {
+                            state = 0; //stop state
+                        }
+                        break;
+                    case '6': //reverse
+                        if (bhit=='1') {
+                            state = -1; //reverse state
+                        } else {
+                            state = 0; //stop state
+                        }
+                        break;
+                    case '7': //left
+                        if (bhit=='1') {
+                            S = S - 0.5; //turn left                                
+                        }
+                        break;
+                    case '8': //right
+                        if (bhit=='1') {
+                            S = S + 0.5; //turn right
+                        }
+                        break;
+                    }
+                }
+            }
+        }
+    }
+}
+
+void autoPilot() {
+    if ((distanceCenter >= 250 || distanceCenter == 0)) {
+        M.speed(1.0);
+        S = 0.5;
+    }
+    else if (distanceRight > distanceLeft) { //More space on right so turn right
+        S = 1.0;
+    }
+    else { //or turn left
+        S = 0;
+    }
+}
 
 
 int main() {
-    int status;
-    uint32_t distance;
+    //LIDAR Initialization
     DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
     /* creates the 53L0A1 expansion board singleton obj */
     board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
@@ -33,61 +103,22 @@
         status = board->init_board();
     }
     
-    
-    //Logic for AdaFruit App
-    char bnum=0;
-    char bhit=0;
-    while(1) {
-        if (blue.getc()=='!') {
-            if (blue.getc()=='B') { //button data packet
-                bnum = blue.getc(); //button number
-                bhit = blue.getc(); //1=hit, 0=release
-                if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
-                    myled = bnum - '0'; //current button number will appear on LEDs
-                    switch (bnum) {
-                        //case '1': //AutoPilot Mode
-//                            if (bhit=='1') {
-//                                M.speed(1.0);
-//                                //loop taking and printing distance
-//                                status = board->sensor_centre->get_distance(&distance);
-//                                if (distance <= 50) {
-//                                    //turn left or right depending on location of sensor
-//                                    M.speed(0);
-//                                }
-//                            }
-//                            break;
-                        case '5': //forward
-                            if (bhit=='1') {
-                                status = board->sensor_centre->get_distance(&distance);
-                                if (distance <= 50) {
-                                    M.speed(1.0); //drive forward
-                                } else {
-                                    M.speed(0.0); //stop
-                                }
-                            } else {
-                                M.speed(0.0); //stop
-                            }   
-                            break;
-                        case '6': //reverse
-                            if (bhit=='1') {
-                                M.speed(-1.0); //reverse
-                            } else {
-                                M.speed(0.0); //stop
-                            }
-                            break;
-                        case '7': //left
-                            if (bhit=='1') {
-                                S = S - 0.5; //turn left                                
-                            }
-                            break;
-                        case '8': //right
-                            if (bhit=='1') {
-                                S = S + 0.5; //turn right
-                            }
-                            break;
-                    }
-                }
-            }
+    while(1) { //main loop for motor control
+        status = board->sensor_centre->get_distance(&distanceCenter);
+        pc.printf("D=%ld mm\r\n", distanceCenter);
+        getNewState();
+        if ((distanceCenter >= 250 || distanceCenter == 0) && state == FORWARD) {
+            M.speed(1.0);
+        }
+        else if (state == REVERSE) {
+            M.speed(-1.0);
+        }
+        else if (state == AUTOPILOT) {
+            autoPilot();
+        }
+        else {
+            M.speed(0);
         }
     }
+    
 }