Diff: wheelchair.cpp

Revision:

37:e0e6d3fe06a2

Parent:

35:5a2fed4c2e9f

Child:

38:cb87f4e353b9

--- a/wheelchair.cpp	Tue Jul 09 21:19:19 2019 +0000
+++ b/wheelchair.cpp	Tue Jul 09 23:36:58 2019 +0000
@@ -76,7 +76,7 @@
 }
 
 /*************************************************************************
-*      -------------------------------------------------------------     *
+*      Thread checks ToF sensors for safety of wheelchair movement       *
 **************************************************************************/
 void Wheelchair::ToFSafe_thread()
 {
@@ -140,46 +140,93 @@
     else
         forwardSafety = 0;
 
-    /*-------Side Tof begin----------*/
-
+    /*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;
+    double currAngularVelocity = imu->gyro_x(); //Current angular velocity from IMU
+    double angle = imu->yaw() * 3.14159 / 180; //from IMU, in rads
+    double arcLength = WheelchairRadius * currAngularVelocity * 
+                       currAngularVelocity / (2 * maxAngularDeceleration); //S = r*Ө, in cm
+   
+    /* 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
+    if(sensor3 <= minWallLength) {
+        leftSafety = 1;
+        out-> printf("Detecting wall to the left!\n");
+    }
+    else{
+        leftSafety = 0;
+    }
+    
+    if(sensor6 <= minWallLength) {
+        rightSafety = 1;
+        out-> printf("Detecting wall to the right!\n");
+    }
+    else {
+        rightSafety = 0;
     }
     
-    /*************************************************************************
-    * 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;
+    /*Check whether safe to keep turning 
+    Know the exact moment you can stop the chair going at a certain speed before 
+    its too late*/
+    if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (sensor3/10 <= arcLength + 10)) {
+        leftSafety = 1; //Not safe to turn left
+        out-> printf("Too fast to the left!\n");
+    }
+    else{
+        leftSafety = 0;
+       }
+    if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (sensor6/10 <= arcLength + 10)) {
+        rightSafety = 1; //Not safe to turn right
+        out-> printf("Too fast to the right!\n");
     }
-
-    /*-------Side Tof end -----------*/
+    else{
+        rightSafety = 0;
+        }
+      
+    //Safe to continue turning 
+    //Check if can turn left and back side sensors
+    
+    //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
+        }
+    }
+    //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;
+        }
+    }*/
+      
+    /*Side Tof end*/
+    
 }
 
 /*************************************************************************
@@ -199,7 +246,7 @@
     /* Initializes IMU Library */
     out = pc;                                                                           // "out" is called for serial monitor
     out->printf("on\r\n");
-    imu = new chair_BNO055(pc, time);
+    imu = new IMUWheelchair(pc, time);
     Wheelchair::stop();                                                                 // Wheelchair is initially stationary
     imu->setup();                                                                       // turns on the IMU
     wheelS = qeiS;                                                                      // "wheel" is called for encoder
@@ -269,7 +316,7 @@
 void Wheelchair::right()
 {
     //if safe to move, from ToFSafety
-    if(sideSafety == 0) {
+    if(rightSafety == 0) {
         x->write(def);
         y->write(low);
     }
@@ -280,7 +327,7 @@
 void Wheelchair::left()
 {
     //if safe to move, from ToFSafety
-    if(sideSafety == 0) {
+    if(leftSafety == 0) {
         x->write(def);
         y->write(high);
     }