Diff: wheelchair.cpp

Revision:

34:f10a892f5e85

Parent:

32:fb26baa75d44

Child:

35:a3585c52723e

--- a/wheelchair.cpp	Tue Jul 02 00:42:05 2019 +0000
+++ b/wheelchair.cpp	Tue Jul 02 16:27:16 2019 +0000
@@ -120,6 +120,31 @@
     
     else
         forwardSafety = 0;
+        
+    /*-------Side Tof begin----------*/
+    
+    int sensor3 = ToFV[2]; //front left
+    int sensor6 = ToFV[5]; //front right
+    int sensor9 = ToFV[8]; //back
+    int sensor12 = ToFV[11]; //back
+    
+    //float currAngularVelocity = IMU DATA; //Current angular velocity from IMU
+    //float angle; //from IMU YAW, convert to cm
+    //float arcLength = angle * WHEELCHAIR_RADIUS; //S = r*Ө
+    
+    //Clear the front side first, else continue going straight or can't turn
+    //After clearing the front sideand movinf forward, check if can clear
+    // the back when turning
+    
+    //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;
+    }
+    
+    /*-------Side Tof end -----------*/
 }
 
 /* Constructor for Wheelchair class */