Affordable Smart Wheelchair / wheelchaircontrol2

Revised for integration

Dependencies:   QEI2 chair_BNO055 PID VL53L1X_Filter

wheelchair.cpp@8:381a4ec3fef8, 2018-07-22 (annotated)

Committer:
ryanlin97
Date:
Sun Jul 22 18:46:24 2018 +0000
Revision:
8:381a4ec3fef8
Parent:
7:5e38d43fbce3
Child:
10:e5463c11e0a0
updated for mpu;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 1:c0beadca1617 2
ryanlin97 8:381a4ec3fef8 3 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
ryanlin97 1:c0beadca1617 4 {
ryanlin97 3:a5e71bfdb492 5 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 6 y = new PwmOut(yPin);
ryanlin97 6:0cd57bdd8fbc 7 //imu = new chair_BNO055();
ryanlin97 7:5e38d43fbce3 8 imu = new chair_MPU9250(pc, time);
ryanlin97 7:5e38d43fbce3 9 imu->setup();
ryanlin97 7:5e38d43fbce3 10 out = pc;
ryanlin97 7:5e38d43fbce3 11 out->printf("wheelchair setup done \n");
ryanlin97 7:5e38d43fbce3 12
ryanlin97 1:c0beadca1617 13 }
ryanlin97 6:0cd57bdd8fbc 14
ryanlin97 3:a5e71bfdb492 15 /*
ryanlin97 3:a5e71bfdb492 16 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
ryanlin97 3:a5e71bfdb492 17 */
ryanlin97 3:a5e71bfdb492 18 void Wheelchair::move(float x_coor, float y_coor)
ryanlin97 1:c0beadca1617 19 {
ryanlin97 6:0cd57bdd8fbc 20
ryanlin97 4:29a27953fe70 21 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
ryanlin97 4:29a27953fe70 22 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
ryanlin97 4:29a27953fe70 23 x->write(scaled_x);
ryanlin97 4:29a27953fe70 24 y->write(scaled_y);
ryanlin97 1:c0beadca1617 25
ryanlin97 5:e0ccaab3959a 26 }
ryanlin97 1:c0beadca1617 27 void Wheelchair::forward()
ryanlin97 1:c0beadca1617 28 {
ryanlin97 0:fc0c4a184482 29 x->write(high);
ryanlin97 3:a5e71bfdb492 30 y->write(def+offset);
ryanlin97 0:fc0c4a184482 31 }
ryanlin97 0:fc0c4a184482 32
ryanlin97 1:c0beadca1617 33 void Wheelchair::backward()
ryanlin97 1:c0beadca1617 34 {
ryanlin97 0:fc0c4a184482 35 x->write(low);
ryanlin97 0:fc0c4a184482 36 y->write(def);
ryanlin97 0:fc0c4a184482 37 }
ryanlin97 0:fc0c4a184482 38
ryanlin97 1:c0beadca1617 39 void Wheelchair::right()
ryanlin97 1:c0beadca1617 40 {
ryanlin97 0:fc0c4a184482 41 x->write(def);
ryanlin97 0:fc0c4a184482 42 y->write(high);
ryanlin97 0:fc0c4a184482 43 }
ryanlin97 0:fc0c4a184482 44
ryanlin97 1:c0beadca1617 45 void Wheelchair::left()
ryanlin97 1:c0beadca1617 46 {
ryanlin97 0:fc0c4a184482 47 x->write(def);
ryanlin97 0:fc0c4a184482 48 y->write(low);
ryanlin97 0:fc0c4a184482 49 }
ryanlin97 0:fc0c4a184482 50
ryanlin97 1:c0beadca1617 51 void Wheelchair::stop()
ryanlin97 1:c0beadca1617 52 {
ryanlin97 0:fc0c4a184482 53 x->write(def);
ryanlin97 0:fc0c4a184482 54 y->write(def);
ryanlin97 6:0cd57bdd8fbc 55 }
ryanlin97 8:381a4ec3fef8 56 // counter clockwise is +
ryanlin97 8:381a4ec3fef8 57 // clockwise is -
ryanlin97 6:0cd57bdd8fbc 58
ryanlin97 7:5e38d43fbce3 59 void Wheelchair::turn_right()
ryanlin97 6:0cd57bdd8fbc 60 {
ryanlin97 8:381a4ec3fef8 61 bool overturn = false;
ryanlin97 7:5e38d43fbce3 62 out->printf("turning right\n");
ryanlin97 7:5e38d43fbce3 63
ryanlin97 6:0cd57bdd8fbc 64 double start = imu->yaw();
ryanlin97 8:381a4ec3fef8 65 double final = start - 90;
ryanlin97 8:381a4ec3fef8 66
ryanlin97 8:381a4ec3fef8 67 if(final < 0) {
ryanlin97 8:381a4ec3fef8 68 final += 360;
ryanlin97 8:381a4ec3fef8 69 overturn = true;
ryanlin97 8:381a4ec3fef8 70 }
ryanlin97 8:381a4ec3fef8 71
ryanlin97 8:381a4ec3fef8 72 out->printf("start %f, final %f\n", start, final);
ryanlin97 7:5e38d43fbce3 73
ryanlin97 8:381a4ec3fef8 74 double curr = 361;
ryanlin97 8:381a4ec3fef8 75 while(curr >= final) {
ryanlin97 8:381a4ec3fef8 76 Wheelchair::right();
ryanlin97 8:381a4ec3fef8 77 if( out->readable()) {
ryanlin97 8:381a4ec3fef8 78 out->printf("stopped\n");
ryanlin97 8:381a4ec3fef8 79 Wheelchair::stop();
ryanlin97 8:381a4ec3fef8 80 return;
ryanlin97 7:5e38d43fbce3 81 }
ryanlin97 8:381a4ec3fef8 82 curr = imu->yaw();
ryanlin97 8:381a4ec3fef8 83 //out->printf("curr %f \n", curr);
ryanlin97 8:381a4ec3fef8 84
ryanlin97 8:381a4ec3fef8 85 if(overturn && curr >= 0 && curr <= start ) {
ryanlin97 8:381a4ec3fef8 86 curr = 360;
ryanlin97 6:0cd57bdd8fbc 87 }
ryanlin97 6:0cd57bdd8fbc 88 }
ryanlin97 8:381a4ec3fef8 89
ryanlin97 7:5e38d43fbce3 90 out->printf("done turning\n");
ryanlin97 8:381a4ec3fef8 91
ryanlin97 6:0cd57bdd8fbc 92 }
ryanlin97 6:0cd57bdd8fbc 93
ryanlin97 6:0cd57bdd8fbc 94 void Wheelchair::turn_left()
ryanlin97 6:0cd57bdd8fbc 95 {
ryanlin97 8:381a4ec3fef8 96 bool overturn = false;
ryanlin97 8:381a4ec3fef8 97 out->printf("turning left\n");
ryanlin97 8:381a4ec3fef8 98
ryanlin97 6:0cd57bdd8fbc 99 double start = imu->yaw();
ryanlin97 8:381a4ec3fef8 100 double final = start + 90;
ryanlin97 8:381a4ec3fef8 101
ryanlin97 8:381a4ec3fef8 102 if(final > 360) {
ryanlin97 8:381a4ec3fef8 103 final -= 360;
ryanlin97 8:381a4ec3fef8 104 overturn = true;
ryanlin97 8:381a4ec3fef8 105 }
ryanlin97 8:381a4ec3fef8 106
ryanlin97 8:381a4ec3fef8 107 out->printf("start %f, final %f\n", start, final);
ryanlin97 6:0cd57bdd8fbc 108
ryanlin97 8:381a4ec3fef8 109 double curr = -1;
ryanlin97 8:381a4ec3fef8 110 while(curr <= final) {
ryanlin97 6:0cd57bdd8fbc 111 Wheelchair::left();
ryanlin97 8:381a4ec3fef8 112 if( out->readable()) {
ryanlin97 8:381a4ec3fef8 113 out->printf("stopped\n");
ryanlin97 8:381a4ec3fef8 114 Wheelchair::stop();
ryanlin97 8:381a4ec3fef8 115 return;
ryanlin97 8:381a4ec3fef8 116 }
ryanlin97 8:381a4ec3fef8 117 curr = imu->yaw();
ryanlin97 8:381a4ec3fef8 118 if(overturn && curr > (360 - 90) ) {
ryanlin97 8:381a4ec3fef8 119 curr = 0;
ryanlin97 8:381a4ec3fef8 120 }
ryanlin97 8:381a4ec3fef8 121 out->printf("curr %f \n", curr);
ryanlin97 6:0cd57bdd8fbc 122 }
ryanlin97 8:381a4ec3fef8 123
ryanlin97 8:381a4ec3fef8 124 out->printf("done turning\n");
ryanlin97 6:0cd57bdd8fbc 125 }
ryanlin97 8:381a4ec3fef8 126