for ros

Dependencies:   QEI chair_BNO055 pid ros_lib_kinetic

Dependents:   wheelchaircontrolrealtimeROS

Fork of wheelchaircontrol by ryan lin

Committer:
ryanlin97
Date:
Thu Aug 09 16:36:46 2018 +0000
Revision:
12:921488918749
Parent:
11:d14a1f7f1297
Child:
14:9caca9fde9b0
incorporated pid and encoder;

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