1
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: wheelchairControlSumer2019
Revision 30:b24d73663499, committed 2020-06-03
- Comitter:
- jvfausto
- Date:
- Wed Jun 03 20:25:18 2020 +0000
- Parent:
- 29:4519f4cdcb5d
- Commit message:
- For sharing
Changed in this revision
wheelchair.cpp | Show annotated file Show diff for this revision Revisions of this file |
wheelchair.h | Show annotated file Show diff for this revision Revisions of this file |
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() {
diff -r 4519f4cdcb5d -r b24d73663499 wheelchair.h --- a/wheelchair.h Mon Jul 01 16:36:47 2019 +0000 +++ b/wheelchair.h Wed Jun 03 20:25:18 2020 +0000 @@ -9,32 +9,40 @@ #include "VL53L1X.h" #include "statistics.h" #include "Watchdog.h" - + #include <ros.h> #include <geometry_msgs/Twist.h> #include <nav_msgs/Odometry.h> - + /************************************************************************* * Joystick has analog out of 200-700, scale values between 1.3 and 3.3 * * Here are some global constants for joystick * **************************************************************************/ -#define def (2.5f/3.3f) //Default axis on joystick to stay neutral; used on x and y axis -#define high 3.3f/3.3f //High power on joystick; used on x and y axis -#define low (1.7f/3.3f) //Low power on joystick; used on x and y axis -#define offset .03f //Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision -#define process .1 //Defines default time delay in seconds +#define def (2.5f/3.3f) // Default axis on joystick to stay neutral; used on x and y axis +#define high 3.3f/3.3f // High power on joystick; used on x and y axis +#define low (1.7f/3.3f) // Low power on joystick; used on x and y axis +#define offset .03f // Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision +#define process .1 // Defines default time delay in seconds /************************************************************************* *Pin plug-in for Nucleo-L432KC/Compatible with Nucleo-400 series (F767ZI)* **************************************************************************/ -#define xDir PA_6 //* PWM Pins */ +#define xDir PA_6 // PWM Pins #define yDir PA_5 -#define Encoder1 D7 //*Digital In Pull Up Pin */ +#define Encoder1 D7 // Digital In Pull Up Pin #define Encoder2 D8 -#define Diameter 31.75 //Diameter of encoder wheel +#define Diameter 31.75 // Diameter of encoder wheel #define maxDecelerationSlow 120 #define maxDecelerationFast 30 #define ToFSensorNum 12 - + +/************************************************************************* +*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 + //acceleration at which chair can + //stop while turning. In degree per sec +#define MIN_WALL_LENGTH 10 // minimum distance from wall to ToF (cm) /************************************************************************* * * * Wheelchair class * @@ -51,12 +59,12 @@ **************************************************************************/ Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS, VL53L1X** ToF); - + /************************************************************************* * This method is to move using the joystick * **************************************************************************/ void move(float x_coor, float y_coor); - + /************************************************************************* * This method is to drive the wheelchair forward manually (NO PID used)* ************************************************************************ */ @@ -65,37 +73,37 @@ * This method is to drive the wheelchair backwards (NO PID used) * ************************************************************************ */ void backward(); - + /************************************************************************* * This method is to turn the wheelchair right manually (NO PID used) * ************************************************************************ */ void right(); - + /************************************************************************* * This method is to turn the wheelchair left manually (NO PID used) * ************************************************************************ */ void left(); - + /************************************************************************* * This method stops the wheelchair and reset the joystick position * ************************************************************************ */ void stop(); - + /************************************************************************* * This method is a thread that will obtain the IMU information such * * as the gyroscope x,y axis. Z-axis is not used. * ************************************************************************ */ void compass_thread(); - + /************************************************************************* * This method is a thread that will calculate the velocity of the * * wheechair using the encoder values this is being obatined. * ************************************************************************ */ void velocity_thread(); - + //not being used void rosCom_thread(); - + /************************************************************************* * This method is a thread that iterates through all the sensor's * * values and determines whether or not the wheelchair is about to hit * @@ -103,61 +111,61 @@ * the chair, then the chair will safely halt and allow movement in the * * direction opposed to where an object is detected. * ************************************************************************ */ - void assistSafe_thread(); - + void ToFSafe_thread(); + /************************************************************************* * This method is a thread that will constantly be checking the value * * of the emergency button. If the button is pressed, then the chair * * will stop and the entire system will reset. * ************************************************************************ */ void emergencyButton_thread(); - + /************************************************************************* * This method gets the encoder values and calculates the distance since* * the last encoder reset. * **************************************************************************/ float getDistance(); - + /************************************************************************* * This method resets the encoder value to recalculate distance * **************************************************************************/ void resetDistance(); - - + + /************************************************************************* * * * PID Control Methods * * * *************************************************************************/ - - + + /************************************************************************* * This method moves the wheelchair x-millimiters forward using PID * **************************************************************************/ void pid_forward(double mm); - + /************************************************************************* * This method turns the chair right a certain amount of degrees using PID* **************************************************************************/ void pid_right(int deg); - + /************************************************************************* * This method turns the chair left a certain amount of degrees using PID * **************************************************************************/ void pid_left(int deg); - + /************************************************************************* * This method determines whether to turn the wheelchair left or right * **************************************************************************/ void pid_turn(int deg); - - + + /************************************************************************* * * * ROS-Embed Communication Methods * * * *************************************************************************/ - + /************************************************************************* * This method computes the relative angle for Twist message in ROS * *************************************************************************/ @@ -185,14 +193,14 @@ * This method prints the Odometry message to the serial monitor * *************************************************************************/ void showOdom(); - + /************************************************************************* * (not being used) Functions with a predetermined path (demo) * *************************************************************************/ void desk(); void kitchen(); void desk_to_kitchen(); - + /************************************************************************* * Public Variables * **************************************************************************/ @@ -206,33 +214,36 @@ 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 - + private: int runningAverage[4]; // Expected data used to compare whether of not there is a ledge. This serves as a ground base // Array is used for calibrating the time of flight sensors // Used to calculate stdev and mean on - int ledgeArrayLF[100]; - int ledgeArrayRF[100]; - + + + /* Pointers for the joystick speed */ PwmOut* x; PwmOut* y; - + //Pointers for PCB PwmOut* on; PwmOut* off; - + DigitalIn* e_button; //Pointer to e_button - + chair_BNO055* 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 - - + + + + }; #endif \ No newline at end of file