wheelchair code for driver assitance

Dependencies:   mbed

Fork of wheelchairalexa by ryan lin

Committer:
ryanlin97
Date:
Fri Aug 17 03:10:39 2018 +0000
Revision:
12:0e5a0571b497
Parent:
11:75f0f13ff6c1
code for driver assistance;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 11:75f0f13ff6c1 2
ryanlin97 10:e5463c11e0a0 3 bool manual_drive = false;
ryanlin97 11:75f0f13ff6c1 4 volatile float north;
ryanlin97 11:75f0f13ff6c1 5 //volatile double curr_yaw;
ryanlin97 11:75f0f13ff6c1 6 double curr_yaw;
ryanlin97 11:75f0f13ff6c1 7 double Setpoint, Output;
ryanlin97 1:c0beadca1617 8
ryanlin97 11:75f0f13ff6c1 9 void Wheelchair::compass_thread() {
ryanlin97 11:75f0f13ff6c1 10
ryanlin97 11:75f0f13ff6c1 11 }
ryanlin97 11:75f0f13ff6c1 12
ryanlin97 8:381a4ec3fef8 13 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
ryanlin97 1:c0beadca1617 14 {
ryanlin97 3:a5e71bfdb492 15 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 16 y = new PwmOut(yPin);
ryanlin97 11:75f0f13ff6c1 17 //imu = new chair_MPU9250(pc, time);
ryanlin97 11:75f0f13ff6c1 18 Wheelchair::stop();
ryanlin97 7:5e38d43fbce3 19 out = pc;
ryanlin97 7:5e38d43fbce3 20 out->printf("wheelchair setup done \n");
ryanlin97 11:75f0f13ff6c1 21 ti = time;
ryanlin97 1:c0beadca1617 22 }
ryanlin97 6:0cd57bdd8fbc 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 11:75f0f13ff6c1 39 y->write(low);
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 11:75f0f13ff6c1 45 y->write(high);
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 11:75f0f13ff6c1 53 // counter clockwise is -
ryanlin97 11:75f0f13ff6c1 54 // clockwise is +
ryanlin97 11:75f0f13ff6c1 55 double Wheelchair::turn_right(int deg)
ryanlin97 6:0cd57bdd8fbc 56 {
ryanlin97 11:75f0f13ff6c1 57 return;
ryanlin97 6:0cd57bdd8fbc 58 }
ryanlin97 6:0cd57bdd8fbc 59
ryanlin97 11:75f0f13ff6c1 60 double Wheelchair::turn_left(int deg)
ryanlin97 6:0cd57bdd8fbc 61 {
ryanlin97 11:75f0f13ff6c1 62 return;
ryanlin97 6:0cd57bdd8fbc 63 }
ryanlin97 8:381a4ec3fef8 64
ryanlin97 11:75f0f13ff6c1 65 void Wheelchair::turn(int deg)
ryanlin97 11:75f0f13ff6c1 66 {
ryanlin97 11:75f0f13ff6c1 67 return;
ryanlin97 11:75f0f13ff6c1 68 }
ryanlin97 10:e5463c11e0a0 69
ryanlin97 12:0e5a0571b497 70 /*
ryanlin97 12:0e5a0571b497 71 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
ryanlin97 12:0e5a0571b497 72 */
ryanlin97 12:0e5a0571b497 73 void Wheelchair::move(float x_coor, float y_coor, bool leftbound, bool rightbound, bool straightbound)
ryanlin97 12:0e5a0571b497 74 {
ryanlin97 12:0e5a0571b497 75
ryanlin97 12:0e5a0571b497 76
ryanlin97 12:0e5a0571b497 77 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
ryanlin97 12:0e5a0571b497 78 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
ryanlin97 12:0e5a0571b497 79
ryanlin97 12:0e5a0571b497 80 if(leftbound && (scaled_x > 2.5) ) {
ryanlin97 12:0e5a0571b497 81 Wheelchair::stop();
ryanlin97 12:0e5a0571b497 82 }
ryanlin97 12:0e5a0571b497 83
ryanlin97 12:0e5a0571b497 84 else if(rightbound && (scaled_x < 2.5) ) {
ryanlin97 12:0e5a0571b497 85 Wheelchair::stop();
ryanlin97 12:0e5a0571b497 86 }
ryanlin97 12:0e5a0571b497 87
ryanlin97 12:0e5a0571b497 88 else if(straightbound && (scaled_y > 2.5) ) {
ryanlin97 12:0e5a0571b497 89 Wheelchair::stop();
ryanlin97 12:0e5a0571b497 90 }
ryanlin97 12:0e5a0571b497 91
ryanlin97 12:0e5a0571b497 92 else {
ryanlin97 12:0e5a0571b497 93 x->write(scaled_x);
ryanlin97 12:0e5a0571b497 94 y->write(scaled_y);
ryanlin97 12:0e5a0571b497 95 }
ryanlin97 12:0e5a0571b497 96 }
ryanlin97 12:0e5a0571b497 97