Diff: wheelchair.cpp

Revision:

35:a3585c52723e

Parent:

34:f10a892f5e85

Child:

36:24c12658bef1

--- a/wheelchair.cpp	Tue Jul 02 16:27:16 2019 +0000
+++ b/wheelchair.cpp	Tue Jul 02 16:46:40 2019 +0000
@@ -141,7 +141,20 @@
     //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;
+        sideSafety = 1;
+    }
+    
+    //Check whether safe to keep turnin, user control <-- make sure 
+    //currAngularVelocity is in correct units. Know the exact moment you can
+    //stop the chair going at a certain speed before its too late
+    else if((currAngularVelocity * currAngularVelocity > 2 * 
+             MAX_ANGULAR_DECELERATION * angle) && (sensor3 <= angle ||
+             sensor6 <= angle)) {
+        sideSafety = 1; //Not safe to turn
+    }
+    //Safe to continue turning
+    else {
+        sideSafety = 0;
     }
     
     /*-------Side Tof end -----------*/
@@ -236,15 +249,21 @@
 /* Automatic mode: move right and update x,y coordinate sent to chair */
 void Wheelchair::right()                                                             
 {
-    x->write(def);
-    y->write(low);
+    //if safe to move, from ToFSafety
+    if(sideSafety == 0) {
+       x->write(def);
+       y->write(low);
+    }
 }
 
  /* Automatic mode: move left and update x,y coordinate sent to chair */
 void Wheelchair::left()                                                               
 {
-    x->write(def);
-    y->write(high);
+    //if safe to move, from ToFSafety
+    if(sideSafety == 0) {
+        x->write(def);
+        y->write(high);
+    }
 }
  
 /* Stop the wheelchair */