Christian Benitez / Mbed 2 deprecated wheelchaircontrol

Wheelchair control

Dependencies:   BNO055 mbed

Fork of wheelchaircontrol by ryan lin

wheelchair.cpp@6:8cd00c26bb47, 2018-07-17 (annotated)

Committer:
cpbenite
Date:
Tue Jul 17 19:19:26 2018 +0000
Revision:
6:8cd00c26bb47
Parent:
5:e0ccaab3959a 
Wheelchair control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 1:c0beadca1617 2
cpbenite 6:8cd00c26bb47 3 // Note: 'def' is short for 'Default Position'
cpbenite 6:8cd00c26bb47 4
cpbenite 6:8cd00c26bb47 5 // Initialize Wheelchair Pins
cpbenite 6:8cd00c26bb47 6 Wheelchair::Wheelchair (PinName xPin, PinName yPin)
ryanlin97 1:c0beadca1617 7 {
ryanlin97 3:a5e71bfdb492 8 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 9 y = new PwmOut(yPin);
ryanlin97 5:e0ccaab3959a 10 imu = new chair_imu();
ryanlin97 1:c0beadca1617 11 }
cpbenite 6:8cd00c26bb47 12
ryanlin97 3:a5e71bfdb492 13 /*
cpbenite 6:8cd00c26bb47 14 * Joystick has AnalogOut of 200-700, scale values between 1.3 and 3.3
ryanlin97 3:a5e71bfdb492 15 */
cpbenite 6:8cd00c26bb47 16
cpbenite 6:8cd00c26bb47 17 // Convert Joystick Values to Move the Wheelchair
ryanlin97 3:a5e71bfdb492 18 void Wheelchair::move(float x_coor, float y_coor)
ryanlin97 1:c0beadca1617 19 {
cpbenite 6:8cd00c26bb47 20
cpbenite 6:8cd00c26bb47 21 float scaled_x = ((x_coor * 1.6f) + 1.7f) / 3.3f;
cpbenite 6:8cd00c26bb47 22 float scaled_y = (3.3f - (y_coor * 1.6f)) / 3.3f;
ryanlin97 4:29a27953fe70 23 x->write(scaled_x);
cpbenite 6:8cd00c26bb47 24 y->write(scaled_y);
ryanlin97 1:c0beadca1617 25 }
ryanlin97 1:c0beadca1617 26
cpbenite 6:8cd00c26bb47 27 // Turn 90 degrees to the Right
ryanlin97 5:e0ccaab3959a 28 void Wheelchair::turn_right(){
ryanlin97 5:e0ccaab3959a 29 double start = imu->yaw();
ryanlin97 5:e0ccaab3959a 30 double final = start + 90;
ryanlin97 5:e0ccaab3959a 31 if(final > 360)
ryanlin97 5:e0ccaab3959a 32 final -= 360;
ryanlin97 5:e0ccaab3959a 33
ryanlin97 5:e0ccaab3959a 34 while(imu->yaw() <= final) {
ryanlin97 5:e0ccaab3959a 35 Wheelchair::right();
ryanlin97 5:e0ccaab3959a 36 }
ryanlin97 5:e0ccaab3959a 37 }
ryanlin97 5:e0ccaab3959a 38
cpbenite 6:8cd00c26bb47 39 // Turn 90 degrees to the Left
ryanlin97 5:e0ccaab3959a 40 void Wheelchair::turn_left(){
ryanlin97 5:e0ccaab3959a 41 double start = imu->yaw();
ryanlin97 5:e0ccaab3959a 42 double final = start - 90;
ryanlin97 5:e0ccaab3959a 43 if(final <0)
ryanlin97 5:e0ccaab3959a 44 final += 360;
ryanlin97 5:e0ccaab3959a 45
ryanlin97 5:e0ccaab3959a 46 while(imu->yaw() >= final) {
ryanlin97 5:e0ccaab3959a 47 Wheelchair::left();
ryanlin97 5:e0ccaab3959a 48 }
ryanlin97 5:e0ccaab3959a 49 }
cpbenite 6:8cd00c26bb47 50
ryanlin97 1:c0beadca1617 51 void Wheelchair::forward()
ryanlin97 1:c0beadca1617 52 {
ryanlin97 0:fc0c4a184482 53 x->write(high);
ryanlin97 3:a5e71bfdb492 54 y->write(def+offset);
ryanlin97 0:fc0c4a184482 55 }
cpbenite 6:8cd00c26bb47 56
ryanlin97 1:c0beadca1617 57 void Wheelchair::backward()
ryanlin97 1:c0beadca1617 58 {
ryanlin97 0:fc0c4a184482 59 x->write(low);
ryanlin97 0:fc0c4a184482 60 y->write(def);
ryanlin97 0:fc0c4a184482 61 }
cpbenite 6:8cd00c26bb47 62
ryanlin97 1:c0beadca1617 63 void Wheelchair::right()
ryanlin97 1:c0beadca1617 64 {
ryanlin97 0:fc0c4a184482 65 x->write(def);
ryanlin97 0:fc0c4a184482 66 y->write(high);
ryanlin97 0:fc0c4a184482 67 }
cpbenite 6:8cd00c26bb47 68
ryanlin97 1:c0beadca1617 69 void Wheelchair::left()
ryanlin97 1:c0beadca1617 70 {
ryanlin97 0:fc0c4a184482 71 x->write(def);
ryanlin97 0:fc0c4a184482 72 y->write(low);
ryanlin97 0:fc0c4a184482 73 }
cpbenite 6:8cd00c26bb47 74
ryanlin97 1:c0beadca1617 75 void Wheelchair::stop()
ryanlin97 1:c0beadca1617 76 {
ryanlin97 0:fc0c4a184482 77 x->write(def);
ryanlin97 0:fc0c4a184482 78 y->write(def);
ryanlin97 1:c0beadca1617 79 }