finished commenting on the .cpp file and changed the statistics library name
Dependencies: QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: Version1-3 Version1-5
Revision 37:e0e6d3fe06a2, committed 2019-07-09
- Comitter:
- JesiMiranda
- Date:
- Tue Jul 09 23:36:58 2019 +0000
- Parent:
- 36:4131f0480f70
- Commit message:
- ff
Changed in this revision
diff -r 4131f0480f70 -r e0e6d3fe06a2 IMU6050Ver11.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU6050Ver11.lib Tue Jul 09 23:36:58 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Affordable-Smart-Wheelchair/code/IMU6050Ver11/#f2e2692762ac
diff -r 4131f0480f70 -r e0e6d3fe06a2 chair_BNO055.lib --- a/chair_BNO055.lib Tue Jul 09 21:19:19 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/jvfausto/code/chair_BNO055/#ce8aa8208590
diff -r 4131f0480f70 -r e0e6d3fe06a2 wheelchair.cpp --- 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); }
diff -r 4131f0480f70 -r e0e6d3fe06a2 wheelchair.h --- a/wheelchair.h Tue Jul 09 21:19:19 2019 +0000 +++ b/wheelchair.h Tue Jul 09 23:36:58 2019 +0000 @@ -3,7 +3,7 @@ /************************************************************************* * Importing libraries into wheelchair.h * **************************************************************************/ -#include "chair_BNO055.h" +#include "IMUWheelchair.h" #include "PID.h" #include "QEI.h" #include "VL53L1X.h" @@ -41,11 +41,11 @@ /************************************************************************* *IMU definitions for turning wheelchair **************************************************************************/ -#define WHEELCHAIR_RADIUS 56 //distance from IMU to edge of wheelchair(cm) -#define MAX_ANGULAR_DECELERATION 60 //found through testing, max +#define WheelchairRadius 80 //distance from IMU to edge of wheelchair(cm) +#define maxAngularDeceleration 1.04 //found through testing, max //acceleration at which chair can - //stop while turning. In degree per sec -#define MIN_WALL_LENGTH 10 // minimum distance from wall to ToF (cm) + //stop while turning. In rads per sec +#define minWallLength 10 // minimum distance from wall to ToF (cm) /************************************************************************* * * * Wheelchair class * @@ -217,8 +217,10 @@ double vel; double test1, test2; bool forwardSafety; - bool sideSafety; //to check if can turn - double curr_yaw, curr_velS; // Variable that contains current relative angle + bool backwardSafety;//Check if can move backward + bool leftSafety; //to check if can turn left + bool rightSafety; //to check if can turn right + double curr_yaw, curr_velS;// Variable that contains current relative angle private: /************************************************************************ @@ -238,14 +240,12 @@ DigitalIn* e_button; //Pointer to e_button - chair_BNO055* imu; // Pointer to IMU + IMUWheelchair* imu; // Pointer to IMU Serial* out; // Pointer to Serial Monitor Timer* ti; // Pointer to the timer QEI* wheel; // Pointer to encoder QEI* wheelS; // Pointer to encoder VL53L1X** ToF; // Arrays of pointers to ToF sensors - - };