Christian Benitez
/
wheelchaircontrol
Wheelchair control
Fork of wheelchaircontrol by
wheelchair.cpp@6:8cd00c26bb47, 2018-07-17 (annotated)
- Committer:
- cpbenite
- Date:
- Tue Jul 17 19:19:26 2018 +0000
- Revision:
- 6:8cd00c26bb47
- Parent:
- 5:e0ccaab3959a
Wheelchair control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 0:fc0c4a184482 | 1 | #include "wheelchair.h" |
ryanlin97 | 1:c0beadca1617 | 2 | |
cpbenite | 6:8cd00c26bb47 | 3 | // Note: 'def' is short for 'Default Position' |
cpbenite | 6:8cd00c26bb47 | 4 | |
cpbenite | 6:8cd00c26bb47 | 5 | // Initialize Wheelchair Pins |
cpbenite | 6:8cd00c26bb47 | 6 | Wheelchair::Wheelchair (PinName xPin, PinName yPin) |
ryanlin97 | 1:c0beadca1617 | 7 | { |
ryanlin97 | 3:a5e71bfdb492 | 8 | x = new PwmOut(xPin); |
ryanlin97 | 3:a5e71bfdb492 | 9 | y = new PwmOut(yPin); |
ryanlin97 | 5:e0ccaab3959a | 10 | imu = new chair_imu(); |
ryanlin97 | 1:c0beadca1617 | 11 | } |
cpbenite | 6:8cd00c26bb47 | 12 | |
ryanlin97 | 3:a5e71bfdb492 | 13 | /* |
cpbenite | 6:8cd00c26bb47 | 14 | * Joystick has AnalogOut of 200-700, scale values between 1.3 and 3.3 |
ryanlin97 | 3:a5e71bfdb492 | 15 | */ |
cpbenite | 6:8cd00c26bb47 | 16 | |
cpbenite | 6:8cd00c26bb47 | 17 | // Convert Joystick Values to Move the Wheelchair |
ryanlin97 | 3:a5e71bfdb492 | 18 | void Wheelchair::move(float x_coor, float y_coor) |
ryanlin97 | 1:c0beadca1617 | 19 | { |
cpbenite | 6:8cd00c26bb47 | 20 | |
cpbenite | 6:8cd00c26bb47 | 21 | float scaled_x = ((x_coor * 1.6f) + 1.7f) / 3.3f; |
cpbenite | 6:8cd00c26bb47 | 22 | float scaled_y = (3.3f - (y_coor * 1.6f)) / 3.3f; |
ryanlin97 | 4:29a27953fe70 | 23 | x->write(scaled_x); |
cpbenite | 6:8cd00c26bb47 | 24 | y->write(scaled_y); |
ryanlin97 | 1:c0beadca1617 | 25 | } |
ryanlin97 | 1:c0beadca1617 | 26 | |
cpbenite | 6:8cd00c26bb47 | 27 | // Turn 90 degrees to the Right |
ryanlin97 | 5:e0ccaab3959a | 28 | void Wheelchair::turn_right(){ |
ryanlin97 | 5:e0ccaab3959a | 29 | double start = imu->yaw(); |
ryanlin97 | 5:e0ccaab3959a | 30 | double final = start + 90; |
ryanlin97 | 5:e0ccaab3959a | 31 | if(final > 360) |
ryanlin97 | 5:e0ccaab3959a | 32 | final -= 360; |
ryanlin97 | 5:e0ccaab3959a | 33 | |
ryanlin97 | 5:e0ccaab3959a | 34 | while(imu->yaw() <= final) { |
ryanlin97 | 5:e0ccaab3959a | 35 | Wheelchair::right(); |
ryanlin97 | 5:e0ccaab3959a | 36 | } |
ryanlin97 | 5:e0ccaab3959a | 37 | } |
ryanlin97 | 5:e0ccaab3959a | 38 | |
cpbenite | 6:8cd00c26bb47 | 39 | // Turn 90 degrees to the Left |
ryanlin97 | 5:e0ccaab3959a | 40 | void Wheelchair::turn_left(){ |
ryanlin97 | 5:e0ccaab3959a | 41 | double start = imu->yaw(); |
ryanlin97 | 5:e0ccaab3959a | 42 | double final = start - 90; |
ryanlin97 | 5:e0ccaab3959a | 43 | if(final <0) |
ryanlin97 | 5:e0ccaab3959a | 44 | final += 360; |
ryanlin97 | 5:e0ccaab3959a | 45 | |
ryanlin97 | 5:e0ccaab3959a | 46 | while(imu->yaw() >= final) { |
ryanlin97 | 5:e0ccaab3959a | 47 | Wheelchair::left(); |
ryanlin97 | 5:e0ccaab3959a | 48 | } |
ryanlin97 | 5:e0ccaab3959a | 49 | } |
cpbenite | 6:8cd00c26bb47 | 50 | |
ryanlin97 | 1:c0beadca1617 | 51 | void Wheelchair::forward() |
ryanlin97 | 1:c0beadca1617 | 52 | { |
ryanlin97 | 0:fc0c4a184482 | 53 | x->write(high); |
ryanlin97 | 3:a5e71bfdb492 | 54 | y->write(def+offset); |
ryanlin97 | 0:fc0c4a184482 | 55 | } |
cpbenite | 6:8cd00c26bb47 | 56 | |
ryanlin97 | 1:c0beadca1617 | 57 | void Wheelchair::backward() |
ryanlin97 | 1:c0beadca1617 | 58 | { |
ryanlin97 | 0:fc0c4a184482 | 59 | x->write(low); |
ryanlin97 | 0:fc0c4a184482 | 60 | y->write(def); |
ryanlin97 | 0:fc0c4a184482 | 61 | } |
cpbenite | 6:8cd00c26bb47 | 62 | |
ryanlin97 | 1:c0beadca1617 | 63 | void Wheelchair::right() |
ryanlin97 | 1:c0beadca1617 | 64 | { |
ryanlin97 | 0:fc0c4a184482 | 65 | x->write(def); |
ryanlin97 | 0:fc0c4a184482 | 66 | y->write(high); |
ryanlin97 | 0:fc0c4a184482 | 67 | } |
cpbenite | 6:8cd00c26bb47 | 68 | |
ryanlin97 | 1:c0beadca1617 | 69 | void Wheelchair::left() |
ryanlin97 | 1:c0beadca1617 | 70 | { |
ryanlin97 | 0:fc0c4a184482 | 71 | x->write(def); |
ryanlin97 | 0:fc0c4a184482 | 72 | y->write(low); |
ryanlin97 | 0:fc0c4a184482 | 73 | } |
cpbenite | 6:8cd00c26bb47 | 74 | |
ryanlin97 | 1:c0beadca1617 | 75 | void Wheelchair::stop() |
ryanlin97 | 1:c0beadca1617 | 76 | { |
ryanlin97 | 0:fc0c4a184482 | 77 | x->write(def); |
ryanlin97 | 0:fc0c4a184482 | 78 | y->write(def); |
ryanlin97 | 1:c0beadca1617 | 79 | } |