Affordable Smart Wheelchair / wheelchaircontrol2

Revised for integration

Dependencies:   QEI2 chair_BNO055 PID VL53L1X_Filter

wheelchair.cpp@20:f42db4ae16f0, 2018-08-31 (annotated)

Committer:
jvfausto
Date:
Fri Aug 31 20:00:01 2018 +0000
Revision:
20:f42db4ae16f0
Parent:
19:71a6621ee5c3
Child:
21:3489cffad196
Child:
23:8d11d953ceeb
j

Who changed what in which revision?

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