ryan lin / wheelchaircontrolRos

for ros

Dependencies:   QEI chair_BNO055 pid ros_lib_kinetic

Dependents:   wheelchaircontrolrealtimeROS

Fork of wheelchaircontrol by ryan lin

wheelchair.cpp@10:e5463c11e0a0, 2018-07-23 (annotated)

Committer:
ryanlin97
Date:
Mon Jul 23 20:17:37 2018 +0000
Revision:
10:e5463c11e0a0
Parent:
8:381a4ec3fef8
Child:
11:d14a1f7f1297
working with turns

Who changed what in which revision?

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