Added more code for side and angled sensor
Dependencies: QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic
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); }