1
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: wheelchairControlSumer2019
Diff: wheelchair.cpp
- Revision:
- 30:b24d73663499
- Parent:
- 27:da718b990837
diff -r 4519f4cdcb5d -r b24d73663499 wheelchair.cpp --- a/wheelchair.cpp Mon Jul 01 16:36:47 2019 +0000 +++ b/wheelchair.cpp Wed Jun 03 20:25:18 2020 +0000 @@ -1,4 +1,3 @@ - #include "wheelchair.h" bool manual_drive = false; // Variable changes between joystick and auto drive @@ -9,9 +8,20 @@ volatile double vIn, vOut, vDesired; // Variables for PID Velosity volatile double vInS, vOutS, vDesiredS; // Variables for PID Slave Wheel volatile double yIn, yOut, yDesired; // Variables for PID turn velosity - +// int* ToFDataPointer1; +// int* ToFDataPointer2; + +int ledgeArrayLF[150]; +int ledgeArrayRF[150]; +int* ToFDataPointer1 = ledgeArrayLF; +int* ToFDataPointer2 = ledgeArrayRF; +statistics LFTStats(ToFDataPointer1, 149, 1); +statistics RFTStats(ToFDataPointer2, 149, 1); +int k = 0; + double dist_old, curr_pos; // Variables for odometry position - +double outlierToF[4]; + PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor @@ -19,14 +29,14 @@ PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT); // Slave Velosity PID Constructor PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Angular Velosity PID Constructor - + /* Thread measures current angular position */ void Wheelchair::compass_thread() { curr_yaw = imu->yaw(); z_angular = curr_yaw; } - + /* Thread measures velocity of wheels and distance traveled */ void Wheelchair::velocity_thread() { @@ -34,11 +44,29 @@ curr_velS = wheelS->getVelocity(); curr_pos = wheel->getDistance(53.975); } - -void Wheelchair::assistSafe_thread() + +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]); @@ -46,43 +74,101 @@ //out->printf("\r\n"); + k++; + + if (k == 150) { + k = 0; + } + + 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");*/ + + outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); + 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); - } + } 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; + 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 } } + + else if ((runningAverage[0] > outlierToF[0]) || (runningAverage[1] > outlierToF[1])) { + forwardSafety = 1; + out->printf("I'M STOPPING BECAUSE OF A LEDGE\r\n"); + } + 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 */ Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, VL53L1X** ToFT) @@ -90,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"); @@ -103,31 +191,37 @@ wheel = qei; ToF = ToFT; // passes pointer with addresses of ToF sensors - for(int i = 0; i < 12; i++) // initializes the ToF Sensors + for(int i = 0; i < 12; i++) // initializes the ToF Sensors { (*(ToF+i))->initReading(0x31+((0x02)*i), 50000); } out->printf("wheelchair setup done \r\n"); // Make sure it initialized; prints in serial monitor ti = time; - for(int i = 0; i < 100; i++) + for(int i = 0; i < 10; i++) + { + (*(ToF+1))->readFromOneSensor(); + (*(ToF+1))->readFromOneSensor(); + } + for(int i = 0; i < 150; i++) { ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor(); - ledgeArrayRF[i] = (*(ToF+1))->readFromOneSensor(); + ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor(); } - int* aaa = ledgeArrayLF; - statistics LFTStats(aaa, 100, 0); - out->printf("stadistics = %f, %f", LFTStats.mean(), LFTStats.stdev()); + + outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); + outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); + myPID.SetMode(AUTOMATIC); // PID mode: Automatic } - + /* Move wheelchair with joystick on manual mode */ void Wheelchair::move(float x_coor, float y_coor) { /* Scales one joystick measurement to the chair's joystick measurement */ float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; - + /* Sends the scaled joystic values to the chair */ x->write(scaled_x); y->write(scaled_y); @@ -155,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 */ @@ -172,7 +272,7 @@ x->write(def); y->write(def); } - + /* Counter-clockwise is - * Clockwise is + * Range of deg: 0 to 360 @@ -180,7 +280,7 @@ */ void Wheelchair::pid_right(int deg) { - bool overturn = false; //Boolean if angle over 360˚ + bool overturn = false; // Boolean if angle over 360˚ out->printf("pid right\r\r\n"); x->write(def); // Update x sent to chair to be stationary @@ -247,7 +347,7 @@ } myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D - myPID.SetOutputLimits(0,high-def-.12); //Limit is set to the differnce between def and low + myPID.SetOutputLimits(0,high-def-.12); // Limit is set to the differnce between def and low myPID.SetControllerDirection(REVERSE); // PID mode: Reverse /* PID stops when approaching a litte more than desired angle */ @@ -277,7 +377,7 @@ /* Saftey stop for wheelchair */ Wheelchair::stop(); out->printf("done \r\n"); - + } /* This constructor determines whether to turn left or right */ @@ -308,9 +408,9 @@ { Wheelchair::pid_left(turnAmt); } - + } - + /* This constructor takes in distance to travel and adjust to move forward */ void Wheelchair::pid_forward(double mm) { @@ -336,31 +436,30 @@ { break; } - + Input = wheel->getDistance(53.975); // Gets distance from Encoder into PID wait(.05); // Slight Delay: *****Test without myPIDDistance.Compute(); // Compute distance traveled by chair tempor = Output + def; // Temporary output variable x->write(tempor); // Update x sent to chair - + /* Prints to serial monitor the distance traveled by chair */ out->printf("distance %f\r\n", Input); } } - + /* This constructor returns the relative angular position of chair */ double Wheelchair::getTwistZ() { return imu->gyro_z(); } - + /* This constructor computes the relative angle for Twist message in ROS */ void Wheelchair::pid_twistA() { /* Initialize variables for angle and update x,y sent to chair */ - char c; double temporA = def; y->write(def); x->write(def); @@ -396,7 +495,7 @@ } } - + /* This constructor computes the relative velocity for Twist message in ROS */ void Wheelchair::pid_twistV() { @@ -434,7 +533,7 @@ { x->write(def); y->write(def); - + vel = 0; vDesired = 0; dist_old = 0; @@ -482,7 +581,7 @@ wait(.01); // Small delay (milliseconds) } } - + /* This constructor calculates the relative position of the chair everytime the encoders reset * by setting its old position as the origin to calculate the new position */ @@ -498,26 +597,26 @@ dist_old = dist_new; } - + /* This constructor prints the Odometry message to the serial monitor */ void Wheelchair::showOdom() { out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular); } - + /* This constructor returns the approximate distance based on the wheel diameter */ float Wheelchair::getDistance() { return wheel->getDistance(Diameter); } - -/* This constructor resets the wheel encoder's */ + +/* This constructor resets the wheel encoders */ void Wheelchair::resetDistance() { wheel->reset(); } - - + + /*Predetermined paths For Demmo*/ void Wheelchair::desk() {