Diff: wheelchairControlSideTof/wheelchair.cpp

Revision:

21:d1faccb96146

Parent:

20:3c1b58654e67

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/wheelchair.cpp
--- a/wheelchairControlSideTof/wheelchair.cpp	Tue Jul 02 21:17:03 2019 +0000
+++ b/wheelchairControlSideTof/wheelchair.cpp	Tue Jul 09 17:52:32 2019 +0000
@@ -85,8 +85,8 @@
         runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
     }
    
-    int sensor1 = ToFV[0];
-    int sensor4 = ToFV[3];
+    int sensor1 = ToFV[0]; //front sensor
+    int sensor4 = ToFV[3]; //front sensor
     //out->printf("%d, %d\r\n", ToFV[1], runningAverage[0]);
     if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || 
     2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && 
@@ -121,16 +121,16 @@
     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*Ө
+    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
@@ -139,7 +139,7 @@
     //Check if can clear side
     
     //When either sensors too close to the wall, can't turn
-    if(sensor3 <= MIN_WALL_LENGTH) {
+    if(sensor3 <= minWallLength) {
         leftSafety = 1;
         out-> printf("Detecting wall to the left!\n");
     }
@@ -147,7 +147,7 @@
         leftSafety = 0;
     }
     
-    if(sensor6 <= MIN_WALL_LENGTH) {
+    if(sensor6 <= minWallLength) {
         rightSafety = 1;
         out-> printf("Detecting wall to the right!\n");
     }
@@ -155,21 +155,66 @@
         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
+    //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");
+    }
+    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
     
-    /*-------Side Tof end -----------*/
+    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 forward
+        }
+    }
+    //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 -------------------------------*/
 }
 
 /* Constructor for Wheelchair class */
-Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, 
+Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, Timer*imuT, QEI* qei, QEI* qeiS, 
 VL53L1X** ToFT)
 {
     x_position = 0;
@@ -181,7 +226,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, imuT);
     Wheelchair::stop();                                                                 // Wheelchair is initially stationary
     imu->setup();                                                                       // turns on the IMU
     wheelS = qeiS;                                                                      // "wheel" is called for encoder