Remote Control Car

Diff: main.cpp

Revision:

4:29839de66eae

Parent:

3:4956cc0efdf3

Child:

5:b1b250500440

--- a/main.cpp	Thu Nov 19 17:23:54 2020 +0000
+++ b/main.cpp	Thu Nov 19 21:19:12 2020 +0000
@@ -1,29 +1,27 @@
 #include "mbed.h"
-#include "XNucleo53L0A1.h"
 #include "Servo.h"
 #include "motordriver.h"
+#include "ultrasonic.h"
 #include <stdio.h>
 Serial pc(USBTX,USBRX);
-Serial blue(p9,p10);
-DigitalOut shdn(p26);
+Serial blue(p13,p14);
 DigitalOut myled(LED1);      
 
-#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;
+int state = 0; //global variable stop state 
+int status1;
+int status2;
+int distanceCenter;
+int distanceLeft;
+int distanceRight;
 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
@@ -74,7 +72,7 @@
 }
 
 void autoPilot() {
-    if ((distanceCenter >= 250 || distanceCenter == 0)) {
+    if (distanceCenter >= 250) {
         M.speed(1.0);
         S = 0.5;
     }
@@ -86,28 +84,41 @@
     }
 }
 
+void dist1(int distance)
+{
+    distanceCenter = distance;
+}
+
+void dist2(int distance)
+{
+    distanceLeft = distance;
+}
+
+void dist3(int distance)
+{
+    distanceRight = distance;
+}
+
+ultrasonic muCenter(p6, p7, .1, 1, &dist1); //sonar 1 initialization
+ultrasonic muLeft(p17, p18, .1, 1, &dist2); //sonar 2 initialization
+ultrasonic muRight(p15, p16, .1, 1, &dist3); //sonar 3 initialization
+
+
 
 int main() {
-    //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);
-    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();
-    }
-    
+    //SONAR Initializations
+    muCenter.startUpdates();//SONAR 1 starts measuring the distance 
+    muLeft.startUpdates();//SONAR 2 starts measuring the distance 
+    muRight.startUpdates();//SONAR 3 starts measuring the distance 
     while(1) { //main loop for motor control
-        status = board->sensor_centre->get_distance(&distanceCenter);
-        pc.printf("D=%ld mm\r\n", distanceCenter);
+        pc.printf("Center D=%ld mm\r\n", distanceCenter);
+        pc.printf("Right D=%ld mm\r\n", distanceRight);
+        pc.printf("Left D=%ld mm\r\n", distanceLeft);
+        muCenter.checkDistance();  //SONAR measuring starts
+        muLeft.checkDistance();
+        muRight.checkDistance();
         getNewState();
-        if ((distanceCenter >= 250 || distanceCenter == 0) && state == FORWARD) {
+        if (distanceCenter >= 250 && state == FORWARD) {
             M.speed(1.0);
         }
         else if (state == REVERSE) {