Updated with Statistics Library
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Diff: wheelchair.cpp
- Revision:
- 7:5e38d43fbce3
- Parent:
- 6:0cd57bdd8fbc
- Child:
- 8:381a4ec3fef8
diff -r 0cd57bdd8fbc -r 5e38d43fbce3 wheelchair.cpp --- a/wheelchair.cpp Fri Jul 20 17:54:43 2018 +0000 +++ b/wheelchair.cpp Sun Jul 22 06:15:03 2018 +0000 @@ -1,12 +1,15 @@ #include "wheelchair.h" -Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc) +Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time) { x = new PwmOut(xPin); y = new PwmOut(yPin); //imu = new chair_BNO055(); - imu = new chair_MPU9250(pc); - + imu = new chair_MPU9250(pc, time); + imu->setup(); + out = pc; + out->printf("wheelchair setup done \n"); + } /* @@ -51,23 +54,40 @@ y->write(def); } -void Wheelchair::turn_right(Serial out) +void Wheelchair::turn_right() { - bool stop = false; + out->printf("turning right\n"); + double start = imu->yaw(); double final = start + 90; - if(final > 360) + + out->printf("final %f \n", final); + out->printf("start %f \n", start); + if(final > 360) { final -= 360; - - while((imu->yaw() <= final)&& (stop == false)) { - Wheelchair::right(); - out.printf("turning right"); - if( out.readable()) { - out.printf("stopped\n"); - Wheelchair::stop(); - return; + out->printf("final %f \n", final); + while(imu->yaw() - 360 <= 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; + } } } + out->printf("done turning\n"); } void Wheelchair::turn_left()