Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

wheelchair.cpp@7:5e38d43fbce3, 2018-07-22 (annotated)

Committer:
ryanlin97
Date:
Sun Jul 22 06:15:03 2018 +0000
Revision:
7:5e38d43fbce3
Parent:
6:0cd57bdd8fbc
Child:
8:381a4ec3fef8
turning

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 1:c0beadca1617 2
ryanlin97 7:5e38d43fbce3 3 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time)
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 7:5e38d43fbce3 8 imu = new chair_MPU9250(pc, time);
ryanlin97 7:5e38d43fbce3 9 imu->setup();
ryanlin97 7:5e38d43fbce3 10 out = pc;
ryanlin97 7:5e38d43fbce3 11 out->printf("wheelchair setup done \n");
ryanlin97 7:5e38d43fbce3 12
ryanlin97 1:c0beadca1617 13 }
ryanlin97 6:0cd57bdd8fbc 14
ryanlin97 3:a5e71bfdb492 15 /*
ryanlin97 3:a5e71bfdb492 16 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
ryanlin97 3:a5e71bfdb492 17 */
ryanlin97 3:a5e71bfdb492 18 void Wheelchair::move(float x_coor, float y_coor)
ryanlin97 1:c0beadca1617 19 {
ryanlin97 6:0cd57bdd8fbc 20
ryanlin97 4:29a27953fe70 21 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
ryanlin97 4:29a27953fe70 22 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
ryanlin97 4:29a27953fe70 23 x->write(scaled_x);
ryanlin97 4:29a27953fe70 24 y->write(scaled_y);
ryanlin97 1:c0beadca1617 25
ryanlin97 5:e0ccaab3959a 26 }
ryanlin97 1:c0beadca1617 27 void Wheelchair::forward()
ryanlin97 1:c0beadca1617 28 {
ryanlin97 0:fc0c4a184482 29 x->write(high);
ryanlin97 3:a5e71bfdb492 30 y->write(def+offset);
ryanlin97 0:fc0c4a184482 31 }
ryanlin97 0:fc0c4a184482 32
ryanlin97 1:c0beadca1617 33 void Wheelchair::backward()
ryanlin97 1:c0beadca1617 34 {
ryanlin97 0:fc0c4a184482 35 x->write(low);
ryanlin97 0:fc0c4a184482 36 y->write(def);
ryanlin97 0:fc0c4a184482 37 }
ryanlin97 0:fc0c4a184482 38
ryanlin97 1:c0beadca1617 39 void Wheelchair::right()
ryanlin97 1:c0beadca1617 40 {
ryanlin97 0:fc0c4a184482 41 x->write(def);
ryanlin97 0:fc0c4a184482 42 y->write(high);
ryanlin97 0:fc0c4a184482 43 }
ryanlin97 0:fc0c4a184482 44
ryanlin97 1:c0beadca1617 45 void Wheelchair::left()
ryanlin97 1:c0beadca1617 46 {
ryanlin97 0:fc0c4a184482 47 x->write(def);
ryanlin97 0:fc0c4a184482 48 y->write(low);
ryanlin97 0:fc0c4a184482 49 }
ryanlin97 0:fc0c4a184482 50
ryanlin97 1:c0beadca1617 51 void Wheelchair::stop()
ryanlin97 1:c0beadca1617 52 {
ryanlin97 0:fc0c4a184482 53 x->write(def);
ryanlin97 0:fc0c4a184482 54 y->write(def);
ryanlin97 6:0cd57bdd8fbc 55 }
ryanlin97 6:0cd57bdd8fbc 56
ryanlin97 7:5e38d43fbce3 57 void Wheelchair::turn_right()
ryanlin97 6:0cd57bdd8fbc 58 {
ryanlin97 7:5e38d43fbce3 59 out->printf("turning right\n");
ryanlin97 7:5e38d43fbce3 60
ryanlin97 6:0cd57bdd8fbc 61 double start = imu->yaw();
ryanlin97 6:0cd57bdd8fbc 62 double final = start + 90;
ryanlin97 7:5e38d43fbce3 63
ryanlin97 7:5e38d43fbce3 64 out->printf("final %f \n", final);
ryanlin97 7:5e38d43fbce3 65 out->printf("start %f \n", start);
ryanlin97 7:5e38d43fbce3 66 if(final > 360) {
ryanlin97 6:0cd57bdd8fbc 67 final -= 360;
ryanlin97 7:5e38d43fbce3 68 out->printf("final %f \n", final);
ryanlin97 7:5e38d43fbce3 69 while(imu->yaw() - 360 <= final) {
ryanlin97 7:5e38d43fbce3 70 Wheelchair::right();
ryanlin97 7:5e38d43fbce3 71 if( out->readable()) {
ryanlin97 7:5e38d43fbce3 72 out->printf("stopped\n");
ryanlin97 7:5e38d43fbce3 73 Wheelchair::stop();
ryanlin97 7:5e38d43fbce3 74 return;
ryanlin97 7:5e38d43fbce3 75 }
ryanlin97 7:5e38d43fbce3 76 }
ryanlin97 7:5e38d43fbce3 77 }
ryanlin97 7:5e38d43fbce3 78
ryanlin97 7:5e38d43fbce3 79 else {
ryanlin97 7:5e38d43fbce3 80 out->printf("final %f \n", final);
ryanlin97 7:5e38d43fbce3 81 while(imu->yaw() <= final) {
ryanlin97 7:5e38d43fbce3 82 Wheelchair::right();
ryanlin97 7:5e38d43fbce3 83 if( out->readable()) {
ryanlin97 7:5e38d43fbce3 84 out->printf("stopped\n");
ryanlin97 7:5e38d43fbce3 85 Wheelchair::stop();
ryanlin97 7:5e38d43fbce3 86 return;
ryanlin97 7:5e38d43fbce3 87 }
ryanlin97 6:0cd57bdd8fbc 88 }
ryanlin97 6:0cd57bdd8fbc 89 }
ryanlin97 7:5e38d43fbce3 90 out->printf("done turning\n");
ryanlin97 6:0cd57bdd8fbc 91 }
ryanlin97 6:0cd57bdd8fbc 92
ryanlin97 6:0cd57bdd8fbc 93 void Wheelchair::turn_left()
ryanlin97 6:0cd57bdd8fbc 94 {
ryanlin97 6:0cd57bdd8fbc 95 double start = imu->yaw();
ryanlin97 6:0cd57bdd8fbc 96 double final = start - 90;
ryanlin97 6:0cd57bdd8fbc 97 if(final <0)
ryanlin97 6:0cd57bdd8fbc 98 final += 360;
ryanlin97 6:0cd57bdd8fbc 99
ryanlin97 6:0cd57bdd8fbc 100 while(imu->yaw() >= final) {
ryanlin97 6:0cd57bdd8fbc 101 Wheelchair::left();
ryanlin97 6:0cd57bdd8fbc 102 }
ryanlin97 6:0cd57bdd8fbc 103 }