wheelchair code for driver assitance

Dependencies:   mbed

Fork of wheelchairalexa by ryan lin

Committer:
ryanlin97
Date:
Fri Jul 20 17:54:43 2018 +0000
Revision:
6:0cd57bdd8fbc
Parent:
5:e0ccaab3959a
Child:
7:5e38d43fbce3
using mpu9250 imu

Who changed what in which revision?

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