Jesus Fausto
/
wheelchaircontrol
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: PID QEI chair_BNO055 ros_lib_kinetic
Fork of
wheelchaircontrol
by 
ryan lin
Embed:
(wiki syntax)
Show/hide line numbers
wheelchair.cpp
00001 #include "wheelchair.h" 00002 00003 bool manual_drive = false; // Variable Changes between joystick and auto drive 00004 double curr_yaw, curr_vel, curr_velS; // Variable that contains current relative angle 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; 00010 volatile double vInS, vOutS, vDesiredS; 00011 volatile double yIn, yOut, yDesired; 00012 00013 double z_angular, dist_old, curr_pos; 00014 double x_position = 0; 00015 double y_position = 0; 00016 00017 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor 00018 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor 00019 PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT); 00020 PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT); 00021 PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT); 00022 00023 void Wheelchair::compass_thread() { // Thread that measures which angle we are at 00024 curr_yaw = imu->yaw(); 00025 z_angular = curr_yaw; 00026 } 00027 void Wheelchair::velosity_thread() { 00028 curr_vel = wheel->getVelosity(); 00029 curr_velS = wheelS->getVelosity(); 00030 curr_pos = wheel->getDistance(53.975); 00031 } 00032 00033 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS) // Function Constructor for Wheelchair class 00034 { 00035 //Initializes X and Y variables to Pins 00036 x = new PwmOut(xPin); 00037 y = new PwmOut(yPin); 00038 00039 // Initializes IMU Library 00040 imu = new chair_BNO055(pc, time); 00041 Wheelchair::stop(); // Wheelchair is not moving when initializing 00042 imu->setup(); // turns on the IMU 00043 out = pc; // "out" is called for serial monitor 00044 wheelS = qeiS; // "wheel" is called for encoder 00045 wheel = qei; 00046 out->printf("wheelchair setup done \r\n"); // make sure it initialized 00047 ti = time; 00048 myPID.SetMode(AUTOMATIC); // set PID to automatic 00049 } 00050 00051 void Wheelchair::move(float x_coor, float y_coor) // moves the chair with joystick on manual 00052 { 00053 00054 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; // Scales one joystic measurement to the 00055 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; // chair's joystic measurement 00056 00057 x->write(scaled_x); // Sends the scaled joystic values to the chair 00058 y->write(scaled_y); 00059 } 00060 00061 void Wheelchair::forward() // In auto to move foward 00062 { 00063 x->write(high); 00064 y->write(def+offset); 00065 } 00066 00067 void Wheelchair::backward() // In auto to move reverse 00068 { 00069 x->write(low); 00070 y->write(def); 00071 } 00072 00073 void Wheelchair::right() // In auto to move right 00074 { 00075 x->write(def); 00076 y->write(low); 00077 } 00078 00079 void Wheelchair::left() // In auto to move left 00080 { 00081 x->write(def); 00082 y->write(high); 00083 } 00084 00085 void Wheelchair::stop() // Stops the chair 00086 { 00087 x->write(def); 00088 y->write(def); 00089 } 00090 // counter clockwise is - 00091 // clockwise is + 00092 void Wheelchair::pid_right(int deg) // Takes in degree and turns right 00093 { 00094 bool overturn = false; //Boolean if we have to turn over relative 360˚ 00095 00096 out->printf("pid right\r\r\n"); 00097 x->write(def); // Not moving fowards or reverse 00098 Setpoint = curr_yaw + deg; // Relative angle we want to turn 00099 pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input) 00100 00101 if(Setpoint > 360) { //Turns on overturn boolean if setpoint over 360˚ 00102 overturn = true; 00103 } 00104 00105 myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D 00106 myPID.SetOutputLimits(0, def-low-.15); // Limits to the differnce between def and low 00107 myPID.SetControllerDirection(DIRECT); // PID mode Direct 00108 00109 while(pid_yaw < Setpoint - 3){ // Tells PID to stop when reaching 00110 // a little less than desired angle 00111 if(overturn && curr_yaw < Setpoint-deg-1) // Sets PID yaw to coterminal angle if necesary 00112 { 00113 pid_yaw = curr_yaw + 360; 00114 } 00115 else 00116 pid_yaw = curr_yaw; 00117 00118 myPID.Compute(); // Does PID calculations 00119 double tempor = -Output+def; // Temporary value with the voltage output 00120 y->write(tempor); // Sends to chair y output command 00121 00122 out->printf("curr_yaw %f\r\r\n", curr_yaw); 00123 out->printf("Setpoint = %f \r\n", Setpoint); 00124 00125 wait(.05); // Small delay 00126 } 00127 Wheelchair::stop(); // Safety Stop 00128 out->printf("done \r\n"); 00129 } 00130 00131 void Wheelchair::pid_left(int deg) // Takes in degree and turns left 00132 { 00133 bool overturn = false; //Boolean if we have to turn under relative 0˚ 00134 00135 out->printf("pid Left\r\r\n"); 00136 x->write(def); // Not moving fowards or reverse 00137 Setpoint = curr_yaw - deg; // Relative angle we want to turn 00138 pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input) 00139 if(Setpoint < 0) { //Turns on overturn boolean if setpoint under 0˚ 00140 overturn = true; 00141 } 00142 myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D 00143 myPID.SetOutputLimits(0,high-def-.12); // Limits to the differnce between High and Def 00144 myPID.SetControllerDirection(REVERSE); // PID mode Reverse 00145 while(pid_yaw > Setpoint+3){ // Tells PID to stop when reaching 00146 // a little more than desired angle 00147 if(overturn && curr_yaw > Setpoint+deg+1) // Sets PID yaw to coterminal angle if necesary 00148 { 00149 pid_yaw = curr_yaw - 360; 00150 } 00151 else 00152 pid_yaw = curr_yaw; 00153 00154 myPID.Compute(); // Does PID calculations 00155 double tempor = Output+def; // Temporary value with the voltage output 00156 y->write(tempor); // Sends to chair y output command 00157 00158 out->printf("curr_yaw %f\r\n", curr_yaw); 00159 wait(.05); // Small Delay 00160 } 00161 Wheelchair::stop(); // Safety Stop 00162 } 00163 00164 void Wheelchair::pid_turn(int deg) { // Determine wether we are turn right or left 00165 00166 if(deg > 180) { // If deg > 180 turn left: coterminal angle 00167 deg -= 360; 00168 } 00169 00170 else if(deg < -180) { // If deg < -180 turn right: coterminal angle 00171 deg+=360; 00172 } 00173 00174 int turnAmt = abs(deg); // Makes sure input angle is positive 00175 00176 if(deg >= 0){ 00177 Wheelchair::pid_right(turnAmt); // Calls PID right if positive degree 00178 } 00179 else { 00180 Wheelchair::pid_left(turnAmt); // Calls PID left if negative degree 00181 } 00182 } 00183 void Wheelchair::pid_forward(double mm) 00184 { 00185 mm -= 20; // Makes sure distance does not overshoot 00186 Input = 0; // Initializes imput to cero: Test latter w/o 00187 wheel->reset(); // Resets encoders so that they start at 0 00188 out->printf("pid foward\r\n"); 00189 00190 double tempor; // Initializes Temporary variable for x input 00191 Setpoint = mm; // Initializes the setpoint to desired value 00192 00193 myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D 00194 myPIDDistance.SetOutputLimits(0,high-def-.15); // Limits to the differnce between High and Def 00195 myPIDDistance.SetControllerDirection(DIRECT); // PID to Direct 00196 y->write(def+offset); // Sets chair to not turn 00197 00198 while(Input < Setpoint){ // Stop moving when reaching setpoint 00199 00200 if(out->readable()) // Emergency Break 00201 break; 00202 00203 Input = wheel->getDistance(53.975); // Gets Distance from Encoder onto PID 00204 wait(.05); // Slight Delay: *****Test without 00205 myPIDDistance.Compute(); // Compute Output for chair 00206 00207 tempor = Output + def; // Temporary output variable 00208 x->write(tempor); // Sends to chair x output 00209 out->printf("distance %f\r\n", Input); 00210 } 00211 00212 } 00213 void Wheelchair::pid_reverse(double mm) 00214 { 00215 00216 } 00217 void Wheelchair::pid_twistA() 00218 { 00219 char c; 00220 double temporA = def; 00221 y->write(def); 00222 x->write(def); 00223 00224 PIDAngularV.SetTunings(.00015,0, 0.00); // Sets the constants for P and D 00225 PIDAngularV.SetOutputLimits(-.1, .1); // Limits to the differnce between def and low 00226 PIDAngularV.SetControllerDirection(DIRECT); // PID mode Direct 00227 while(1) 00228 { 00229 if(out->readable()) 00230 c = out->getc(); 00231 if(c == '0') 00232 { 00233 yDesired = 0; 00234 } 00235 if(c == '1') 00236 yDesired = 30; 00237 if(c == '2') 00238 yDesired = 90; 00239 if(c == '9') 00240 yDesired = -30; 00241 if(c == '8') 00242 yDesired = -90; 00243 if(c == 'r') 00244 { 00245 yDesired = 0; 00246 return; 00247 } 00248 00249 yIn = imu->gyro_z(); 00250 PIDAngularV.Compute(); 00251 temporA += yOut; // Temporary value with the voltage output 00252 y->write(temporA); 00253 //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z()); 00254 wait(.05); 00255 } 00256 } 00257 void Wheelchair::pid_twistV() 00258 { 00259 double temporV = def; 00260 double temporS = def; 00261 vDesiredS = 0; 00262 char c; 00263 x->write(def); 00264 y->write(def); 00265 wheel->reset(); 00266 PIDVelosity.SetTunings(.00005,0, 0.00); // Sets the constants for P and D 00267 PIDSlaveV.SetTunings(.007,0.000001, 0.000001); // Sets the constants for P and D 00268 PIDVelosity.SetOutputLimits(-.005, .005); // Limits to the differnce between def and low 00269 PIDSlaveV.SetOutputLimits(-.002, .002); // Limits to the differnce between def and low 00270 PIDVelosity.SetControllerDirection(DIRECT); 00271 PIDSlaveV.SetControllerDirection(DIRECT); 00272 while(1) 00273 { 00274 if(out->readable()) 00275 c = out->getc(); 00276 if(c == '0') 00277 vDesired = 0; 00278 if(c == '3') 00279 vDesired = 100; 00280 if(c == '4') 00281 vDesired = 150; 00282 if(c == '7') 00283 vDesired = -50; 00284 if(c == '6') 00285 vDesired = -100; 00286 if(c == 'r') 00287 { 00288 yDesired = 0; 00289 dist_old = 0; 00290 return; 00291 } 00292 if(vDesired >= 0) 00293 { 00294 PIDVelosity.SetTunings(.000004,0, 0.00); // Sets the constants for P and D 00295 PIDVelosity.SetOutputLimits(-.002, .002); // Limits to the differnce between def and low 00296 } 00297 else 00298 { 00299 PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D 00300 PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to the differnce between def and low 00301 } 00302 if(temporV >= 1) 00303 temporV = 1; 00304 vIn = curr_vel*100; 00305 vInS = curr_vel-curr_velS; 00306 PIDVelosity.Compute(); 00307 PIDSlaveV.Compute(); 00308 temporV += vOut; 00309 temporS += vOutS; 00310 x->write(temporV); 00311 y->write(temporS); 00312 //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS); 00313 Wheelchair::odomMsg(); 00314 wait(.01); 00315 } 00316 } 00317 void Wheelchair::odomMsg(){ 00318 double dist_new = curr_pos; 00319 double dist = dist_new-dist_old; 00320 double temp_x = dist*sin(z_angular*3.14159/180); 00321 double temp_y = dist*cos(z_angular*3.14159/180); 00322 00323 x_position += temp_x; 00324 y_position += temp_y; 00325 00326 dist_old = dist_new; 00327 } 00328 void Wheelchair::showOdom(){ 00329 out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular); 00330 } 00331 float Wheelchair::getDistance() { 00332 return wheel->getDistance(Diameter); 00333 } 00334 00335 void Wheelchair::resetDistance(){ 00336 wheel->reset(); 00337 } 00338 /*Predetermined paths For Demmo*/ 00339 void Wheelchair::desk() { 00340 Wheelchair::pid_forward(5461); 00341 Wheelchair::pid_right(87); 00342 Wheelchair::pid_forward(3658); 00343 Wheelchair::pid_right(87); 00344 Wheelchair::pid_forward(3658); 00345 } 00346 00347 void Wheelchair::kitchen() { 00348 Wheelchair::pid_forward(5461); 00349 Wheelchair::pid_right(87); 00350 Wheelchair::pid_forward(3658); 00351 Wheelchair::pid_left(90); 00352 Wheelchair::pid_forward(305); 00353 } 00354 00355 void Wheelchair::desk_to_kitchen(){ 00356 Wheelchair::pid_right(180); 00357 Wheelchair::pid_forward(3700); 00358 } 00359 00360
Generated on Sat Jul 16 2022 19:47:22 by
1.7.2