ryan lin / wheelchaircontrolRos

for ros

Dependencies:   QEI chair_BNO055 pid ros_lib_kinetic

Dependents:   wheelchaircontrolrealtimeROS

Fork of wheelchaircontrol by ryan lin

wheelchair.cpp@17:93a636b16b9e, 2018-08-24 (annotated)

Committer:
ryanlin97
Date:
Fri Aug 24 22:41:55 2018 +0000
Revision:
17:93a636b16b9e
Parent:
16:53d912c86c7f
Child:
18:3eadf01ec1b0
for pid turns

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 12:921488918749 5 //volatile double curr_yaw;
ryanlin97 12:921488918749 6 double curr_yaw;
ryanlin97 12:921488918749 7 double Setpoint, Output;
ryanlin97 17:93a636b16b9e 8 PID myPID(&curr_yaw, &Output, &Setpoint, 5.5, 0, .0036, P_ON_E, DIRECT);
ryanlin97 1:c0beadca1617 9
ryanlin97 11:d14a1f7f1297 10 void Wheelchair::compass_thread() {
ryanlin97 11:d14a1f7f1297 11 curr_yaw = imu->yaw();
ryanlin97 11:d14a1f7f1297 12 }
ryanlin97 11:d14a1f7f1297 13
ryanlin97 8:381a4ec3fef8 14 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
ryanlin97 1:c0beadca1617 15 {
ryanlin97 3:a5e71bfdb492 16 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 17 y = new PwmOut(yPin);
ryanlin97 11:d14a1f7f1297 18 imu = new chair_BNO055(pc, time);
ryanlin97 11:d14a1f7f1297 19 //imu = new chair_MPU9250(pc, time);
ryanlin97 11:d14a1f7f1297 20 Wheelchair::stop();
ryanlin97 7:5e38d43fbce3 21 imu->setup();
ryanlin97 7:5e38d43fbce3 22 out = pc;
ryanlin97 7:5e38d43fbce3 23 out->printf("wheelchair setup done \n");
ryanlin97 11:d14a1f7f1297 24 ti = time;
ryanlin97 14:9caca9fde9b0 25 wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate);
ryanlin97 12:921488918749 26 myPID.SetMode(AUTOMATIC);
ryanlin97 1:c0beadca1617 27 }
ryanlin97 6:0cd57bdd8fbc 28
ryanlin97 3:a5e71bfdb492 29 /*
ryanlin97 3:a5e71bfdb492 30 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
ryanlin97 3:a5e71bfdb492 31 */
ryanlin97 3:a5e71bfdb492 32 void Wheelchair::move(float x_coor, float y_coor)
ryanlin97 1:c0beadca1617 33 {
ryanlin97 6:0cd57bdd8fbc 34
ryanlin97 4:29a27953fe70 35 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
ryanlin97 4:29a27953fe70 36 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
ryanlin97 11:d14a1f7f1297 37
ryanlin97 11:d14a1f7f1297 38 // lowPass(scaled_x);
ryanlin97 11:d14a1f7f1297 39 //lowPass(scaled_y);
ryanlin97 11:d14a1f7f1297 40
ryanlin97 4:29a27953fe70 41 x->write(scaled_x);
ryanlin97 4:29a27953fe70 42 y->write(scaled_y);
ryanlin97 11:d14a1f7f1297 43
ryanlin97 11:d14a1f7f1297 44 //out->printf("yaw %f\n", imu->yaw());
ryanlin97 1:c0beadca1617 45
ryanlin97 5:e0ccaab3959a 46 }
ryanlin97 11:d14a1f7f1297 47
ryanlin97 1:c0beadca1617 48 void Wheelchair::forward()
ryanlin97 1:c0beadca1617 49 {
ryanlin97 0:fc0c4a184482 50 x->write(high);
ryanlin97 3:a5e71bfdb492 51 y->write(def+offset);
ryanlin97 0:fc0c4a184482 52 }
ryanlin97 0:fc0c4a184482 53
ryanlin97 1:c0beadca1617 54 void Wheelchair::backward()
ryanlin97 1:c0beadca1617 55 {
ryanlin97 0:fc0c4a184482 56 x->write(low);
ryanlin97 0:fc0c4a184482 57 y->write(def);
ryanlin97 0:fc0c4a184482 58 }
ryanlin97 0:fc0c4a184482 59
ryanlin97 1:c0beadca1617 60 void Wheelchair::right()
ryanlin97 1:c0beadca1617 61 {
ryanlin97 0:fc0c4a184482 62 x->write(def);
ryanlin97 11:d14a1f7f1297 63 y->write(low);
ryanlin97 0:fc0c4a184482 64 }
ryanlin97 0:fc0c4a184482 65
ryanlin97 1:c0beadca1617 66 void Wheelchair::left()
ryanlin97 1:c0beadca1617 67 {
ryanlin97 0:fc0c4a184482 68 x->write(def);
ryanlin97 11:d14a1f7f1297 69 y->write(high);
ryanlin97 0:fc0c4a184482 70 }
ryanlin97 0:fc0c4a184482 71
ryanlin97 1:c0beadca1617 72 void Wheelchair::stop()
ryanlin97 1:c0beadca1617 73 {
ryanlin97 0:fc0c4a184482 74 x->write(def);
ryanlin97 0:fc0c4a184482 75 y->write(def);
ryanlin97 6:0cd57bdd8fbc 76 }
ryanlin97 11:d14a1f7f1297 77 // counter clockwise is -
ryanlin97 11:d14a1f7f1297 78 // clockwise is +
ryanlin97 12:921488918749 79 void Wheelchair::pid_right(int deg)
ryanlin97 12:921488918749 80 {
ryanlin97 17:93a636b16b9e 81 float pid_yaw;
ryanlin97 12:921488918749 82 bool overturn = false;
ryanlin97 12:921488918749 83
ryanlin97 17:93a636b16b9e 84 out->printf("pid right\r\r\n");
ryanlin97 12:921488918749 85 x->write(def);
ryanlin97 12:921488918749 86 Setpoint = curr_yaw + deg;
ryanlin97 17:93a636b16b9e 87 pid_yaw = curr_yaw;
ryanlin97 12:921488918749 88 if(Setpoint > 360) {
ryanlin97 17:93a636b16b9e 89 // Setpoint -= 360;
ryanlin97 12:921488918749 90 overturn = true;
ryanlin97 12:921488918749 91 }
ryanlin97 17:93a636b16b9e 92 myPID.SetTunings(5.5,0, 0.00345);
ryanlin97 16:53d912c86c7f 93 myPID.SetOutputLimits(0, def-low);
ryanlin97 12:921488918749 94 myPID.SetControllerDirection(REVERSE);
ryanlin97 17:93a636b16b9e 95 while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) {
ryanlin97 17:93a636b16b9e 96 if(overturn && curr_yaw < Setpoint-deg-1)
ryanlin97 17:93a636b16b9e 97 {
ryanlin97 17:93a636b16b9e 98 pid_yaw = curr_yaw + 360;
ryanlin97 17:93a636b16b9e 99 }
ryanlin97 17:93a636b16b9e 100 else
ryanlin97 17:93a636b16b9e 101 pid_yaw = curr_yaw;
ryanlin97 16:53d912c86c7f 102 myPID.Compute();
ryanlin97 17:93a636b16b9e 103 double tempor = Output+low;
ryanlin97 17:93a636b16b9e 104 y->write(tempor);
ryanlin97 17:93a636b16b9e 105 out->printf("curr_yaw %f\r\r\n", curr_yaw);
ryanlin97 17:93a636b16b9e 106 out->printf("Setpoint = %f \r\n", Setpoint);
ryanlin97 17:93a636b16b9e 107
ryanlin97 17:93a636b16b9e 108 wait(.05);
ryanlin97 12:921488918749 109 }
ryanlin97 12:921488918749 110 Wheelchair::stop();
ryanlin97 17:93a636b16b9e 111 out->printf("done \r\n");
ryanlin97 12:921488918749 112 }
ryanlin97 12:921488918749 113 void Wheelchair::pid_left(int deg)
ryanlin97 12:921488918749 114 {
ryanlin97 17:93a636b16b9e 115 float pid_yaw;
ryanlin97 12:921488918749 116 bool overturn = false;
ryanlin97 12:921488918749 117
ryanlin97 17:93a636b16b9e 118 out->printf("pid Left\r\r\n");
ryanlin97 12:921488918749 119 x->write(def);
ryanlin97 12:921488918749 120 Setpoint = curr_yaw - deg;
ryanlin97 17:93a636b16b9e 121 pid_yaw = curr_yaw;
ryanlin97 12:921488918749 122 if(Setpoint < 0) {
ryanlin97 17:93a636b16b9e 123 // Setpoint += 360;
ryanlin97 12:921488918749 124 overturn = true;
ryanlin97 12:921488918749 125 }
ryanlin97 17:93a636b16b9e 126 myPID.SetTunings(5,0, 0.004);
ryanlin97 17:93a636b16b9e 127 myPID.SetOutputLimits(0,high-def);
ryanlin97 17:93a636b16b9e 128 myPID.SetControllerDirection(REVERSE);
ryanlin97 17:93a636b16b9e 129 while(pid_yaw > Setpoint + 3){//pid_yaw < Setpoint + 2) {
ryanlin97 17:93a636b16b9e 130 myPID.Compute();
ryanlin97 17:93a636b16b9e 131 if(overturn && curr_yaw > Setpoint+deg+1)
ryanlin97 17:93a636b16b9e 132 {
ryanlin97 17:93a636b16b9e 133 pid_yaw = curr_yaw - 360;
ryanlin97 17:93a636b16b9e 134 }
ryanlin97 17:93a636b16b9e 135 else
ryanlin97 17:93a636b16b9e 136 pid_yaw = curr_yaw;
ryanlin97 17:93a636b16b9e 137 double tempor = Output+def;
ryanlin97 17:93a636b16b9e 138
ryanlin97 17:93a636b16b9e 139 y->write(tempor);
ryanlin97 17:93a636b16b9e 140 out->printf("curr_yaw %f\r\n", curr_yaw);
ryanlin97 17:93a636b16b9e 141 wait(.05);
ryanlin97 12:921488918749 142 }
ryanlin97 17:93a636b16b9e 143 Wheelchair::stop();
ryanlin97 12:921488918749 144 }
ryanlin97 12:921488918749 145
ryanlin97 12:921488918749 146 void Wheelchair::pid_turn(int deg) {
ryanlin97 12:921488918749 147 if(deg > 180) {
ryanlin97 12:921488918749 148 deg -= 360;
ryanlin97 12:921488918749 149 }
ryanlin97 12:921488918749 150
ryanlin97 12:921488918749 151 else if(deg < -180) {
ryanlin97 12:921488918749 152 deg+=360;
ryanlin97 12:921488918749 153 }
ryanlin97 12:921488918749 154
ryanlin97 12:921488918749 155 int turnAmt = abs(deg);
ryanlin97 12:921488918749 156 ti->reset();
ryanlin97 12:921488918749 157
ryanlin97 12:921488918749 158 if(deg >= 0){
ryanlin97 12:921488918749 159 Wheelchair::pid_right(turnAmt);
ryanlin97 12:921488918749 160 }
ryanlin97 12:921488918749 161 else {
ryanlin97 12:921488918749 162 Wheelchair::pid_left(turnAmt);
ryanlin97 12:921488918749 163 }
ryanlin97 12:921488918749 164 }
ryanlin97 12:921488918749 165
ryanlin97 11:d14a1f7f1297 166 double Wheelchair::turn_right(int deg)
ryanlin97 6:0cd57bdd8fbc 167 {
ryanlin97 8:381a4ec3fef8 168 bool overturn = false;
ryanlin97 7:5e38d43fbce3 169 out->printf("turning right\n");
ryanlin97 7:5e38d43fbce3 170
ryanlin97 11:d14a1f7f1297 171 double start = curr_yaw;
ryanlin97 11:d14a1f7f1297 172 double final = start + deg;
ryanlin97 11:d14a1f7f1297 173
ryanlin97 11:d14a1f7f1297 174 if(final > 360) {
ryanlin97 11:d14a1f7f1297 175 final -= 360;
ryanlin97 8:381a4ec3fef8 176 overturn = true;
ryanlin97 8:381a4ec3fef8 177 }
ryanlin97 8:381a4ec3fef8 178
ryanlin97 8:381a4ec3fef8 179 out->printf("start %f, final %f\n", start, final);
ryanlin97 11:d14a1f7f1297 180
ryanlin97 11:d14a1f7f1297 181 double curr = -1;
ryanlin97 12:921488918749 182 while(curr <= final - 30) {
ryanlin97 8:381a4ec3fef8 183 Wheelchair::right();
ryanlin97 8:381a4ec3fef8 184 if( out->readable()) {
ryanlin97 8:381a4ec3fef8 185 out->printf("stopped\n");
ryanlin97 8:381a4ec3fef8 186 Wheelchair::stop();
ryanlin97 8:381a4ec3fef8 187 return;
ryanlin97 7:5e38d43fbce3 188 }
ryanlin97 11:d14a1f7f1297 189 curr = curr_yaw;
ryanlin97 11:d14a1f7f1297 190 if(overturn && curr > (360 - deg) ) {
ryanlin97 11:d14a1f7f1297 191 curr = 0;
ryanlin97 6:0cd57bdd8fbc 192 }
ryanlin97 6:0cd57bdd8fbc 193 }
ryanlin97 11:d14a1f7f1297 194
ryanlin97 11:d14a1f7f1297 195 out->printf("done turning start %f final %f\n", start, final);
ryanlin97 10:e5463c11e0a0 196 Wheelchair::stop();
ryanlin97 11:d14a1f7f1297 197
ryanlin97 12:921488918749 198 //delete me
ryanlin97 12:921488918749 199 wait(5);
ryanlin97 12:921488918749 200
ryanlin97 12:921488918749 201 float correction = final - curr_yaw;
ryanlin97 12:921488918749 202 out->printf("final pos %f actual pos %f\n", final, curr_yaw);
ryanlin97 12:921488918749 203 Wheelchair::turn_left(abs(correction));
ryanlin97 12:921488918749 204 Wheelchair::stop();
ryanlin97 12:921488918749 205
ryanlin97 12:921488918749 206 wait(5);
ryanlin97 12:921488918749 207 out->printf("curr_yaw %f\n", curr_yaw);
ryanlin97 11:d14a1f7f1297 208 return final;
ryanlin97 6:0cd57bdd8fbc 209 }
ryanlin97 6:0cd57bdd8fbc 210
ryanlin97 11:d14a1f7f1297 211 double Wheelchair::turn_left(int deg)
ryanlin97 6:0cd57bdd8fbc 212 {
ryanlin97 8:381a4ec3fef8 213 bool overturn = false;
ryanlin97 8:381a4ec3fef8 214 out->printf("turning left\n");
ryanlin97 8:381a4ec3fef8 215
ryanlin97 12:921488918749 216 double start = curr_yaw;
ryanlin97 11:d14a1f7f1297 217 double final = start - deg;
ryanlin97 8:381a4ec3fef8 218
ryanlin97 11:d14a1f7f1297 219 if(final < 0) {
ryanlin97 11:d14a1f7f1297 220 final += 360;
ryanlin97 8:381a4ec3fef8 221 overturn = true;
ryanlin97 8:381a4ec3fef8 222 }
ryanlin97 8:381a4ec3fef8 223
ryanlin97 11:d14a1f7f1297 224 out->printf("start %f, final %f\n", start, final);
ryanlin97 6:0cd57bdd8fbc 225
ryanlin97 11:d14a1f7f1297 226 double curr = 361;
ryanlin97 11:d14a1f7f1297 227 while(curr >= final) {
ryanlin97 6:0cd57bdd8fbc 228 Wheelchair::left();
ryanlin97 8:381a4ec3fef8 229 if( out->readable()) {
ryanlin97 8:381a4ec3fef8 230 out->printf("stopped\n");
ryanlin97 8:381a4ec3fef8 231 Wheelchair::stop();
ryanlin97 8:381a4ec3fef8 232 return;
ryanlin97 8:381a4ec3fef8 233 }
ryanlin97 11:d14a1f7f1297 234 curr = curr_yaw;
ryanlin97 11:d14a1f7f1297 235
ryanlin97 11:d14a1f7f1297 236 if(overturn && curr >= 0 && curr <= start ) {
ryanlin97 11:d14a1f7f1297 237 curr = 361;
ryanlin97 8:381a4ec3fef8 238 }
ryanlin97 6:0cd57bdd8fbc 239 }
ryanlin97 8:381a4ec3fef8 240
ryanlin97 11:d14a1f7f1297 241 out->printf("done turning start %f final %f\n", start, final);
ryanlin97 10:e5463c11e0a0 242 Wheelchair::stop();
ryanlin97 12:921488918749 243
ryanlin97 17:93a636b16b9e 244
ryanlin97 11:d14a1f7f1297 245 return final;
ryanlin97 11:d14a1f7f1297 246 }
ryanlin97 11:d14a1f7f1297 247
ryanlin97 11:d14a1f7f1297 248 void Wheelchair::turn(int deg)
ryanlin97 11:d14a1f7f1297 249 {
ryanlin97 11:d14a1f7f1297 250 if(deg > 180) {
ryanlin97 11:d14a1f7f1297 251 deg -= 360;
ryanlin97 11:d14a1f7f1297 252 }
ryanlin97 11:d14a1f7f1297 253
ryanlin97 11:d14a1f7f1297 254 else if(deg < -180) {
ryanlin97 11:d14a1f7f1297 255 deg+=360;
ryanlin97 11:d14a1f7f1297 256 }
ryanlin97 11:d14a1f7f1297 257
ryanlin97 11:d14a1f7f1297 258 double finalpos;
ryanlin97 11:d14a1f7f1297 259 int turnAmt = abs(deg);
ryanlin97 17:93a636b16b9e 260
ryanlin97 11:d14a1f7f1297 261
ryanlin97 11:d14a1f7f1297 262 float correction = finalpos - curr_yaw;
ryanlin97 11:d14a1f7f1297 263 out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw);
ryanlin97 11:d14a1f7f1297 264
ryanlin97 11:d14a1f7f1297 265
ryanlin97 11:d14a1f7f1297 266 //if(abs(correction) > turn_precision) {
ryanlin97 11:d14a1f7f1297 267 out->printf("correcting %f\n", correction);
ryanlin97 11:d14a1f7f1297 268 //ti->reset();
ryanlin97 11:d14a1f7f1297 269 Wheelchair::turn_left(curr_yaw - finalpos);
ryanlin97 11:d14a1f7f1297 270 return;
ryanlin97 11:d14a1f7f1297 271 //}
ryanlin97 11:d14a1f7f1297 272
ryanlin97 6:0cd57bdd8fbc 273 }
ryanlin97 8:381a4ec3fef8 274
ryanlin97 12:921488918749 275 float Wheelchair::getDistance() {
ryanlin97 12:921488918749 276 return wheel->getDistance(Diameter);
ryanlin97 12:921488918749 277 }
ryanlin97 12:921488918749 278
ryanlin97 12:921488918749 279 void Wheelchair::resetDistance(){
ryanlin97 12:921488918749 280 wheel->reset();
ryanlin97 12:921488918749 281 }
ryanlin97 10:e5463c11e0a0 282
ryanlin97 12:921488918749 283