Where we will test the side ToF sensors
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Diff: wheelchairControlSideTof/wheelchair.cpp
- Revision:
- 21:d1faccb96146
- Parent:
- 20:3c1b58654e67
--- 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