Working file for communication with ROS
Dependencies: QEI2 PID VL53L1X_Filter
wheelchair.cpp
- Committer:
- JesiMiranda
- Date:
- 2019-08-20
- Revision:
- 31:9283c8cd5362
- Parent:
- 29:5714715ab1f5
File content as of revision 31:9283c8cd5362:
/************************************************************************* * Importing header into wheelchair.cpp * **************************************************************************/ #include "wheelchair.h" /************************************************************************* * Defining global variables * **************************************************************************/ 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 Velocity volatile double vInS, vOutS, vDesiredS; // Variables for PID Slave Wheel volatile double yIn, yOut, yDesired; // Variables for PID turn velosity double dist_old, curr_pos, curr_posS; // Variables for odometry position /************************************************************************* * Creating PID objects * **************************************************************************/ 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 //e_button thread missing /************************************************************************* * Thread measures current angular position * **************************************************************************/ void Wheelchair::compass_thread() { //curr_yaw = imu->yaw(); //uncomment when imu is implemented 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); curr_posS = wheelS->getDistance(53.975); //Sending slave encoder to ROS } /************************************************************************* * ------------------------------------------------------------- * **************************************************************************/ void Wheelchair::assistSafe_thread() { //int ToFV[12]; for(int i = 0; i < 12; i++) { // reads from the ToF Sensors ToFV[i] = (*(ToF+i))->readFromOneSensor(); //out->printf("%d ", ToFV[i]); } //out->printf("\r\n"); /* int sensor1 = ToFV[1]; int sensor4 = ToFV[5]; //out->printf("%d, %d\r\n", sensor1, sensor4); 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; } } 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 //removed the/* forwardSafety = 0; */ } /************************************************************************* * 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 */ imu = new chair_BNO055(pc, time); Wheelchair::stop(); // Wheelchair is initially stationary imu->setup(); // turns on the IMU out = pc; // "out" is called for serial monitor 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; 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() { if(forwardSafety == 0) { x->write(high); y->write(def+offset); } } /************************************************************************* * 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() { 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); } /************************************************************************* * Stop the wheelchair * **************************************************************************/ void Wheelchair::stop() { x->write(def); y->write(def); } /************************************************************************* * Counter-clockwise is ( - ) * * Clockwise is ( + ) * * Range of deg: 0 to 360 * * This method 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 method 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 method 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 method 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 method returns the relative angular position of chair * **************************************************************************/ double Wheelchair::getTwistZ() { return imu->gyro_z(); } /************************************************************************* * This method 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 method 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) { //commandRead.linear.x = .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 method 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() { dist_new = curr_pos; dist_newS = curr_posS; 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 method 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 method returns the approximate distance based on the wheel diameter* ***************************************************************************/ float Wheelchair::getDistance() { return wheel->getDistance(Diameter); } /************************************************************************** * This method resets the 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); }