Affordable Smart Wheelchair
Updated with Statistics Library
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Embed:
(wiki syntax)
Show/hide line numbers
wheelchair.cpp
00001 00002 #include "wheelchair.h" 00003 00004 bool manual_drive = false; // Variable changes between joystick and auto drive 00005 double encoder_distance; // Keeps distanse due to original position 00006 00007 volatile double Setpoint, Output, Input, Input2; // Variables for PID 00008 volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; // Variables for PID 00009 volatile double vIn, vOut, vDesired; // Variables for PID Velosity 00010 volatile double vInS, vOutS, vDesiredS; // Variables for PID Slave Wheel 00011 volatile double yIn, yOut, yDesired; // Variables for PID turn velosity 00012 00013 double dist_old, curr_pos; // Variables for odometry position 00014 00015 00016 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor 00017 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor 00018 PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Velosity PID Constructor 00019 PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT); // Slave Velosity PID Constructor 00020 PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Angular Velosity PID Constructor 00021 00022 00023 /* Thread measures current angular position */ 00024 void Wheelchair::compass_thread() 00025 { 00026 curr_yaw = imu->yaw(); 00027 z_angular = curr_yaw; 00028 } 00029 00030 /* Thread measures velocity of wheels and distance traveled */ 00031 void Wheelchair::velocity_thread() 00032 { 00033 curr_vel = wheel->getVelocity(); 00034 curr_velS = wheelS->getVelocity(); 00035 curr_pos = wheel->getDistance(53.975); 00036 } 00037 00038 void Wheelchair::emergencyButton_thread () 00039 { 00040 while(1) { 00041 while(!e_button) { 00042 00043 //Stop wheelchair 00044 Wheelchair::stop(); 00045 printf("E-button has been pressed\n\n\n"); 00046 off->write(high); //turn off PCB 00047 on->write(0); //make sure PCB not on 00048 //Reset Board 00049 NVIC_SystemReset(); 00050 00051 } 00052 00053 } 00054 } 00055 00056 void Wheelchair::assistSafe_thread() 00057 { 00058 int ToFV[12]; 00059 for(int i = 0; i < 6; i++) // reads from the ToF Sensors 00060 { 00061 ToFV[i] = (*(ToF+i))->readFromOneSensor(); 00062 //out->printf("%d ", ToFV[i]); 00063 } 00064 00065 //out->printf("\r\n"); 00066 00067 00068 for(int i = 0; i < 4; i++) { // Reads from the ToF Sensors 00069 runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); 00070 } 00071 00072 int sensor1 = ToFV[0]; 00073 int sensor4 = ToFV[3]; 00074 out->printf("%d, %d\r\n", ToFV[1], runningAverage[0]); 00075 if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || 00076 2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && 00077 (sensor1 < 1500 || sensor4 < 1500)) || 00078 550 > sensor1 || 550 > sensor4) 00079 { 00080 //out->printf("i am in danger\r\n"); 00081 if(x->read() > def) 00082 { 00083 x->write(def); 00084 forwardSafety = 1; 00085 } 00086 } 00087 else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || 00088 2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && 00089 (sensor1 < 1500 || sensor4 < 1500)) || 00090 550 > sensor1 || 550 > sensor4) 00091 { 00092 //out->printf("i am in danger\r\n"); 00093 if(x->read() > def) 00094 { 00095 x->write(def); 00096 forwardSafety = 1; 00097 } 00098 } 00099 else 00100 forwardSafety = 0; 00101 00102 } 00103 00104 /* Constructor for Wheelchair class */ 00105 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, 00106 VL53L1X** ToFT) 00107 { 00108 x_position = 0; 00109 y_position = 0; 00110 forwardSafety = 0; 00111 /* Initializes X and Y variables to Pins */ 00112 x = new PwmOut(xPin); 00113 y = new PwmOut(yPin); 00114 /* Initializes IMU Library */ 00115 out = pc; // "out" is called for serial monitor 00116 out->printf("on\r\n"); 00117 imu = new chair_BNO055(pc, time); 00118 Wheelchair::stop(); // Wheelchair is initially stationary 00119 imu->setup(); // turns on the IMU 00120 wheelS = qeiS; // "wheel" is called for encoder 00121 wheel = qei; 00122 ToF = ToFT; // passes pointer with addresses of ToF sensors 00123 00124 for(int i = 0; i < 12; i++) // initializes the ToF Sensors 00125 { 00126 (*(ToF+i))->initReading(0x31+((0x02)*i), 50000); 00127 } 00128 00129 out->printf("wheelchair setup done \r\n"); // Make sure it initialized; prints in serial monitor 00130 ti = time; 00131 for(int i = 0; i < 100; i++) 00132 { 00133 ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor(); 00134 ledgeArrayRF[i] = (*(ToF+1))->readFromOneSensor(); 00135 } 00136 int* aaa = ledgeArrayLF; 00137 00138 statistics LFTStats(aaa, 99, 1); 00139 out->printf("Statistics: Mean = %f, Standard Dev = %f\n", LFTStats.mean(), LFTStats.stdev()); 00140 myPID.SetMode(AUTOMATIC); // PID mode: Automatic 00141 } 00142 00143 /* Move wheelchair with joystick on manual mode */ 00144 void Wheelchair::move(float x_coor, float y_coor) 00145 { 00146 /* Scales one joystick measurement to the chair's joystick measurement */ 00147 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; 00148 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; 00149 00150 /* Sends the scaled joystic values to the chair */ 00151 x->write(scaled_x); 00152 y->write(scaled_y); 00153 } 00154 00155 /* Automatic mode: move forward and update x,y coordinate sent to chair */ 00156 void Wheelchair::forward() 00157 { 00158 //printf("current velosity; %f, curr vel S %f\r\n", curr_vel, curr_velS); 00159 if(forwardSafety == 0) 00160 { 00161 x->write(high); 00162 y->write(def+offset); 00163 } 00164 out->printf("%f, %f\r\n", curr_pos, wheelS->getDistance(53.975)); 00165 } 00166 00167 /* Automatic mode: move in reverse and update x,y coordinate sent to chair */ 00168 void Wheelchair::backward() 00169 { 00170 x->write(low); 00171 y->write(def); 00172 } 00173 00174 /* Automatic mode: move right and update x,y coordinate sent to chair */ 00175 void Wheelchair::right() 00176 { 00177 x->write(def); 00178 y->write(low); 00179 } 00180 00181 /* Automatic mode: move left and update x,y coordinate sent to chair */ 00182 void Wheelchair::left() 00183 { 00184 x->write(def); 00185 y->write(high); 00186 } 00187 00188 /* Stop the wheelchair */ 00189 void Wheelchair::stop() 00190 { 00191 x->write(def); 00192 y->write(def); 00193 } 00194 00195 /* Counter-clockwise is - 00196 * Clockwise is + 00197 * Range of deg: 0 to 360 00198 * This constructor takes in an angle from user and adjusts for turning right 00199 */ 00200 void Wheelchair::pid_right(int deg) 00201 { 00202 bool overturn = false; // Boolean if angle over 360˚ 00203 00204 out->printf("pid right\r\r\n"); 00205 x->write(def); // Update x sent to chair to be stationary 00206 Setpoint = curr_yaw + deg; // Relative angle we want to turn 00207 pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user 00208 00209 /* Turns on overturn boolean if setpoint over 360˚ */ 00210 if(Setpoint > 360) 00211 { 00212 overturn = true; 00213 } 00214 00215 myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D 00216 myPID.SetOutputLimits(0, def-low-.15); // Limit is set to the differnce between def and low 00217 myPID.SetControllerDirection(DIRECT); // PID mode: Direct 00218 00219 /* PID stops when approaching a litte less than desired angle */ 00220 while(pid_yaw < Setpoint - 3) 00221 { 00222 /* PID is set to correct angle range if angle greater than 360˚*/ 00223 if(overturn && curr_yaw < Setpoint-deg-1) 00224 { 00225 pid_yaw = curr_yaw + 360; 00226 } 00227 else 00228 { 00229 pid_yaw = curr_yaw; 00230 } 00231 00232 myPID.Compute(); // Does PID calculations 00233 double tempor = -Output+def; // Temporary value with the voltage output 00234 y->write(tempor); // Update y sent to chair 00235 00236 /* Prints to serial monitor the current angle and setpoint */ 00237 out->printf("curr_yaw %f\r\r\n", curr_yaw); 00238 out->printf("Setpoint = %f \r\n", Setpoint); 00239 00240 wait(.05); // Small delay (milliseconds) 00241 } 00242 00243 /* Saftey stop for wheelchair */ 00244 Wheelchair::stop(); 00245 out->printf("done \r\n"); 00246 } 00247 00248 /* Counter-clockwise is - 00249 * Clockwise is + 00250 * Range of deg: 0 to 360 00251 * This constructor takes in an angle from user and adjusts for turning left 00252 */ 00253 void Wheelchair::pid_left(int deg) 00254 { 00255 bool overturn = false; //Boolean if angle under 0˚ 00256 00257 out->printf("pid Left\r\r\n"); 00258 x->write(def); // Update x sent to chair to be stationary 00259 Setpoint = curr_yaw - deg; // Relative angle we want to turn 00260 pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user 00261 00262 /* Turns on overturn boolean if setpoint less than 0˚ */ 00263 if(Setpoint < 0) 00264 { 00265 overturn = true; 00266 } 00267 00268 myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D 00269 myPID.SetOutputLimits(0,high-def-.12); //Limit is set to the differnce between def and low 00270 myPID.SetControllerDirection(REVERSE); // PID mode: Reverse 00271 00272 /* PID stops when approaching a litte more than desired angle */ 00273 while(pid_yaw > Setpoint+3) 00274 { 00275 /* PID is set to correct angle range if angle less than 0˚ */ 00276 if(overturn && curr_yaw > Setpoint+deg+1) 00277 { 00278 pid_yaw = curr_yaw - 360; 00279 } 00280 else 00281 { 00282 pid_yaw = curr_yaw; 00283 } 00284 00285 myPID.Compute(); // Does PID calculations 00286 double tempor = Output+def; // Temporary value with the voltage output 00287 y->write(tempor); // Update y sent to chair 00288 00289 /* Prints to serial monitor the current angle and setpoint */ 00290 out->printf("curr_yaw %f\r\n", curr_yaw); 00291 out->printf("Setpoint = %f \r\n", Setpoint); 00292 00293 wait(.05); // Small delay (milliseconds) 00294 } 00295 00296 /* Saftey stop for wheelchair */ 00297 Wheelchair::stop(); 00298 out->printf("done \r\n"); 00299 00300 } 00301 00302 /* This constructor determines whether to turn left or right */ 00303 void Wheelchair::pid_turn(int deg) 00304 { 00305 00306 /* Sets angle to coterminal angle for left turn if deg > 180 00307 * Sets angle to coterminal angle for right turn if deg < -180 00308 */ 00309 if(deg > 180) 00310 { 00311 deg -= 360; 00312 } 00313 else if(deg < -180) 00314 { 00315 deg +=360; 00316 } 00317 00318 /* Makes sure angle inputted to function is positive */ 00319 int turnAmt = abs(deg); 00320 00321 /* Calls PID_right if deg > 0, else calls PID_left if deg < 0 */ 00322 if(deg >= 0) 00323 { 00324 Wheelchair::pid_right(turnAmt); 00325 } 00326 else 00327 { 00328 Wheelchair::pid_left(turnAmt); 00329 } 00330 00331 } 00332 00333 /* This constructor takes in distance to travel and adjust to move forward */ 00334 void Wheelchair::pid_forward(double mm) 00335 { 00336 mm -= 20; // Makes sure distance does not overshoot 00337 Input = 0; // Initializes input to zero: Test latter w/o 00338 wheel->reset(); // Resets encoders so that they start at 0 00339 00340 out->printf("pid foward\r\n"); 00341 00342 double tempor; // Initializes Temporary variable for x input 00343 Setpoint = mm; // Initializes the setpoint to desired value 00344 00345 myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D 00346 myPIDDistance.SetOutputLimits(0,high-def-.15); // Limit set to difference between high and def 00347 myPIDDistance.SetControllerDirection(DIRECT); // PID mode: Direct 00348 00349 y->write(def+offset); // Update y to make chair stationary 00350 00351 /* Chair stops moving when Setpoint is reached */ 00352 while(Input < Setpoint){ 00353 00354 if(out->readable()) // Emergency Break 00355 { 00356 break; 00357 } 00358 00359 Input = wheel->getDistance(53.975); // Gets distance from Encoder into PID 00360 wait(.05); // Slight Delay: *****Test without 00361 myPIDDistance.Compute(); // Compute distance traveled by chair 00362 00363 tempor = Output + def; // Temporary output variable 00364 x->write(tempor); // Update x sent to chair 00365 00366 /* Prints to serial monitor the distance traveled by chair */ 00367 out->printf("distance %f\r\n", Input); 00368 } 00369 00370 } 00371 00372 /* This constructor returns the relative angular position of chair */ 00373 double Wheelchair::getTwistZ() 00374 { 00375 return imu->gyro_z(); 00376 } 00377 00378 /* This constructor computes the relative angle for Twist message in ROS */ 00379 void Wheelchair::pid_twistA() 00380 { 00381 /* Initialize variables for angle and update x,y sent to chair */ 00382 char c; 00383 double temporA = def; 00384 y->write(def); 00385 x->write(def); 00386 00387 PIDAngularV.SetTunings(.00015,0, 0.00); // Sets the constants for P and D 00388 PIDAngularV.SetOutputLimits(-.1, .1); // Limit set to be in range specified 00389 PIDAngularV.SetControllerDirection(DIRECT); // PID mode: Direct 00390 00391 /* Computes angular position of wheelchair while turning */ 00392 while(1) 00393 { 00394 yDesired = angularV; 00395 00396 /* Update and set all variable so that the chair is stationary 00397 * if the desired angle is zero 00398 */ 00399 if(yDesired == 0) 00400 { 00401 x->write(def); 00402 y->write(def); 00403 yDesired = 0; 00404 return; 00405 } 00406 00407 /* Continuously updates with current angle measured by IMU */ 00408 yIn = imu->gyro_z(); 00409 PIDAngularV.Compute(); 00410 temporA += yOut; // Temporary value with the voltage output 00411 y->write(temporA); // Update y sent to chair 00412 00413 //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z()); 00414 wait(.05); // Small delay (milliseconds) 00415 } 00416 00417 } 00418 00419 /* This constructor computes the relative velocity for Twist message in ROS */ 00420 void Wheelchair::pid_twistV() 00421 { 00422 /* Initializes variables as default */ 00423 double temporV = def; 00424 double temporS = def+offset; 00425 vDesiredS = 0; 00426 x->write(def); 00427 y->write(def); 00428 wheel->reset(); // Resets the encoders 00429 /* Sets the constants for P and D */ 00430 PIDVelosity.SetTunings(.0005,0, 0.00); 00431 PIDSlaveV.SetTunings(.005,0.000001, 0.000001); 00432 00433 /* Limits to the range specified */ 00434 PIDVelosity.SetOutputLimits(-.005, .005); 00435 PIDSlaveV.SetOutputLimits(-.002, .002); 00436 00437 /* PID mode: Direct */ 00438 PIDVelosity.SetControllerDirection(DIRECT); 00439 PIDSlaveV.SetControllerDirection(DIRECT); 00440 00441 while(1) 00442 { 00443 linearV = .7; 00444 test1 = linearV*100; 00445 vel = curr_vel; 00446 vDesired = linearV*100; 00447 if(out->readable()) 00448 return; 00449 /* Update and set all variable so that the chair is stationary 00450 * if the velocity is zero 00451 */ 00452 if(linearV == 0) 00453 { 00454 x->write(def); 00455 y->write(def); 00456 00457 vel = 0; 00458 vDesired = 0; 00459 dist_old = 0; 00460 return; 00461 } 00462 00463 if(vDesired >= 0) 00464 { 00465 PIDVelosity.SetTunings(.000004,0, 0.00); // Sets the constants for P and D 00466 PIDVelosity.SetOutputLimits(-.002, .002); // Limits to the range specified 00467 } 00468 else 00469 { 00470 PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D 00471 PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to range specified 00472 } 00473 00474 /* Sets maximum value of variable to 1 */ 00475 if(temporV >= 1.5) 00476 { 00477 temporV = 1.5; 00478 } 00479 /* Scales and makes some adjustments to velocity */ 00480 vIn = curr_vel*100; 00481 vInS = curr_vel-curr_velS; 00482 PIDVelosity.Compute(); 00483 PIDSlaveV.Compute(); 00484 if(forwardSafety == 0) 00485 { 00486 temporV += vOut; 00487 temporS += vOutS; 00488 00489 /* Updates x,y sent to Wheelchair and for Odometry message in ROS */ 00490 x->write(temporV); 00491 test2 = temporV; 00492 y->write(temporS); 00493 } 00494 else 00495 { 00496 x->write(def); 00497 y->write(def); 00498 } 00499 //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS); 00500 Wheelchair::odomMsg(); 00501 wait(.01); // Small delay (milliseconds) 00502 } 00503 } 00504 00505 /* This constructor calculates the relative position of the chair everytime the encoders reset 00506 * by setting its old position as the origin to calculate the new position 00507 */ 00508 void Wheelchair::odomMsg() 00509 { 00510 double dist_new = curr_pos; 00511 double dist = dist_new-dist_old; 00512 double temp_x = dist*sin(z_angular*3.14159/180); 00513 double temp_y = dist*cos(z_angular*3.14159/180); 00514 00515 x_position += temp_x; 00516 y_position += temp_y; 00517 00518 dist_old = dist_new; 00519 } 00520 00521 /* This constructor prints the Odometry message to the serial monitor */ 00522 void Wheelchair::showOdom() 00523 { 00524 out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular); 00525 } 00526 00527 /* This constructor returns the approximate distance based on the wheel diameter */ 00528 float Wheelchair::getDistance() 00529 { 00530 return wheel->getDistance(Diameter); 00531 } 00532 00533 /* This constructor resets the wheel encoder's */ 00534 void Wheelchair::resetDistance() 00535 { 00536 wheel->reset(); 00537 } 00538 00539 00540 /*Predetermined paths For Demmo*/ 00541 void Wheelchair::desk() 00542 { 00543 Wheelchair::pid_forward(5461); 00544 Wheelchair::pid_right(87); 00545 Wheelchair::pid_forward(3658); 00546 Wheelchair::pid_right(87); 00547 Wheelchair::pid_forward(3658); 00548 } 00549 00550 void Wheelchair::kitchen() 00551 { 00552 Wheelchair::pid_forward(5461); 00553 Wheelchair::pid_right(87); 00554 Wheelchair::pid_forward(3658); 00555 Wheelchair::pid_left(90); 00556 Wheelchair::pid_forward(305); 00557 } 00558 00559 void Wheelchair::desk_to_kitchen() 00560 { 00561 Wheelchair::pid_right(180); 00562 Wheelchair::pid_forward(3700); 00563 }
Generated on Wed Jul 27 2022 13:13:23 by
1.7.2