ryan lin / wheelchaircontrolRos

for ros

Dependencies:   QEI chair_BNO055 pid ros_lib_kinetic

Dependents:   wheelchaircontrolrealtimeROS

Fork of wheelchaircontrol by ryan lin

wheelchair.cpp@21:69df88af7c46, 2018-08-31 (annotated)

Committer:
ryanlin97
Date:
Fri Aug 31 19:41:42 2018 +0000
Revision:
21:69df88af7c46
Parent:
19:df4257c75ed0 
for jesus

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 19:df4257c75ed0 2 Serial qei(D1, D0);
ryanlin97 19:df4257c75ed0 3
ryanlin97 19:df4257c75ed0 4 DigitalOut on(D12);
ryanlin97 19:df4257c75ed0 5 DigitalOut off(D11);
ryanlin97 19:df4257c75ed0 6 DigitalOut up(D2);
ryanlin97 19:df4257c75ed0 7 DigitalOut down(D3);
ryanlin97 11:d14a1f7f1297 8
ryanlin97 10:e5463c11e0a0 9 bool manual_drive = false;
ryanlin97 19:df4257c75ed0 10 double encoder_distance;
ryanlin97 19:df4257c75ed0 11
ryanlin97 12:921488918749 12 double curr_yaw;
ryanlin97 19:df4257c75ed0 13 double Setpoint, Output, Input;
ryanlin97 17:93a636b16b9e 14 PID myPID(&curr_yaw, &Output, &Setpoint, 5.5, 0, .0036, P_ON_E, DIRECT);
ryanlin97 19:df4257c75ed0 15 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);
ryanlin97 1:c0beadca1617 16
ryanlin97 11:d14a1f7f1297 17 void Wheelchair::compass_thread() {
ryanlin97 11:d14a1f7f1297 18 curr_yaw = imu->yaw();
ryanlin97 11:d14a1f7f1297 19 }
ryanlin97 11:d14a1f7f1297 20
ryanlin97 19:df4257c75ed0 21 double Wheelchair::readEncoder() {
ryanlin97 19:df4257c75ed0 22 char input[64] = {0};
ryanlin97 19:df4257c75ed0 23 for (int i=0; input[i-1] != '\n'; i++) {
ryanlin97 19:df4257c75ed0 24 while (true) {
ryanlin97 19:df4257c75ed0 25 //pc.printf("%f\r\n", ti.read());
ryanlin97 19:df4257c75ed0 26 if (ti->read() > .02) break;
ryanlin97 19:df4257c75ed0 27 if (qei.readable()) {
ryanlin97 19:df4257c75ed0 28 input[i]= qei.getc();
ryanlin97 19:df4257c75ed0 29 break;
ryanlin97 19:df4257c75ed0 30 }
ryanlin97 19:df4257c75ed0 31 }
ryanlin97 19:df4257c75ed0 32 }
ryanlin97 19:df4257c75ed0 33 return atoi(input);
ryanlin97 19:df4257c75ed0 34 }
ryanlin97 19:df4257c75ed0 35
ryanlin97 21:69df88af7c46 36 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei )
ryanlin97 1:c0beadca1617 37 {
ryanlin97 3:a5e71bfdb492 38 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 39 y = new PwmOut(yPin);
ryanlin97 11:d14a1f7f1297 40 imu = new chair_BNO055(pc, time);
ryanlin97 21:69df88af7c46 41
ryanlin97 11:d14a1f7f1297 42 Wheelchair::stop();
ryanlin97 7:5e38d43fbce3 43 imu->setup();
ryanlin97 7:5e38d43fbce3 44 out = pc;
ryanlin97 7:5e38d43fbce3 45 out->printf("wheelchair setup done \n");
ryanlin97 11:d14a1f7f1297 46 ti = time;
ryanlin97 21:69df88af7c46 47 wheel = qei;
ryanlin97 12:921488918749 48 myPID.SetMode(AUTOMATIC);
ryanlin97 1:c0beadca1617 49 }
ryanlin97 6:0cd57bdd8fbc 50
ryanlin97 3:a5e71bfdb492 51 /*
ryanlin97 3:a5e71bfdb492 52 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
ryanlin97 3:a5e71bfdb492 53 */
ryanlin97 3:a5e71bfdb492 54 void Wheelchair::move(float x_coor, float y_coor)
ryanlin97 1:c0beadca1617 55 {
ryanlin97 6:0cd57bdd8fbc 56
ryanlin97 4:29a27953fe70 57 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
ryanlin97 4:29a27953fe70 58 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
ryanlin97 11:d14a1f7f1297 59
ryanlin97 11:d14a1f7f1297 60 // lowPass(scaled_x);
ryanlin97 11:d14a1f7f1297 61 //lowPass(scaled_y);
ryanlin97 11:d14a1f7f1297 62
ryanlin97 4:29a27953fe70 63 x->write(scaled_x);
ryanlin97 4:29a27953fe70 64 y->write(scaled_y);
ryanlin97 11:d14a1f7f1297 65
ryanlin97 11:d14a1f7f1297 66 //out->printf("yaw %f\n", imu->yaw());
ryanlin97 1:c0beadca1617 67
ryanlin97 5:e0ccaab3959a 68 }
ryanlin97 11:d14a1f7f1297 69
ryanlin97 1:c0beadca1617 70 void Wheelchair::forward()
ryanlin97 1:c0beadca1617 71 {
ryanlin97 0:fc0c4a184482 72 x->write(high);
ryanlin97 3:a5e71bfdb492 73 y->write(def+offset);
ryanlin97 0:fc0c4a184482 74 }
ryanlin97 0:fc0c4a184482 75
ryanlin97 1:c0beadca1617 76 void Wheelchair::backward()
ryanlin97 1:c0beadca1617 77 {
ryanlin97 0:fc0c4a184482 78 x->write(low);
ryanlin97 0:fc0c4a184482 79 y->write(def);
ryanlin97 0:fc0c4a184482 80 }
ryanlin97 0:fc0c4a184482 81
ryanlin97 1:c0beadca1617 82 void Wheelchair::right()
ryanlin97 1:c0beadca1617 83 {
ryanlin97 0:fc0c4a184482 84 x->write(def);
ryanlin97 11:d14a1f7f1297 85 y->write(low);
ryanlin97 0:fc0c4a184482 86 }
ryanlin97 0:fc0c4a184482 87
ryanlin97 1:c0beadca1617 88 void Wheelchair::left()
ryanlin97 1:c0beadca1617 89 {
ryanlin97 0:fc0c4a184482 90 x->write(def);
ryanlin97 11:d14a1f7f1297 91 y->write(high);
ryanlin97 0:fc0c4a184482 92 }
ryanlin97 0:fc0c4a184482 93
ryanlin97 1:c0beadca1617 94 void Wheelchair::stop()
ryanlin97 1:c0beadca1617 95 {
ryanlin97 0:fc0c4a184482 96 x->write(def);
ryanlin97 0:fc0c4a184482 97 y->write(def);
ryanlin97 6:0cd57bdd8fbc 98 }
ryanlin97 11:d14a1f7f1297 99 // counter clockwise is -
ryanlin97 11:d14a1f7f1297 100 // clockwise is +
ryanlin97 12:921488918749 101 void Wheelchair::pid_right(int deg)
ryanlin97 12:921488918749 102 {
ryanlin97 17:93a636b16b9e 103 float pid_yaw;
ryanlin97 12:921488918749 104 bool overturn = false;
ryanlin97 12:921488918749 105
ryanlin97 17:93a636b16b9e 106 out->printf("pid right\r\r\n");
ryanlin97 12:921488918749 107 x->write(def);
ryanlin97 12:921488918749 108 Setpoint = curr_yaw + deg;
ryanlin97 17:93a636b16b9e 109 pid_yaw = curr_yaw;
ryanlin97 12:921488918749 110 if(Setpoint > 360) {
ryanlin97 12:921488918749 111 overturn = true;
ryanlin97 12:921488918749 112 }
ryanlin97 17:93a636b16b9e 113 myPID.SetTunings(5.5,0, 0.00345);
ryanlin97 21:69df88af7c46 114 myPID.SetOutputLimits(0, def - low - .15);
ryanlin97 21:69df88af7c46 115 myPID.SetControllerDirection(DIRECT);
ryanlin97 18:3eadf01ec1b0 116
ryanlin97 18:3eadf01ec1b0 117 while(pid_yaw < Setpoint - 3){
ryanlin97 17:93a636b16b9e 118 if(overturn && curr_yaw < Setpoint-deg-1)
ryanlin97 17:93a636b16b9e 119 {
ryanlin97 17:93a636b16b9e 120 pid_yaw = curr_yaw + 360;
ryanlin97 17:93a636b16b9e 121 }
ryanlin97 17:93a636b16b9e 122 else
ryanlin97 17:93a636b16b9e 123 pid_yaw = curr_yaw;
ryanlin97 16:53d912c86c7f 124 myPID.Compute();
ryanlin97 21:69df88af7c46 125 double tempor = -Output+def;
ryanlin97 17:93a636b16b9e 126 y->write(tempor);
ryanlin97 17:93a636b16b9e 127 out->printf("curr_yaw %f\r\r\n", curr_yaw);
ryanlin97 17:93a636b16b9e 128 out->printf("Setpoint = %f \r\n", Setpoint);
ryanlin97 17:93a636b16b9e 129 wait(.05);
ryanlin97 12:921488918749 130 }
ryanlin97 18:3eadf01ec1b0 131
ryanlin97 12:921488918749 132 Wheelchair::stop();
ryanlin97 17:93a636b16b9e 133 out->printf("done \r\n");
ryanlin97 12:921488918749 134 }
ryanlin97 18:3eadf01ec1b0 135
ryanlin97 12:921488918749 136 void Wheelchair::pid_left(int deg)
ryanlin97 12:921488918749 137 {
ryanlin97 17:93a636b16b9e 138 float pid_yaw;
ryanlin97 12:921488918749 139 bool overturn = false;
ryanlin97 12:921488918749 140
ryanlin97 17:93a636b16b9e 141 out->printf("pid Left\r\r\n");
ryanlin97 12:921488918749 142 x->write(def);
ryanlin97 12:921488918749 143 Setpoint = curr_yaw - deg;
ryanlin97 17:93a636b16b9e 144 pid_yaw = curr_yaw;
ryanlin97 12:921488918749 145 if(Setpoint < 0) {
ryanlin97 12:921488918749 146 overturn = true;
ryanlin97 12:921488918749 147 }
ryanlin97 18:3eadf01ec1b0 148
ryanlin97 17:93a636b16b9e 149 myPID.SetTunings(5,0, 0.004);
ryanlin97 21:69df88af7c46 150 myPID.SetOutputLimits(0,high - def -.12);
ryanlin97 17:93a636b16b9e 151 myPID.SetControllerDirection(REVERSE);
ryanlin97 18:3eadf01ec1b0 152
ryanlin97 18:3eadf01ec1b0 153 while(pid_yaw > Setpoint + 3){
ryanlin97 17:93a636b16b9e 154 myPID.Compute();
ryanlin97 17:93a636b16b9e 155 if(overturn && curr_yaw > Setpoint+deg+1)
ryanlin97 17:93a636b16b9e 156 {
ryanlin97 17:93a636b16b9e 157 pid_yaw = curr_yaw - 360;
ryanlin97 17:93a636b16b9e 158 }
ryanlin97 17:93a636b16b9e 159 else
ryanlin97 17:93a636b16b9e 160 pid_yaw = curr_yaw;
ryanlin97 17:93a636b16b9e 161 double tempor = Output+def;
ryanlin97 17:93a636b16b9e 162
ryanlin97 17:93a636b16b9e 163 y->write(tempor);
ryanlin97 17:93a636b16b9e 164 out->printf("curr_yaw %f\r\n", curr_yaw);
ryanlin97 17:93a636b16b9e 165 wait(.05);
ryanlin97 12:921488918749 166 }
ryanlin97 17:93a636b16b9e 167 Wheelchair::stop();
ryanlin97 12:921488918749 168 }
ryanlin97 12:921488918749 169
ryanlin97 12:921488918749 170 void Wheelchair::pid_turn(int deg) {
ryanlin97 12:921488918749 171 if(deg > 180) {
ryanlin97 12:921488918749 172 deg -= 360;
ryanlin97 12:921488918749 173 }
ryanlin97 12:921488918749 174
ryanlin97 12:921488918749 175 else if(deg < -180) {
ryanlin97 12:921488918749 176 deg+=360;
ryanlin97 12:921488918749 177 }
ryanlin97 12:921488918749 178
ryanlin97 12:921488918749 179 int turnAmt = abs(deg);
ryanlin97 12:921488918749 180 ti->reset();
ryanlin97 12:921488918749 181
ryanlin97 12:921488918749 182 if(deg >= 0){
ryanlin97 12:921488918749 183 Wheelchair::pid_right(turnAmt);
ryanlin97 12:921488918749 184 }
ryanlin97 12:921488918749 185 else {
ryanlin97 12:921488918749 186 Wheelchair::pid_left(turnAmt);
ryanlin97 12:921488918749 187 }
ryanlin97 12:921488918749 188 }
ryanlin97 12:921488918749 189
ryanlin97 11:d14a1f7f1297 190 double Wheelchair::turn_right(int deg)
ryanlin97 6:0cd57bdd8fbc 191 {
ryanlin97 8:381a4ec3fef8 192 bool overturn = false;
ryanlin97 7:5e38d43fbce3 193 out->printf("turning right\n");
ryanlin97 7:5e38d43fbce3 194
ryanlin97 11:d14a1f7f1297 195 double start = curr_yaw;
ryanlin97 11:d14a1f7f1297 196 double final = start + deg;
ryanlin97 11:d14a1f7f1297 197
ryanlin97 11:d14a1f7f1297 198 if(final > 360) {
ryanlin97 11:d14a1f7f1297 199 final -= 360;
ryanlin97 8:381a4ec3fef8 200 overturn = true;
ryanlin97 8:381a4ec3fef8 201 }
ryanlin97 8:381a4ec3fef8 202
ryanlin97 8:381a4ec3fef8 203 out->printf("start %f, final %f\n", start, final);
ryanlin97 11:d14a1f7f1297 204
ryanlin97 11:d14a1f7f1297 205 double curr = -1;
ryanlin97 12:921488918749 206 while(curr <= final - 30) {
ryanlin97 8:381a4ec3fef8 207 Wheelchair::right();
ryanlin97 8:381a4ec3fef8 208 if( out->readable()) {
ryanlin97 8:381a4ec3fef8 209 out->printf("stopped\n");
ryanlin97 8:381a4ec3fef8 210 Wheelchair::stop();
ryanlin97 8:381a4ec3fef8 211 return;
ryanlin97 7:5e38d43fbce3 212 }
ryanlin97 11:d14a1f7f1297 213 curr = curr_yaw;
ryanlin97 11:d14a1f7f1297 214 if(overturn && curr > (360 - deg) ) {
ryanlin97 11:d14a1f7f1297 215 curr = 0;
ryanlin97 6:0cd57bdd8fbc 216 }
ryanlin97 6:0cd57bdd8fbc 217 }
ryanlin97 11:d14a1f7f1297 218
ryanlin97 11:d14a1f7f1297 219 out->printf("done turning start %f final %f\n", start, final);
ryanlin97 10:e5463c11e0a0 220 Wheelchair::stop();
ryanlin97 11:d14a1f7f1297 221
ryanlin97 12:921488918749 222 //delete me
ryanlin97 12:921488918749 223 wait(5);
ryanlin97 12:921488918749 224
ryanlin97 12:921488918749 225 float correction = final - curr_yaw;
ryanlin97 12:921488918749 226 out->printf("final pos %f actual pos %f\n", final, curr_yaw);
ryanlin97 12:921488918749 227 Wheelchair::turn_left(abs(correction));
ryanlin97 12:921488918749 228 Wheelchair::stop();
ryanlin97 12:921488918749 229
ryanlin97 12:921488918749 230 wait(5);
ryanlin97 12:921488918749 231 out->printf("curr_yaw %f\n", curr_yaw);
ryanlin97 11:d14a1f7f1297 232 return final;
ryanlin97 6:0cd57bdd8fbc 233 }
ryanlin97 6:0cd57bdd8fbc 234
ryanlin97 11:d14a1f7f1297 235 double Wheelchair::turn_left(int deg)
ryanlin97 6:0cd57bdd8fbc 236 {
ryanlin97 8:381a4ec3fef8 237 bool overturn = false;
ryanlin97 8:381a4ec3fef8 238 out->printf("turning left\n");
ryanlin97 8:381a4ec3fef8 239
ryanlin97 12:921488918749 240 double start = curr_yaw;
ryanlin97 11:d14a1f7f1297 241 double final = start - deg;
ryanlin97 8:381a4ec3fef8 242
ryanlin97 11:d14a1f7f1297 243 if(final < 0) {
ryanlin97 11:d14a1f7f1297 244 final += 360;
ryanlin97 8:381a4ec3fef8 245 overturn = true;
ryanlin97 8:381a4ec3fef8 246 }
ryanlin97 8:381a4ec3fef8 247
ryanlin97 11:d14a1f7f1297 248 out->printf("start %f, final %f\n", start, final);
ryanlin97 6:0cd57bdd8fbc 249
ryanlin97 11:d14a1f7f1297 250 double curr = 361;
ryanlin97 11:d14a1f7f1297 251 while(curr >= final) {
ryanlin97 6:0cd57bdd8fbc 252 Wheelchair::left();
ryanlin97 8:381a4ec3fef8 253 if( out->readable()) {
ryanlin97 8:381a4ec3fef8 254 out->printf("stopped\n");
ryanlin97 8:381a4ec3fef8 255 Wheelchair::stop();
ryanlin97 8:381a4ec3fef8 256 return;
ryanlin97 8:381a4ec3fef8 257 }
ryanlin97 11:d14a1f7f1297 258 curr = curr_yaw;
ryanlin97 11:d14a1f7f1297 259
ryanlin97 11:d14a1f7f1297 260 if(overturn && curr >= 0 && curr <= start ) {
ryanlin97 11:d14a1f7f1297 261 curr = 361;
ryanlin97 8:381a4ec3fef8 262 }
ryanlin97 6:0cd57bdd8fbc 263 }
ryanlin97 8:381a4ec3fef8 264
ryanlin97 11:d14a1f7f1297 265 out->printf("done turning start %f final %f\n", start, final);
ryanlin97 10:e5463c11e0a0 266 Wheelchair::stop();
ryanlin97 12:921488918749 267
ryanlin97 17:93a636b16b9e 268
ryanlin97 11:d14a1f7f1297 269 return final;
ryanlin97 11:d14a1f7f1297 270 }
ryanlin97 11:d14a1f7f1297 271
ryanlin97 11:d14a1f7f1297 272 void Wheelchair::turn(int deg)
ryanlin97 11:d14a1f7f1297 273 {
ryanlin97 11:d14a1f7f1297 274 if(deg > 180) {
ryanlin97 11:d14a1f7f1297 275 deg -= 360;
ryanlin97 11:d14a1f7f1297 276 }
ryanlin97 11:d14a1f7f1297 277
ryanlin97 11:d14a1f7f1297 278 else if(deg < -180) {
ryanlin97 11:d14a1f7f1297 279 deg+=360;
ryanlin97 11:d14a1f7f1297 280 }
ryanlin97 11:d14a1f7f1297 281
ryanlin97 11:d14a1f7f1297 282 double finalpos;
ryanlin97 11:d14a1f7f1297 283 int turnAmt = abs(deg);
ryanlin97 17:93a636b16b9e 284
ryanlin97 11:d14a1f7f1297 285
ryanlin97 11:d14a1f7f1297 286 float correction = finalpos - curr_yaw;
ryanlin97 11:d14a1f7f1297 287 out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw);
ryanlin97 11:d14a1f7f1297 288
ryanlin97 11:d14a1f7f1297 289
ryanlin97 11:d14a1f7f1297 290 //if(abs(correction) > turn_precision) {
ryanlin97 11:d14a1f7f1297 291 out->printf("correcting %f\n", correction);
ryanlin97 11:d14a1f7f1297 292 //ti->reset();
ryanlin97 11:d14a1f7f1297 293 Wheelchair::turn_left(curr_yaw - finalpos);
ryanlin97 11:d14a1f7f1297 294 return;
ryanlin97 11:d14a1f7f1297 295 //}
ryanlin97 11:d14a1f7f1297 296
ryanlin97 6:0cd57bdd8fbc 297 }
ryanlin97 8:381a4ec3fef8 298
ryanlin97 12:921488918749 299 float Wheelchair::getDistance() {
ryanlin97 12:921488918749 300 return wheel->getDistance(Diameter);
ryanlin97 12:921488918749 301 }
ryanlin97 12:921488918749 302
ryanlin97 12:921488918749 303 void Wheelchair::resetDistance(){
ryanlin97 12:921488918749 304 wheel->reset();
ryanlin97 12:921488918749 305 }
ryanlin97 10:e5463c11e0a0 306
ryanlin97 19:df4257c75ed0 307 void Wheelchair::turn_on() {
ryanlin97 19:df4257c75ed0 308 on = 1;
ryanlin97 19:df4257c75ed0 309 wait(1);
ryanlin97 19:df4257c75ed0 310 on = 0;
ryanlin97 19:df4257c75ed0 311 }
ryanlin97 12:921488918749 312
ryanlin97 19:df4257c75ed0 313 void Wheelchair::turn_off() {
ryanlin97 19:df4257c75ed0 314 off = 1;
ryanlin97 19:df4257c75ed0 315 wait(1);
ryanlin97 19:df4257c75ed0 316 off = 0;
ryanlin97 19:df4257c75ed0 317 }
ryanlin97 19:df4257c75ed0 318
ryanlin97 19:df4257c75ed0 319 void Wheelchair::pid_forward(double mm)
ryanlin97 19:df4257c75ed0 320 {
ryanlin97 21:69df88af7c46 321 y->write(def + offset);
ryanlin97 21:69df88af7c46 322 wheel->reset();
ryanlin97 21:69df88af7c46 323 Input = 0;
ryanlin97 19:df4257c75ed0 324 out->printf("pid foward\r\n");
ryanlin97 19:df4257c75ed0 325
ryanlin97 21:69df88af7c46 326
ryanlin97 21:69df88af7c46 327 Setpoint = mm-20;
ryanlin97 19:df4257c75ed0 328
ryanlin97 19:df4257c75ed0 329 // Setpoint = wheel_right.getDistance(37.5)+mm;
ryanlin97 19:df4257c75ed0 330 myPIDDistance.SetTunings(5,0, 0.004);
ryanlin97 21:69df88af7c46 331 myPIDDistance.SetOutputLimits(0,high - def - 0.15);
ryanlin97 19:df4257c75ed0 332 myPIDDistance.SetControllerDirection(DIRECT);
ryanlin97 19:df4257c75ed0 333
ryanlin97 21:69df88af7c46 334 while(Input < Setpoint-5){//pid_yaw < Setpoint + 2) {
ryanlin97 21:69df88af7c46 335 if(out->readable()){
ryanlin97 21:69df88af7c46 336 Wheelchair::stop();
ryanlin97 21:69df88af7c46 337 break;}
ryanlin97 21:69df88af7c46 338 Input = wheel->getDistance(53.975);
ryanlin97 21:69df88af7c46 339 //out->printf("input foward %d\r\n", wheel->getPulses());
ryanlin97 21:69df88af7c46 340 wait(.05);
ryanlin97 19:df4257c75ed0 341 myPIDDistance.Compute();
ryanlin97 21:69df88af7c46 342
ryanlin97 21:69df88af7c46 343 x->write(Output + def);
ryanlin97 21:69df88af7c46 344 out->printf("distance %f\r\n", Input);
ryanlin97 19:df4257c75ed0 345 }
ryanlin97 19:df4257c75ed0 346
ryanlin97 19:df4257c75ed0 347 }
ryanlin97 19:df4257c75ed0 348
ryanlin97 21:69df88af7c46 349 void Wheelchair::desk() {
ryanlin97 21:69df88af7c46 350 Wheelchair::pid_forward(5461);
ryanlin97 21:69df88af7c46 351 Wheelchair::pid_right(87);
ryanlin97 21:69df88af7c46 352 Wheelchair::pid_forward(3658);
ryanlin97 21:69df88af7c46 353 Wheelchair::pid_left(87);
ryanlin97 21:69df88af7c46 354 Wheelchair::pid_forward(3734);
ryanlin97 21:69df88af7c46 355 }
ryanlin97 21:69df88af7c46 356
ryanlin97 19:df4257c75ed0 357 void Wheelchair::kitchen() {
ryanlin97 19:df4257c75ed0 358 Wheelchair::pid_forward(5461);
ryanlin97 21:69df88af7c46 359 Wheelchair::pid_right(87);
ryanlin97 19:df4257c75ed0 360 Wheelchair::pid_forward(3658);
ryanlin97 21:69df88af7c46 361 Wheelchair::pid_left(87);
ryanlin97 21:69df88af7c46 362 Wheelchair::pid_forward(305);
ryanlin97 21:69df88af7c46 363 }
ryanlin97 21:69df88af7c46 364
ryanlin97 21:69df88af7c46 365 void Wheelchair::back() {
ryanlin97 21:69df88af7c46 366 Wheelchair::pid_right(177);
ryanlin97 21:69df88af7c46 367 Wheelchair::pid_forward(3700);
ryanlin97 19:df4257c75ed0 368 }