Diff: wheelchairControlSideTof/wheelchair.cpp

Revision:

20:3c1b58654e67

Parent:

18:a10277a63b53

Child:

21:d1faccb96146

--- a/wheelchairControlSideTof/wheelchair.cpp	Tue Jul 02 17:45:37 2019 +0000
+++ b/wheelchairControlSideTof/wheelchair.cpp	Tue Jul 02 21:17:03 2019 +0000
@@ -139,10 +139,20 @@
     //Check if can clear side
     
     //When either sensors too close to the wall, can't turn
-    if((sensor3 <= MIN_WALL_LENGTH) || (sensor6 <= MIN_WALL_LENGTH) ||
-       (sensor12 <= MIN_WALL_LENGTH)) {
-        sideSafety = 1;
-        out-> printf("Detecting wall!\n");
+    if(sensor3 <= MIN_WALL_LENGTH) {
+        leftSafety = 1;
+        out-> printf("Detecting wall to the left!\n");
+    }
+    else{
+        leftSafety = 0;
+    }
+    
+    if(sensor6 <= MIN_WALL_LENGTH) {
+        rightSafety = 1;
+        out-> printf("Detecting wall to the right!\n");
+    }
+    else {
+        rightSafety = 0;
     }
     
     //Check whether safe to keep turnin, user control <-- make sure 
@@ -154,9 +164,6 @@
      //   sideSafety = 1; //Not safe to turn
     //}
     //Safe to continue turning
-    else {
-        sideSafety = 0;
-    }
     
     /*-------Side Tof end -----------*/
 }
@@ -251,7 +258,7 @@
 void Wheelchair::right()                                                             
 {
     //if safe to move, from ToFSafety
-    if(sideSafety == 0) { 
+    if(rightSafety == 0) { 
        out->printf("Moving right!\n");
        x->write(def);
        y->write(low);
@@ -262,7 +269,7 @@
 void Wheelchair::left()                                                               
 {
     //if safe to move, from ToFSafety
-    if(sideSafety == 0) {
+    if(leftSafety == 0) {
         out->printf("Moving left!\n");
         x->write(def);
         y->write(high);