Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

wheelchair.cpp@11:d14a1f7f1297, 2018-08-01 (annotated)

Committer:
ryanlin97
Date:
Wed Aug 01 22:39:22 2018 +0000
Revision:
11:d14a1f7f1297
Parent:
10:e5463c11e0a0
Child:
12:921488918749
fiddling with the turning to correct itself

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 11:d14a1f7f1297 2
ryanlin97 10:e5463c11e0a0 3 bool manual_drive = false;
ryanlin97 11:d14a1f7f1297 4 volatile float north;
ryanlin97 11:d14a1f7f1297 5 volatile float curr_yaw;
ryanlin97 11:d14a1f7f1297 6 volatile float comp_yn;
ryanlin97 1:c0beadca1617 7
ryanlin97 11:d14a1f7f1297 8 void Wheelchair::compass_thread() {
ryanlin97 11:d14a1f7f1297 9 //north = lowPass(imu->angle_north());
ryanlin97 11:d14a1f7f1297 10 curr_yaw = imu->yaw();
ryanlin97 11:d14a1f7f1297 11 north = boxcar(imu->angle_north());
ryanlin97 11:d14a1f7f1297 12 //out->printf("curr_yaw %f\n", curr_yaw);
ryanlin97 11:d14a1f7f1297 13 comp_yn = complement(north,curr_yaw,.5);
ryanlin97 11:d14a1f7f1297 14 out->printf("%f curr_yaw\n", curr_yaw);
ryanlin97 11:d14a1f7f1297 15 out->printf("%f gyroz\n",imu->gyro_z());
ryanlin97 11:d14a1f7f1297 16 //out->printf("yaw is %f, north is %f, curr_yaw is %f\n", comp_yn, north, curr_yaw);
ryanlin97 11:d14a1f7f1297 17
ryanlin97 11:d14a1f7f1297 18 //out->printf("Yaw is %f\n", imu->yaw());
ryanlin97 11:d14a1f7f1297 19 //out->printf("north is %f\n", imu->angle_north());
ryanlin97 11:d14a1f7f1297 20 }
ryanlin97 11:d14a1f7f1297 21
ryanlin97 8:381a4ec3fef8 22 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
ryanlin97 1:c0beadca1617 23 {
ryanlin97 3:a5e71bfdb492 24 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 25 y = new PwmOut(yPin);
ryanlin97 11:d14a1f7f1297 26 imu = new chair_BNO055(pc, time);
ryanlin97 11:d14a1f7f1297 27 //imu = new chair_MPU9250(pc, time);
ryanlin97 11:d14a1f7f1297 28 Wheelchair::stop();
ryanlin97 7:5e38d43fbce3 29 imu->setup();
ryanlin97 7:5e38d43fbce3 30 out = pc;
ryanlin97 7:5e38d43fbce3 31 out->printf("wheelchair setup done \n");
ryanlin97 11:d14a1f7f1297 32 ti = time;
ryanlin97 7:5e38d43fbce3 33
ryanlin97 1:c0beadca1617 34 }
ryanlin97 6:0cd57bdd8fbc 35
ryanlin97 3:a5e71bfdb492 36 /*
ryanlin97 3:a5e71bfdb492 37 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
ryanlin97 3:a5e71bfdb492 38 */
ryanlin97 3:a5e71bfdb492 39 void Wheelchair::move(float x_coor, float y_coor)
ryanlin97 1:c0beadca1617 40 {
ryanlin97 6:0cd57bdd8fbc 41
ryanlin97 4:29a27953fe70 42 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
ryanlin97 4:29a27953fe70 43 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
ryanlin97 11:d14a1f7f1297 44
ryanlin97 11:d14a1f7f1297 45 // lowPass(scaled_x);
ryanlin97 11:d14a1f7f1297 46 //lowPass(scaled_y);
ryanlin97 11:d14a1f7f1297 47
ryanlin97 4:29a27953fe70 48 x->write(scaled_x);
ryanlin97 4:29a27953fe70 49 y->write(scaled_y);
ryanlin97 11:d14a1f7f1297 50
ryanlin97 11:d14a1f7f1297 51 //out->printf("yaw %f\n", imu->yaw());
ryanlin97 1:c0beadca1617 52
ryanlin97 5:e0ccaab3959a 53 }
ryanlin97 11:d14a1f7f1297 54
ryanlin97 1:c0beadca1617 55 void Wheelchair::forward()
ryanlin97 1:c0beadca1617 56 {
ryanlin97 0:fc0c4a184482 57 x->write(high);
ryanlin97 3:a5e71bfdb492 58 y->write(def+offset);
ryanlin97 0:fc0c4a184482 59 }
ryanlin97 0:fc0c4a184482 60
ryanlin97 1:c0beadca1617 61 void Wheelchair::backward()
ryanlin97 1:c0beadca1617 62 {
ryanlin97 0:fc0c4a184482 63 x->write(low);
ryanlin97 0:fc0c4a184482 64 y->write(def);
ryanlin97 0:fc0c4a184482 65 }
ryanlin97 0:fc0c4a184482 66
ryanlin97 1:c0beadca1617 67 void Wheelchair::right()
ryanlin97 1:c0beadca1617 68 {
ryanlin97 0:fc0c4a184482 69 x->write(def);
ryanlin97 11:d14a1f7f1297 70 y->write(low);
ryanlin97 11:d14a1f7f1297 71 //y->write(1.9);
ryanlin97 11:d14a1f7f1297 72
ryanlin97 0:fc0c4a184482 73 }
ryanlin97 0:fc0c4a184482 74
ryanlin97 1:c0beadca1617 75 void Wheelchair::left()
ryanlin97 1:c0beadca1617 76 {
ryanlin97 0:fc0c4a184482 77 x->write(def);
ryanlin97 11:d14a1f7f1297 78 y->write(high);
ryanlin97 0:fc0c4a184482 79 }
ryanlin97 0:fc0c4a184482 80
ryanlin97 1:c0beadca1617 81 void Wheelchair::stop()
ryanlin97 1:c0beadca1617 82 {
ryanlin97 0:fc0c4a184482 83 x->write(def);
ryanlin97 0:fc0c4a184482 84 y->write(def);
ryanlin97 6:0cd57bdd8fbc 85 }
ryanlin97 11:d14a1f7f1297 86 // counter clockwise is -
ryanlin97 11:d14a1f7f1297 87 // clockwise is +
ryanlin97 6:0cd57bdd8fbc 88
ryanlin97 11:d14a1f7f1297 89 double Wheelchair::turn_right(int deg)
ryanlin97 6:0cd57bdd8fbc 90 {
ryanlin97 8:381a4ec3fef8 91 bool overturn = false;
ryanlin97 7:5e38d43fbce3 92 out->printf("turning right\n");
ryanlin97 7:5e38d43fbce3 93
ryanlin97 11:d14a1f7f1297 94 double start = curr_yaw;
ryanlin97 11:d14a1f7f1297 95 double final = start + deg;
ryanlin97 11:d14a1f7f1297 96
ryanlin97 11:d14a1f7f1297 97 if(final > 360) {
ryanlin97 11:d14a1f7f1297 98 final -= 360;
ryanlin97 8:381a4ec3fef8 99 overturn = true;
ryanlin97 8:381a4ec3fef8 100 }
ryanlin97 8:381a4ec3fef8 101
ryanlin97 8:381a4ec3fef8 102 out->printf("start %f, final %f\n", start, final);
ryanlin97 11:d14a1f7f1297 103
ryanlin97 11:d14a1f7f1297 104 double curr = -1;
ryanlin97 11:d14a1f7f1297 105 while(curr <= final) {
ryanlin97 8:381a4ec3fef8 106 Wheelchair::right();
ryanlin97 8:381a4ec3fef8 107 if( out->readable()) {
ryanlin97 8:381a4ec3fef8 108 out->printf("stopped\n");
ryanlin97 8:381a4ec3fef8 109 Wheelchair::stop();
ryanlin97 8:381a4ec3fef8 110 return;
ryanlin97 7:5e38d43fbce3 111 }
ryanlin97 11:d14a1f7f1297 112 curr = curr_yaw;
ryanlin97 11:d14a1f7f1297 113 if(overturn && curr > (360 - deg) ) {
ryanlin97 11:d14a1f7f1297 114 curr = 0;
ryanlin97 6:0cd57bdd8fbc 115 }
ryanlin97 6:0cd57bdd8fbc 116 }
ryanlin97 11:d14a1f7f1297 117
ryanlin97 11:d14a1f7f1297 118 out->printf("done turning start %f final %f\n", start, final);
ryanlin97 10:e5463c11e0a0 119 Wheelchair::stop();
ryanlin97 11:d14a1f7f1297 120
ryanlin97 11:d14a1f7f1297 121 return final;
ryanlin97 6:0cd57bdd8fbc 122 }
ryanlin97 6:0cd57bdd8fbc 123
ryanlin97 11:d14a1f7f1297 124 double Wheelchair::turn_left(int deg)
ryanlin97 6:0cd57bdd8fbc 125 {
ryanlin97 8:381a4ec3fef8 126 bool overturn = false;
ryanlin97 8:381a4ec3fef8 127 out->printf("turning left\n");
ryanlin97 8:381a4ec3fef8 128
ryanlin97 11:d14a1f7f1297 129 double start = comp_yn;
ryanlin97 11:d14a1f7f1297 130 double final = start - deg;
ryanlin97 8:381a4ec3fef8 131
ryanlin97 11:d14a1f7f1297 132 if(final < 0) {
ryanlin97 11:d14a1f7f1297 133 final += 360;
ryanlin97 8:381a4ec3fef8 134 overturn = true;
ryanlin97 8:381a4ec3fef8 135 }
ryanlin97 8:381a4ec3fef8 136
ryanlin97 11:d14a1f7f1297 137 out->printf("start %f, final %f\n", start, final);
ryanlin97 6:0cd57bdd8fbc 138
ryanlin97 11:d14a1f7f1297 139 double curr = 361;
ryanlin97 11:d14a1f7f1297 140 while(curr >= final) {
ryanlin97 6:0cd57bdd8fbc 141 Wheelchair::left();
ryanlin97 8:381a4ec3fef8 142 if( out->readable()) {
ryanlin97 8:381a4ec3fef8 143 out->printf("stopped\n");
ryanlin97 8:381a4ec3fef8 144 Wheelchair::stop();
ryanlin97 8:381a4ec3fef8 145 return;
ryanlin97 8:381a4ec3fef8 146 }
ryanlin97 11:d14a1f7f1297 147 curr = curr_yaw;
ryanlin97 11:d14a1f7f1297 148
ryanlin97 11:d14a1f7f1297 149 if(overturn && curr >= 0 && curr <= start ) {
ryanlin97 11:d14a1f7f1297 150 curr = 361;
ryanlin97 8:381a4ec3fef8 151 }
ryanlin97 6:0cd57bdd8fbc 152 }
ryanlin97 8:381a4ec3fef8 153
ryanlin97 11:d14a1f7f1297 154 out->printf("done turning start %f final %f\n", start, final);
ryanlin97 10:e5463c11e0a0 155 Wheelchair::stop();
ryanlin97 11:d14a1f7f1297 156
ryanlin97 11:d14a1f7f1297 157 return final;
ryanlin97 11:d14a1f7f1297 158 }
ryanlin97 11:d14a1f7f1297 159
ryanlin97 11:d14a1f7f1297 160 void Wheelchair::turn(int deg)
ryanlin97 11:d14a1f7f1297 161 {
ryanlin97 11:d14a1f7f1297 162 if(deg > 180) {
ryanlin97 11:d14a1f7f1297 163 deg -= 360;
ryanlin97 11:d14a1f7f1297 164 }
ryanlin97 11:d14a1f7f1297 165
ryanlin97 11:d14a1f7f1297 166 else if(deg < -180) {
ryanlin97 11:d14a1f7f1297 167 deg+=360;
ryanlin97 11:d14a1f7f1297 168 }
ryanlin97 11:d14a1f7f1297 169
ryanlin97 11:d14a1f7f1297 170 double finalpos;
ryanlin97 11:d14a1f7f1297 171 int turnAmt = abs(deg);
ryanlin97 11:d14a1f7f1297 172 //ti->reset();
ryanlin97 11:d14a1f7f1297 173 /*
ryanlin97 11:d14a1f7f1297 174 if(deg >= 0){
ryanlin97 11:d14a1f7f1297 175 finalpos = Wheelchair::turn_right(turnAmt);
ryanlin97 11:d14a1f7f1297 176 }
ryanlin97 11:d14a1f7f1297 177 else {
ryanlin97 11:d14a1f7f1297 178 finalpos = Wheelchair::turn_left(turnAmt);
ryanlin97 11:d14a1f7f1297 179 }
ryanlin97 11:d14a1f7f1297 180 */
ryanlin97 11:d14a1f7f1297 181 wait(2);
ryanlin97 11:d14a1f7f1297 182
ryanlin97 11:d14a1f7f1297 183 float correction = finalpos - curr_yaw;
ryanlin97 11:d14a1f7f1297 184 out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw);
ryanlin97 11:d14a1f7f1297 185
ryanlin97 11:d14a1f7f1297 186
ryanlin97 11:d14a1f7f1297 187 //if(abs(correction) > turn_precision) {
ryanlin97 11:d14a1f7f1297 188 out->printf("correcting %f\n", correction);
ryanlin97 11:d14a1f7f1297 189 //ti->reset();
ryanlin97 11:d14a1f7f1297 190 Wheelchair::turn_left(curr_yaw - finalpos);
ryanlin97 11:d14a1f7f1297 191 return;
ryanlin97 11:d14a1f7f1297 192 //}
ryanlin97 11:d14a1f7f1297 193
ryanlin97 11:d14a1f7f1297 194
ryanlin97 6:0cd57bdd8fbc 195 }
ryanlin97 8:381a4ec3fef8 196
ryanlin97 10:e5463c11e0a0 197