Where we will test the side ToF sensors
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Diff: wheelchairControlSideTof/wheelchair.cpp
- Revision:
- 18:a10277a63b53
- Child:
- 20:3c1b58654e67
diff -r 770a161346ed -r a10277a63b53 wheelchairControlSideTof/wheelchair.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/wheelchairControlSideTof/wheelchair.cpp Tue Jul 02 17:27:51 2019 +0000 @@ -0,0 +1,647 @@ +#include "wheelchair.h" + +bool manual_drive = false; // Variable changes between joystick and auto drive +double encoder_distance; // Keeps distanse due to original position + +volatile double Setpoint, Output, Input, Input2; // Variables for PID +volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; // Variables for PID +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 +PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Velosity PID Constructor +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() +{ + curr_vel = wheel->getVelocity(); + curr_velS = wheelS->getVelocity(); + curr_pos = wheel->getDistance(53.975); +} + +void Wheelchair::ToFSafe_thread() +{ + int ToFV[12]; + for(int i = 0; i < 6; i++) // reads from the ToF Sensors + { + ToFV[i] = (*(ToF+i))->readFromOneSensor(); + //out->printf("%d ", ToFV[i]); + } + + //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");*/ +// 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]); + + + + 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; // 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; + } + } + + 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; + out-> printf("Detecting wall!\n"); + } + + //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) +{ + 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"); + imu = new chair_BNO055(pc, time); + Wheelchair::stop(); // Wheelchair is initially stationary + imu->setup(); // turns on the IMU + wheelS = qeiS; // "wheel" is called for encoder + wheel = qei; + ToF = ToFT; // passes pointer with addresses of 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 < 10; i++) + { + (*(ToF+1))->readFromOneSensor(); + (*(ToF+1))->readFromOneSensor(); + } + for(int i = 0; i < 150; i++) + { + ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor(); + 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 +} + +/* 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); +} + +/* Automatic mode: move forward and update x,y coordinate sent to chair */ +void Wheelchair::forward() +{ + //printf("current velosity; %f, curr vel S %f\r\n", curr_vel, curr_velS); + if(forwardSafety == 0) + { + x->write(high); + y->write(def+offset); + } + out->printf("%f, %f\r\n", curr_pos, wheelS->getDistance(53.975)); +} + +/* Automatic mode: move in reverse and update x,y coordinate sent to chair */ +void Wheelchair::backward() +{ + x->write(low); + y->write(def); +} + +/* Automatic mode: move right and update x,y coordinate sent to chair */ +void Wheelchair::right() +{ + //if safe to move, from ToFSafety + if(sideSafety == 0) { + out->printf("Moving right!\n"); + x->write(def); + y->write(low); + } +} + + /* Automatic mode: move left and update x,y coordinate sent to chair */ +void Wheelchair::left() +{ + //if safe to move, from ToFSafety + if(sideSafety == 0) { + out->printf("Moving left!\n"); + x->write(def); + y->write(high); + } +} + +/* Stop the wheelchair */ +void Wheelchair::stop() +{ + x->write(def); + y->write(def); +} + +/* Counter-clockwise is - + * Clockwise is + + * Range of deg: 0 to 360 + * This constructor takes in an angle from user and adjusts for turning right + */ +void Wheelchair::pid_right(int deg) +{ + 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 + Setpoint = curr_yaw + deg; // Relative angle we want to turn + pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user + + /* Turns on overturn boolean if setpoint over 360˚ */ + if(Setpoint > 360) + { + overturn = true; + } + + myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D + myPID.SetOutputLimits(0, def-low-.15); // Limit is set to the differnce between def and low + myPID.SetControllerDirection(DIRECT); // PID mode: Direct + + /* PID stops when approaching a litte less than desired angle */ + while(pid_yaw < Setpoint - 3) + { + /* PID is set to correct angle range if angle greater than 360˚*/ + if(overturn && curr_yaw < Setpoint-deg-1) + { + pid_yaw = curr_yaw + 360; + } + else + { + pid_yaw = curr_yaw; + } + + myPID.Compute(); // Does PID calculations + double tempor = -Output+def; // Temporary value with the voltage output + y->write(tempor); // Update y sent to chair + + /* Prints to serial monitor the current angle and setpoint */ + out->printf("curr_yaw %f\r\r\n", curr_yaw); + out->printf("Setpoint = %f \r\n", Setpoint); + + wait(.05); // Small delay (milliseconds) + } + + /* Saftey stop for wheelchair */ + Wheelchair::stop(); + out->printf("done \r\n"); +} + +/* Counter-clockwise is - + * Clockwise is + + * Range of deg: 0 to 360 + * This constructor takes in an angle from user and adjusts for turning left + */ +void Wheelchair::pid_left(int deg) +{ + bool overturn = false; //Boolean if angle under 0˚ + + out->printf("pid Left\r\r\n"); + x->write(def); // Update x sent to chair to be stationary + Setpoint = curr_yaw - deg; // Relative angle we want to turn + pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user + + /* Turns on overturn boolean if setpoint less than 0˚ */ + if(Setpoint < 0) + { + overturn = true; + } + + 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.SetControllerDirection(REVERSE); // PID mode: Reverse + + /* PID stops when approaching a litte more than desired angle */ + while(pid_yaw > Setpoint+3) + { + /* PID is set to correct angle range if angle less than 0˚ */ + if(overturn && curr_yaw > Setpoint+deg+1) + { + pid_yaw = curr_yaw - 360; + } + else + { + pid_yaw = curr_yaw; + } + + myPID.Compute(); // Does PID calculations + double tempor = Output+def; // Temporary value with the voltage output + y->write(tempor); // Update y sent to chair + + /* Prints to serial monitor the current angle and setpoint */ + out->printf("curr_yaw %f\r\n", curr_yaw); + out->printf("Setpoint = %f \r\n", Setpoint); + + wait(.05); // Small delay (milliseconds) + } + + /* Saftey stop for wheelchair */ + Wheelchair::stop(); + out->printf("done \r\n"); + +} + +/* This constructor determines whether to turn left or right */ +void Wheelchair::pid_turn(int deg) +{ + + /* Sets angle to coterminal angle for left turn if deg > 180 + * Sets angle to coterminal angle for right turn if deg < -180 + */ + if(deg > 180) + { + deg -= 360; + } + else if(deg < -180) + { + deg +=360; + } + + /* Makes sure angle inputted to function is positive */ + int turnAmt = abs(deg); + + /* Calls PID_right if deg > 0, else calls PID_left if deg < 0 */ + if(deg >= 0) + { + Wheelchair::pid_right(turnAmt); + } + else + { + Wheelchair::pid_left(turnAmt); + } + +} + +/* This constructor takes in distance to travel and adjust to move forward */ +void Wheelchair::pid_forward(double mm) +{ + mm -= 20; // Makes sure distance does not overshoot + Input = 0; // Initializes input to zero: Test latter w/o + wheel->reset(); // Resets encoders so that they start at 0 + + out->printf("pid foward\r\n"); + + double tempor; // Initializes Temporary variable for x input + Setpoint = mm; // Initializes the setpoint to desired value + + myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D + myPIDDistance.SetOutputLimits(0,high-def-.15); // Limit set to difference between high and def + myPIDDistance.SetControllerDirection(DIRECT); // PID mode: Direct + + y->write(def+offset); // Update y to make chair stationary + + /* Chair stops moving when Setpoint is reached */ + while(Input < Setpoint){ + + if(out->readable()) // Emergency Break + { + 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); + + PIDAngularV.SetTunings(.00015,0, 0.00); // Sets the constants for P and D + PIDAngularV.SetOutputLimits(-.1, .1); // Limit set to be in range specified + PIDAngularV.SetControllerDirection(DIRECT); // PID mode: Direct + + /* Computes angular position of wheelchair while turning */ + while(1) + { + yDesired = angularV; + + /* Update and set all variable so that the chair is stationary + * if the desired angle is zero + */ + if(yDesired == 0) + { + x->write(def); + y->write(def); + yDesired = 0; + return; + } + + /* Continuously updates with current angle measured by IMU */ + yIn = imu->gyro_z(); + PIDAngularV.Compute(); + temporA += yOut; // Temporary value with the voltage output + y->write(temporA); // Update y sent to chair + + //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z()); + wait(.05); // Small delay (milliseconds) + } + +} + +/* This constructor computes the relative velocity for Twist message in ROS */ +void Wheelchair::pid_twistV() +{ + /* Initializes variables as default */ + double temporV = def; + double temporS = def+offset; + vDesiredS = 0; + x->write(def); + y->write(def); + wheel->reset(); // Resets the encoders + /* Sets the constants for P and D */ + PIDVelosity.SetTunings(.0005,0, 0.00); + PIDSlaveV.SetTunings(.005,0.000001, 0.000001); + + /* Limits to the range specified */ + PIDVelosity.SetOutputLimits(-.005, .005); + PIDSlaveV.SetOutputLimits(-.002, .002); + + /* PID mode: Direct */ + PIDVelosity.SetControllerDirection(DIRECT); + PIDSlaveV.SetControllerDirection(DIRECT); + + while(1) + { + linearV = .7; + test1 = linearV*100; + vel = curr_vel; + vDesired = linearV*100; + if(out->readable()) + return; + /* Update and set all variable so that the chair is stationary + * if the velocity is zero + */ + if(linearV == 0) + { + x->write(def); + y->write(def); + + vel = 0; + vDesired = 0; + dist_old = 0; + return; + } + + if(vDesired >= 0) + { + PIDVelosity.SetTunings(.000004,0, 0.00); // Sets the constants for P and D + PIDVelosity.SetOutputLimits(-.002, .002); // Limits to the range specified + } + else + { + PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D + PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to range specified + } + + /* Sets maximum value of variable to 1 */ + if(temporV >= 1.5) + { + temporV = 1.5; + } + /* Scales and makes some adjustments to velocity */ + vIn = curr_vel*100; + vInS = curr_vel-curr_velS; + PIDVelosity.Compute(); + PIDSlaveV.Compute(); + if(forwardSafety == 0) + { + temporV += vOut; + temporS += vOutS; + + /* Updates x,y sent to Wheelchair and for Odometry message in ROS */ + x->write(temporV); + test2 = temporV; + y->write(temporS); + } + else + { + x->write(def); + y->write(def); + } + //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS); + Wheelchair::odomMsg(); + 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 + */ +void Wheelchair::odomMsg() +{ + double dist_new = curr_pos; + double dist = dist_new-dist_old; + double temp_x = dist*sin(z_angular*3.14159/180); + double temp_y = dist*cos(z_angular*3.14159/180); + + x_position += temp_x; + y_position += temp_y; + + 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 */ +void Wheelchair::resetDistance() +{ + wheel->reset(); +} + + +/*Predetermined paths For Demmo*/ +void Wheelchair::desk() +{ + Wheelchair::pid_forward(5461); + Wheelchair::pid_right(87); + Wheelchair::pid_forward(3658); + Wheelchair::pid_right(87); + Wheelchair::pid_forward(3658); +} + +void Wheelchair::kitchen() +{ + Wheelchair::pid_forward(5461); + Wheelchair::pid_right(87); + Wheelchair::pid_forward(3658); + Wheelchair::pid_left(90); + Wheelchair::pid_forward(305); +} + +void Wheelchair::desk_to_kitchen() +{ + Wheelchair::pid_right(180); + Wheelchair::pid_forward(3700); +} \ No newline at end of file