Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

wheelchair.cpp@5:e0ccaab3959a, 2018-07-17 (annotated)

Committer:
ryanlin97
Date:
Tue Jul 17 07:19:04 2018 +0000
Revision:
5:e0ccaab3959a
Parent:
4:29a27953fe70
Child:
6:0cd57bdd8fbc
imu added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 1:c0beadca1617 2
ryanlin97 1:c0beadca1617 3 Wheelchair::Wheelchair(PinName xPin, PinName yPin)
ryanlin97 1:c0beadca1617 4 {
ryanlin97 3:a5e71bfdb492 5 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 6 y = new PwmOut(yPin);
ryanlin97 5:e0ccaab3959a 7 imu = new chair_imu();
ryanlin97 1:c0beadca1617 8 }
ryanlin97 3:a5e71bfdb492 9 /*
ryanlin97 3:a5e71bfdb492 10 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
ryanlin97 3:a5e71bfdb492 11 */
ryanlin97 3:a5e71bfdb492 12 void Wheelchair::move(float x_coor, float y_coor)
ryanlin97 1:c0beadca1617 13 {
ryanlin97 5:e0ccaab3959a 14
ryanlin97 4:29a27953fe70 15 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
ryanlin97 4:29a27953fe70 16 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
ryanlin97 4:29a27953fe70 17 x->write(scaled_x);
ryanlin97 4:29a27953fe70 18 y->write(scaled_y);
ryanlin97 3:a5e71bfdb492 19
ryanlin97 1:c0beadca1617 20 }
ryanlin97 1:c0beadca1617 21
ryanlin97 5:e0ccaab3959a 22 void Wheelchair::turn_right(){
ryanlin97 5:e0ccaab3959a 23 double start = imu->yaw();
ryanlin97 5:e0ccaab3959a 24 double final = start + 90;
ryanlin97 5:e0ccaab3959a 25 if(final > 360)
ryanlin97 5:e0ccaab3959a 26 final -= 360;
ryanlin97 5:e0ccaab3959a 27
ryanlin97 5:e0ccaab3959a 28 while(imu->yaw() <= final) {
ryanlin97 5:e0ccaab3959a 29 Wheelchair::right();
ryanlin97 5:e0ccaab3959a 30 }
ryanlin97 5:e0ccaab3959a 31 }
ryanlin97 5:e0ccaab3959a 32
ryanlin97 5:e0ccaab3959a 33 void Wheelchair::turn_left(){
ryanlin97 5:e0ccaab3959a 34 double start = imu->yaw();
ryanlin97 5:e0ccaab3959a 35 double final = start - 90;
ryanlin97 5:e0ccaab3959a 36 if(final <0)
ryanlin97 5:e0ccaab3959a 37 final += 360;
ryanlin97 5:e0ccaab3959a 38
ryanlin97 5:e0ccaab3959a 39 while(imu->yaw() >= final) {
ryanlin97 5:e0ccaab3959a 40 Wheelchair::left();
ryanlin97 5:e0ccaab3959a 41 }
ryanlin97 5:e0ccaab3959a 42 }
ryanlin97 1:c0beadca1617 43 void Wheelchair::forward()
ryanlin97 1:c0beadca1617 44 {
ryanlin97 0:fc0c4a184482 45 x->write(high);
ryanlin97 3:a5e71bfdb492 46 y->write(def+offset);
ryanlin97 0:fc0c4a184482 47 }
ryanlin97 0:fc0c4a184482 48
ryanlin97 1:c0beadca1617 49 void Wheelchair::backward()
ryanlin97 1:c0beadca1617 50 {
ryanlin97 0:fc0c4a184482 51 x->write(low);
ryanlin97 0:fc0c4a184482 52 y->write(def);
ryanlin97 0:fc0c4a184482 53 }
ryanlin97 0:fc0c4a184482 54
ryanlin97 1:c0beadca1617 55 void Wheelchair::right()
ryanlin97 1:c0beadca1617 56 {
ryanlin97 0:fc0c4a184482 57 x->write(def);
ryanlin97 0:fc0c4a184482 58 y->write(high);
ryanlin97 0:fc0c4a184482 59 }
ryanlin97 0:fc0c4a184482 60
ryanlin97 1:c0beadca1617 61 void Wheelchair::left()
ryanlin97 1:c0beadca1617 62 {
ryanlin97 0:fc0c4a184482 63 x->write(def);
ryanlin97 0:fc0c4a184482 64 y->write(low);
ryanlin97 0:fc0c4a184482 65 }
ryanlin97 0:fc0c4a184482 66
ryanlin97 1:c0beadca1617 67 void Wheelchair::stop()
ryanlin97 1:c0beadca1617 68 {
ryanlin97 0:fc0c4a184482 69 x->write(def);
ryanlin97 0:fc0c4a184482 70 y->write(def);
ryanlin97 1:c0beadca1617 71 }