ryan lin
/
wheeldriveassistance
wheelchair code for driver assitance
Fork of wheelchairalexa by
wheelchair.cpp@12:0e5a0571b497, 2018-08-17 (annotated)
- Committer:
- ryanlin97
- Date:
- Fri Aug 17 03:10:39 2018 +0000
- Revision:
- 12:0e5a0571b497
- Parent:
- 11:75f0f13ff6c1
code for driver assistance;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 0:fc0c4a184482 | 1 | #include "wheelchair.h" |
ryanlin97 | 11:75f0f13ff6c1 | 2 | |
ryanlin97 | 10:e5463c11e0a0 | 3 | bool manual_drive = false; |
ryanlin97 | 11:75f0f13ff6c1 | 4 | volatile float north; |
ryanlin97 | 11:75f0f13ff6c1 | 5 | //volatile double curr_yaw; |
ryanlin97 | 11:75f0f13ff6c1 | 6 | double curr_yaw; |
ryanlin97 | 11:75f0f13ff6c1 | 7 | double Setpoint, Output; |
ryanlin97 | 1:c0beadca1617 | 8 | |
ryanlin97 | 11:75f0f13ff6c1 | 9 | void Wheelchair::compass_thread() { |
ryanlin97 | 11:75f0f13ff6c1 | 10 | |
ryanlin97 | 11:75f0f13ff6c1 | 11 | } |
ryanlin97 | 11:75f0f13ff6c1 | 12 | |
ryanlin97 | 8:381a4ec3fef8 | 13 | Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time ) |
ryanlin97 | 1:c0beadca1617 | 14 | { |
ryanlin97 | 3:a5e71bfdb492 | 15 | x = new PwmOut(xPin); |
ryanlin97 | 3:a5e71bfdb492 | 16 | y = new PwmOut(yPin); |
ryanlin97 | 11:75f0f13ff6c1 | 17 | //imu = new chair_MPU9250(pc, time); |
ryanlin97 | 11:75f0f13ff6c1 | 18 | Wheelchair::stop(); |
ryanlin97 | 7:5e38d43fbce3 | 19 | out = pc; |
ryanlin97 | 7:5e38d43fbce3 | 20 | out->printf("wheelchair setup done \n"); |
ryanlin97 | 11:75f0f13ff6c1 | 21 | ti = time; |
ryanlin97 | 1:c0beadca1617 | 22 | } |
ryanlin97 | 6:0cd57bdd8fbc | 23 | |
ryanlin97 | 1:c0beadca1617 | 24 | void Wheelchair::forward() |
ryanlin97 | 1:c0beadca1617 | 25 | { |
ryanlin97 | 0:fc0c4a184482 | 26 | x->write(high); |
ryanlin97 | 3:a5e71bfdb492 | 27 | y->write(def+offset); |
ryanlin97 | 0:fc0c4a184482 | 28 | } |
ryanlin97 | 0:fc0c4a184482 | 29 | |
ryanlin97 | 1:c0beadca1617 | 30 | void Wheelchair::backward() |
ryanlin97 | 1:c0beadca1617 | 31 | { |
ryanlin97 | 0:fc0c4a184482 | 32 | x->write(low); |
ryanlin97 | 0:fc0c4a184482 | 33 | y->write(def); |
ryanlin97 | 0:fc0c4a184482 | 34 | } |
ryanlin97 | 0:fc0c4a184482 | 35 | |
ryanlin97 | 1:c0beadca1617 | 36 | void Wheelchair::right() |
ryanlin97 | 1:c0beadca1617 | 37 | { |
ryanlin97 | 0:fc0c4a184482 | 38 | x->write(def); |
ryanlin97 | 11:75f0f13ff6c1 | 39 | y->write(low); |
ryanlin97 | 0:fc0c4a184482 | 40 | } |
ryanlin97 | 0:fc0c4a184482 | 41 | |
ryanlin97 | 1:c0beadca1617 | 42 | void Wheelchair::left() |
ryanlin97 | 1:c0beadca1617 | 43 | { |
ryanlin97 | 0:fc0c4a184482 | 44 | x->write(def); |
ryanlin97 | 11:75f0f13ff6c1 | 45 | y->write(high); |
ryanlin97 | 0:fc0c4a184482 | 46 | } |
ryanlin97 | 0:fc0c4a184482 | 47 | |
ryanlin97 | 1:c0beadca1617 | 48 | void Wheelchair::stop() |
ryanlin97 | 1:c0beadca1617 | 49 | { |
ryanlin97 | 0:fc0c4a184482 | 50 | x->write(def); |
ryanlin97 | 0:fc0c4a184482 | 51 | y->write(def); |
ryanlin97 | 6:0cd57bdd8fbc | 52 | } |
ryanlin97 | 11:75f0f13ff6c1 | 53 | // counter clockwise is - |
ryanlin97 | 11:75f0f13ff6c1 | 54 | // clockwise is + |
ryanlin97 | 11:75f0f13ff6c1 | 55 | double Wheelchair::turn_right(int deg) |
ryanlin97 | 6:0cd57bdd8fbc | 56 | { |
ryanlin97 | 11:75f0f13ff6c1 | 57 | return; |
ryanlin97 | 6:0cd57bdd8fbc | 58 | } |
ryanlin97 | 6:0cd57bdd8fbc | 59 | |
ryanlin97 | 11:75f0f13ff6c1 | 60 | double Wheelchair::turn_left(int deg) |
ryanlin97 | 6:0cd57bdd8fbc | 61 | { |
ryanlin97 | 11:75f0f13ff6c1 | 62 | return; |
ryanlin97 | 6:0cd57bdd8fbc | 63 | } |
ryanlin97 | 8:381a4ec3fef8 | 64 | |
ryanlin97 | 11:75f0f13ff6c1 | 65 | void Wheelchair::turn(int deg) |
ryanlin97 | 11:75f0f13ff6c1 | 66 | { |
ryanlin97 | 11:75f0f13ff6c1 | 67 | return; |
ryanlin97 | 11:75f0f13ff6c1 | 68 | } |
ryanlin97 | 10:e5463c11e0a0 | 69 | |
ryanlin97 | 12:0e5a0571b497 | 70 | /* |
ryanlin97 | 12:0e5a0571b497 | 71 | * joystick has analog out of 200-700, scale values between 1.3 and 3.3 |
ryanlin97 | 12:0e5a0571b497 | 72 | */ |
ryanlin97 | 12:0e5a0571b497 | 73 | void Wheelchair::move(float x_coor, float y_coor, bool leftbound, bool rightbound, bool straightbound) |
ryanlin97 | 12:0e5a0571b497 | 74 | { |
ryanlin97 | 12:0e5a0571b497 | 75 | |
ryanlin97 | 12:0e5a0571b497 | 76 | |
ryanlin97 | 12:0e5a0571b497 | 77 | float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; |
ryanlin97 | 12:0e5a0571b497 | 78 | float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; |
ryanlin97 | 12:0e5a0571b497 | 79 | |
ryanlin97 | 12:0e5a0571b497 | 80 | if(leftbound && (scaled_x > 2.5) ) { |
ryanlin97 | 12:0e5a0571b497 | 81 | Wheelchair::stop(); |
ryanlin97 | 12:0e5a0571b497 | 82 | } |
ryanlin97 | 12:0e5a0571b497 | 83 | |
ryanlin97 | 12:0e5a0571b497 | 84 | else if(rightbound && (scaled_x < 2.5) ) { |
ryanlin97 | 12:0e5a0571b497 | 85 | Wheelchair::stop(); |
ryanlin97 | 12:0e5a0571b497 | 86 | } |
ryanlin97 | 12:0e5a0571b497 | 87 | |
ryanlin97 | 12:0e5a0571b497 | 88 | else if(straightbound && (scaled_y > 2.5) ) { |
ryanlin97 | 12:0e5a0571b497 | 89 | Wheelchair::stop(); |
ryanlin97 | 12:0e5a0571b497 | 90 | } |
ryanlin97 | 12:0e5a0571b497 | 91 | |
ryanlin97 | 12:0e5a0571b497 | 92 | else { |
ryanlin97 | 12:0e5a0571b497 | 93 | x->write(scaled_x); |
ryanlin97 | 12:0e5a0571b497 | 94 | y->write(scaled_y); |
ryanlin97 | 12:0e5a0571b497 | 95 | } |
ryanlin97 | 12:0e5a0571b497 | 96 | } |
ryanlin97 | 12:0e5a0571b497 | 97 |