Diff: wheelchair.cpp

Revision:

43:9858a580b3d7

Parent:

42:62c49ad06707

--- a/wheelchair.cpp	Wed Aug 07 19:01:17 2019 +0000
+++ b/wheelchair.cpp	Tue Aug 13 19:22:08 2019 +0000
@@ -195,8 +195,11 @@
     /*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
+    int sensor27 = ToFV[98];      //left side on back
+    int sensor28 = ToFV[99];      //right side on back
+    
+    int angleRight = ToFV[100];   //The angled sensor for blindspots   
+    int angleLeft = ToFV[101];   
     
     double currAngularVelocity = imu->gyro_x(); //Current angular velocity from IMU
     double angle = imu->yaw() * 3.14159 / 180; //from IMU, in rads
@@ -206,8 +209,8 @@
     /* 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 */
-        
-    //When either sensors too close to the wall, can't turn
+    
+    //When either sensors too close to the wall, can't turn 
     if(sensor3 <= minWallLength) {
         leftSafety = 1;
         out-> printf("Detecting wall to the left!\n");
@@ -224,6 +227,46 @@
         rightSafety = 0;
     }
     
+    //For the back side ToF. 
+    
+    //Scenario 1: Wheelchair close to right side wall and you want to turn left.
+    //The back right side sensor say too close and corner get bumped
+    if(sensor28 <= minWallLength) {
+        leftSafety = 1;
+        out-> printf("Detecting wall to the right at back! Don't turn left. \n");
+    }
+    else {
+        leftSafety = 0;
+    } 
+    
+    //Scenario 2: Wheelchair close to left side wall and you want to turn right.
+    //The back left side sensor say too close and corner get bumped
+     if(sensor27 <= minWallLength) {
+        rightSafety = 1;
+        out-> printf("Detecting wall to the left at back! Don't turn right. \n");
+    }
+    else {
+        rightSafety = 0;
+    }   
+    
+    //Below for the angled sensor when detect something in the blindspot between
+    //the side sensors
+    if(angleLeft <= 100000) {
+        leftSafety = 1;
+        out-> printf("Blind spot on the left side\n");
+    }
+    else{
+        leftSafety = 0;
+    }
+    
+    if(angleRight <= 100000) {
+        rightSafety = 1;
+        out-> printf("Blindspot on the right side\n");
+    }
+    else{
+        rightSafety = 0;
+    }
+    
     /*Check whether safe to keep turning 
     Know the exact moment you can stop the chair going at a certain speed before 
     its too late*/
@@ -234,7 +277,7 @@
     }
     else{
         leftSafety = 0;
-       }
+    }
     if((currAngularVelocity * currAngularVelocity > 2 * 
         maxAngularDeceleration * angle) && (sensor6/10 <= arcLength + 10)) {
         rightSafety = 1; //Not safe to turn right
@@ -242,42 +285,51 @@
     }
     else{
         rightSafety = 0;
-        }
+    }
       
-    //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
-    
-    if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor7 < curr_vel*curr_vel*1000*1000 || 
-    2 * maxDecelerationSlow*sensor8 < curr_vel*curr_vel*1000*1000) && 
-    (sensor7 < 1500 || sensor8 < 1500)) ||
-    550 > sensor7 || 550 > sensor8)
-    {
-        //out->printf("i am in danger\r\n");
-        if(x->read() > def)
-        {
-            x->write(def);
-            backwardSafety = 1;// You cannot move backward
-        }
+    //Deal with the speed of turning when moving forward, stop in time when
+    //Scenario 1: Turning to the right forward, but the left back side sensor pick
+    //something up. Can't turn right
+/*    if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (sensor27/10 <= arcLength + 10)) {
+        rightSafety = 1; //Not safe to turn left
+        out-> printf("Too fast to the left! Back left area danger \n");
+    }
+    else{
+        rightSafety = 0;
+    }  
+    //Scenario 2: Turning to the left forward, but the right back side sensor pick
+    //something up. CAn't turn left
+     if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (sensor28/10 <= arcLength + 10)) {
+        leftSafety = 1; //Not safe to turn left
+        out-> printf("Too fast to the left! Back right area danger\n");
     }
-    //When going to fast to stop from wall
-    else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor7 < curr_vel*curr_vel*1000*1000 || 
-    2 * maxDecelerationFast*sensor8 < curr_vel*curr_vel*1000*1000) && 
-    (sensor7 < 1500 || sensor8 < 1500)) ||
-    550 > sensor7 || 550 > sensor8)
-    {
-        //out->printf("i am in danger\r\n");
-        if(x->read() > def)
-        {
-            x->write(def);
-            backwardSafety = 1;
-        }
-    }*/
+    else{
+        leftSafety = 0;
+    } */
       
-    /*Side Tof end*/
+    //In the blindspot of the side sensor, keep wheelchair from turning if detect in
+    //that blind area. Account from certain distance from the angled sensor and how
+    //fast turning/moving toward obstacle.
+     if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (angleLeft/10 <= arcLength + 10)) {
+        leftSafety = 1; //Not safe to turn left
+        out-> printf("Too fast to the left!, blindspot \n");
+    }
+    else{
+        leftSafety = 0;
+    }
+    if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (angleRight/10 <= arcLength + 10)) {
+        rightSafety = 1; //Not safe to turn right
+        out-> printf("Too fast to the right!, blindspot \n");
+    }
+    else{
+        rightSafety = 0;
+    }   
+ 
+     /*Side Tof end*/   
     
 }