Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

wheelchair.cpp@17:7f3b69300bb6, 2018-08-28 (annotated)

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