Added more code for side and angled sensor

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic

Revision:
39:5438557a8b4e
Parent:
38:cb87f4e353b9
Child:
40:11b8567c507b
--- a/wheelchair.cpp	Wed Jul 10 18:06:58 2019 +0000
+++ b/wheelchair.cpp	Wed Jul 10 19:30:57 2019 +0000
@@ -174,7 +174,7 @@
             550 > sensor7 || 550 > sensor10) {
         if(x->read() > def) {
             x->write(def);
-            backwardSafety = 1;          // You cannot move backward
+            backwardSafety = 1;          // You cannot move backwards
         }
     }
 
@@ -184,7 +184,7 @@
             550 > sensor7 || 550 > sensor10) {
         if(x->read() > def) {
             x->write(def);
-            backwardSafety = 1;          // You cannot move backward
+            backwardSafety = 1;          // You cannot move backwards
         }
     }
 
@@ -253,7 +253,7 @@
       
     //Safe to continue turning 
     //Check if can turn left and back side sensors
-    
+    // Staring t road and frequenctly checking
     //Check the back sensor
     /*int sensor7 = ToFV[0]; //back sensor NOTTT SURE
     int sensor8 = ToFV[3]; //back sensor
@@ -297,6 +297,7 @@
     x_position = 0;
     y_position = 0;
     forwardSafety = 0;
+    backwardSafety = 0;
 
     /* Initializes X and Y variables to Pins */
     x = new PwmOut(xPin);
@@ -366,8 +367,10 @@
 **************************************************************************/
 void Wheelchair::backward()
 {
-    x->write(low);
-    y->write(def);
+    if (backwardSafety == 0) {
+        x->write(low);
+        y->write(def);
+    }
 }
 /*************************************************************************
 *   Automatic mode: move right and update x,y coordinate sent to chair   *