Where we will test the side ToF sensors
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
wheelchairControlSideTof/wheelchair.cpp
- Committer:
- isagmz
- Date:
- 2019-07-02
- Revision:
- 20:3c1b58654e67
- Parent:
- 18:a10277a63b53
- Child:
- 21:d1faccb96146
File content as of revision 20:3c1b58654e67:
#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) { leftSafety = 1; out-> printf("Detecting wall to the left!\n"); } else{ leftSafety = 0; } if(sensor6 <= MIN_WALL_LENGTH) { 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 /*-------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(rightSafety == 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(leftSafety == 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); }