Working file for communication with ROS
Dependencies: QEI2 chair_BNO055 PID VL53L1X_Filter
Dependents: wheelchaircontrolrealRosCom
Diff: wheelchair.cpp
- Revision:
- 8:381a4ec3fef8
- Parent:
- 7:5e38d43fbce3
- Child:
- 10:e5463c11e0a0
diff -r 5e38d43fbce3 -r 381a4ec3fef8 wheelchair.cpp --- a/wheelchair.cpp Sun Jul 22 06:15:03 2018 +0000 +++ b/wheelchair.cpp Sun Jul 22 18:46:24 2018 +0000 @@ -1,6 +1,6 @@ #include "wheelchair.h" -Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time) +Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time ) { x = new PwmOut(xPin); y = new PwmOut(yPin); @@ -53,51 +53,74 @@ x->write(def); y->write(def); } +// counter clockwise is + +// clockwise is - void Wheelchair::turn_right() { + bool overturn = false; out->printf("turning right\n"); double start = imu->yaw(); - double final = start + 90; + double final = start - 90; + + if(final < 0) { + final += 360; + overturn = true; + } + + out->printf("start %f, final %f\n", start, final); - out->printf("final %f \n", final); - out->printf("start %f \n", start); - if(final > 360) { - final -= 360; - out->printf("final %f \n", final); - while(imu->yaw() - 360 <= final) { - Wheelchair::right(); - if( out->readable()) { - out->printf("stopped\n"); - Wheelchair::stop(); - return; - } + double curr = 361; + while(curr >= final) { + Wheelchair::right(); + if( out->readable()) { + out->printf("stopped\n"); + Wheelchair::stop(); + return; } - } - - else { - out->printf("final %f \n", final); - while(imu->yaw() <= final) { - Wheelchair::right(); - if( out->readable()) { - out->printf("stopped\n"); - Wheelchair::stop(); - return; - } + curr = imu->yaw(); + //out->printf("curr %f \n", curr); + + if(overturn && curr >= 0 && curr <= start ) { + curr = 360; } } + out->printf("done turning\n"); + } void Wheelchair::turn_left() { + bool overturn = false; + out->printf("turning left\n"); + double start = imu->yaw(); - double final = start - 90; - if(final <0) - final += 360; + double final = start + 90; + + if(final > 360) { + final -= 360; + overturn = true; + } + +out->printf("start %f, final %f\n", start, final); - while(imu->yaw() >= final) { + double curr = -1; + while(curr <= final) { Wheelchair::left(); + if( out->readable()) { + out->printf("stopped\n"); + Wheelchair::stop(); + return; + } + curr = imu->yaw(); + if(overturn && curr > (360 - 90) ) { + curr = 0; + } + out->printf("curr %f \n", curr); } + + out->printf("done turning\n"); } +