Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

wheelchair.cpp@18:663b6d693252, 2018-08-29 (annotated)

Committer:
jvfausto
Date:
Wed Aug 29 16:51:33 2018 +0000
Revision:
18:663b6d693252
Parent:
17:7f3b69300bb6
Child:
19:71a6621ee5c3
with pid left right and foward

Who changed what in which revision?

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