Remote Control Car

Diff: main.cpp

Revision:

1:1c1ad0c1c260

Parent:

0:20a8dfc396cb

Child:

2:5c9526ccb055

--- a/main.cpp	Sun Nov 15 22:18:34 2020 +0000
+++ b/main.cpp	Sun Nov 15 23:01:55 2020 +0000
@@ -3,48 +3,88 @@
 #include "Servo.h"
 #include "motordriver.h"
 #include <stdio.h>
-//Serial pc(USBTX,USBRX);
-//DigitalOut shdn(p26);
+Serial pc(USBTX,USBRX);
+Serial blue(p28,p27);
+DigitalOut shdn(p26);
 DigitalOut myled(LED1);      
 
-//#define VL53L0_I2C_SDA   p28
-//#define VL53L0_I2C_SCL   p27
+#define VL53L0_I2C_SDA   p28
+#define VL53L0_I2C_SCL   p27
 
-//static XNucleo53L0A1 *board=NULL;
+static XNucleo53L0A1 *board=NULL;
 Motor M(p21, p22, p23, 1); // pwm, fwd, rev, can brake 
-Servo myservo(p24);
+Servo S(p24);
 
 
 int main() {
-//    int status;
-//    uint32_t distance;
-//    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);
-//    shdn = 0; //must reset sensor for an mbed reset to work
-//    wait(0.1);
-//    shdn = 1;
-//    wait(0.1);
-//    /* init the 53L0A1 board with default values */
-//    status = board->init_board();
-//    while (status) {
-//        pc.printf("Failed to init board! \r\n");
-//        status = board->init_board();
-//    }
-//    //loop taking and printing distance
-//    while (1) {
-//        status = board->sensor_centre->get_distance(&distance);
-//        if (status == VL53L0X_ERROR_NONE) {
-//            pc.printf("D=%ld mm\r\n", distance);
-//        }
-//    }
-    for (float s= -1.0; s < 1.0 ; s += 0.01) {
-       M.speed(s); 
-       wait(0.02);
+    int status;
+    uint32_t distance;
+    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);
+    shdn = 0; //must reset sensor for an mbed reset to work
+    wait(0.1);
+    shdn = 1;
+    wait(0.1);
+    /* init the 53L0A1 board with default values */
+    status = board->init_board();
+    while (status) {
+        pc.printf("Failed to init board! \r\n");
+        status = board->init_board();
+    }
+    //loop taking and printing distance
+    while (1) {
+        status = board->sensor_centre->get_distance(&distance);
+        if (status == VL53L0X_ERROR_NONE) {
+            pc.printf("D=%ld mm\r\n", distance);
+        }
     }
     
-    for(float p=0; p<1.0; p += 0.1) {
-        myservo = p;
-        wait(0.2);
-    }
+    
+    
+    //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') {
+                                //loop taking and printing distance
+                                while (1) {
+                                    status = board->sensor_centre->get_distance(&distance);
+                                    if (distance <= 50) {
+                                        //turn left or right depending on location of sensor
+                                    }
+                                }
+                            break;
+                        case '5': //forward
+                            if (bhit=='1') {
+                                M.speed(1.0); //drive forward
+                            } 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;
 }