Working file for communication with ROS
Dependencies: QEI2 PID VL53L1X_Filter
wheelchair.cpp@6:0cd57bdd8fbc, 2018-07-20 (annotated)
- Committer:
- ryanlin97
- Date:
- Fri Jul 20 17:54:43 2018 +0000
- Revision:
- 6:0cd57bdd8fbc
- Parent:
- 5:e0ccaab3959a
- Child:
- 7:5e38d43fbce3
using mpu9250 imu
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 | |
ryanlin97 | 6:0cd57bdd8fbc | 3 | Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc) |
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 | 6:0cd57bdd8fbc | 8 | imu = new chair_MPU9250(pc); |
ryanlin97 | 6:0cd57bdd8fbc | 9 | |
ryanlin97 | 1:c0beadca1617 | 10 | } |
ryanlin97 | 6:0cd57bdd8fbc | 11 | |
ryanlin97 | 3:a5e71bfdb492 | 12 | /* |
ryanlin97 | 3:a5e71bfdb492 | 13 | * joystick has analog out of 200-700, scale values between 1.3 and 3.3 |
ryanlin97 | 3:a5e71bfdb492 | 14 | */ |
ryanlin97 | 3:a5e71bfdb492 | 15 | void Wheelchair::move(float x_coor, float y_coor) |
ryanlin97 | 1:c0beadca1617 | 16 | { |
ryanlin97 | 6:0cd57bdd8fbc | 17 | |
ryanlin97 | 4:29a27953fe70 | 18 | float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; |
ryanlin97 | 4:29a27953fe70 | 19 | float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; |
ryanlin97 | 4:29a27953fe70 | 20 | x->write(scaled_x); |
ryanlin97 | 4:29a27953fe70 | 21 | y->write(scaled_y); |
ryanlin97 | 1:c0beadca1617 | 22 | |
ryanlin97 | 5:e0ccaab3959a | 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 | 0:fc0c4a184482 | 39 | y->write(high); |
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 | 0:fc0c4a184482 | 45 | y->write(low); |
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 | 6:0cd57bdd8fbc | 53 | |
ryanlin97 | 6:0cd57bdd8fbc | 54 | void Wheelchair::turn_right(Serial out) |
ryanlin97 | 6:0cd57bdd8fbc | 55 | { |
ryanlin97 | 6:0cd57bdd8fbc | 56 | bool stop = false; |
ryanlin97 | 6:0cd57bdd8fbc | 57 | double start = imu->yaw(); |
ryanlin97 | 6:0cd57bdd8fbc | 58 | double final = start + 90; |
ryanlin97 | 6:0cd57bdd8fbc | 59 | if(final > 360) |
ryanlin97 | 6:0cd57bdd8fbc | 60 | final -= 360; |
ryanlin97 | 6:0cd57bdd8fbc | 61 | |
ryanlin97 | 6:0cd57bdd8fbc | 62 | while((imu->yaw() <= final)&& (stop == false)) { |
ryanlin97 | 6:0cd57bdd8fbc | 63 | Wheelchair::right(); |
ryanlin97 | 6:0cd57bdd8fbc | 64 | out.printf("turning right"); |
ryanlin97 | 6:0cd57bdd8fbc | 65 | if( out.readable()) { |
ryanlin97 | 6:0cd57bdd8fbc | 66 | out.printf("stopped\n"); |
ryanlin97 | 6:0cd57bdd8fbc | 67 | Wheelchair::stop(); |
ryanlin97 | 6:0cd57bdd8fbc | 68 | return; |
ryanlin97 | 6:0cd57bdd8fbc | 69 | } |
ryanlin97 | 6:0cd57bdd8fbc | 70 | } |
ryanlin97 | 6:0cd57bdd8fbc | 71 | } |
ryanlin97 | 6:0cd57bdd8fbc | 72 | |
ryanlin97 | 6:0cd57bdd8fbc | 73 | void Wheelchair::turn_left() |
ryanlin97 | 6:0cd57bdd8fbc | 74 | { |
ryanlin97 | 6:0cd57bdd8fbc | 75 | double start = imu->yaw(); |
ryanlin97 | 6:0cd57bdd8fbc | 76 | double final = start - 90; |
ryanlin97 | 6:0cd57bdd8fbc | 77 | if(final <0) |
ryanlin97 | 6:0cd57bdd8fbc | 78 | final += 360; |
ryanlin97 | 6:0cd57bdd8fbc | 79 | |
ryanlin97 | 6:0cd57bdd8fbc | 80 | while(imu->yaw() >= final) { |
ryanlin97 | 6:0cd57bdd8fbc | 81 | Wheelchair::left(); |
ryanlin97 | 6:0cd57bdd8fbc | 82 | } |
ryanlin97 | 6:0cd57bdd8fbc | 83 | } |