a
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: wheelchairControlSumer19 Version1-0
Diff: wheelchair.cpp
- Revision:
- 34:b89967adc86c
- Parent:
- 33:f3585571f11e
diff -r f3585571f11e -r b89967adc86c wheelchair.cpp --- a/wheelchair.cpp Tue Jul 02 16:40:41 2019 +0000 +++ b/wheelchair.cpp Tue Jul 02 17:49:18 2019 +0000 @@ -45,10 +45,28 @@ curr_pos = wheel->getDistance(53.975); } +void Wheelchair::emergencyButton_thread () +{ + while(1) { + while(!e_button) { + + //Stop wheelchair + Wheelchair::stop(); + printf("E-button has been pressed\r\n"); + off->write(high); // Turn off PCB + on->write(0); // Make sure PCB not on + //Reset Board + NVIC_SystemReset(); + + } + + } +} + void Wheelchair::ToFSafe_thread() { int ToFV[12]; - for(int i = 0; i < 6; i++) // reads from the ToF Sensors + for(int i = 0; i < 6; i++) // Reads from the ToF Sensors { ToFV[i] = (*(ToF+i))->readFromOneSensor(); //out->printf("%d ", ToFV[i]); @@ -64,22 +82,15 @@ ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor(); ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor(); + /*for(int i = 0; i < 100; i++) { out->printf("%d, ",ledgeArrayRF[i]); } out->printf("\r\n");*/ -// statistics LFTStats(ToFDataPointer1, 99, 1); -// statistics RFTStats(ToFDataPointer1, 99, 1); - //out->printf("Right Mean: %f ", RFTStats.mean()); - //out->printf("Std Dev: % f", RFTStats.stdev()); + outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); - //out->printf("Left Mean: %f ", LFTStats.mean()); - //out->printf("Std Dev: %f ", LFTStats.stdev()); - outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); - //out->printf("New outliers: %f, %f\n", outlierToF[0], outlierToF[1]); - - + outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); for(int i = 0; i < 4; i++) { // Reads from the ToF Sensors runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); @@ -87,29 +98,27 @@ int sensor1 = ToFV[0]; int sensor4 = ToFV[3]; - //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) && (sensor1 < 1500 || sensor4 < 1500)) || 550 > sensor1 || 550 > sensor4) { - //out->printf("i am in danger\r\n"); if(x->read() > def) { x->write(def); forwardSafety = 1; // You cannot move forward } } + else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || 2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && (sensor1 < 1500 || sensor4 < 1500)) || 550 > sensor1 || 550 > sensor4) { - //out->printf("i am in danger\r\n"); if(x->read() > def) { x->write(def); - forwardSafety = 1; + forwardSafety = 1; // You cannot move forward } } @@ -120,6 +129,44 @@ else forwardSafety = 0; + + /*-------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; + } + + // 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; + } + + /*-------Side Tof end -----------*/ } /* Constructor for Wheelchair class */ @@ -129,9 +176,11 @@ x_position = 0; y_position = 0; forwardSafety = 0; + /* Initializes X and Y variables to Pins */ x = new PwmOut(xPin); y = new PwmOut(yPin); + /* Initializes IMU Library */ out = pc; // "out" is called for serial monitor out->printf("on\r\n"); @@ -160,19 +209,9 @@ ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor(); } - - //statistics LFTStats(ToFDataPointer1, 99, 1); - // //ToFDataPointer = ledgeArrayRF; - //statistics RFTStats(ToFDataPointer2, 99, 1); - - outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); - - //out->printf("Left outlier = %f\n", outlierToF[0]); - //out->printf("Right outlier = %f\n", outlierToF[1]); - //out->printf("Left statistics = %f, %f\n", LFTStats.mean(), LFTStats.stdev()); - //out->printf("Right statistics = %f, %f\n", RFTStats.mean(), RFTStats.stdev()); + myPID.SetMode(AUTOMATIC); // PID mode: Automatic } @@ -210,15 +249,21 @@ /* Automatic mode: move right and update x,y coordinate sent to chair */ void Wheelchair::right() { - x->write(def); - y->write(low); + //if safe to move, from ToFSafety + if(sideSafety == 0) { + x->write(def); + y->write(low); + } } /* Automatic mode: move left and update x,y coordinate sent to chair */ void Wheelchair::left() { - x->write(def); - y->write(high); + //if safe to move, from ToFSafety + if(sideSafety == 0) { + x->write(def); + y->write(high); + } } /* Stop the wheelchair */ @@ -566,7 +611,7 @@ return wheel->getDistance(Diameter); } -/* This constructor resets the wheel encoder's */ +/* This constructor resets the wheel encoders */ void Wheelchair::resetDistance() { wheel->reset();