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