Added more code for side and angled sensor
Dependencies: QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic
Diff: wheelchair.cpp
- Revision:
- 11:d14a1f7f1297
- Parent:
- 10:e5463c11e0a0
- Child:
- 12:921488918749
diff -r e5463c11e0a0 -r d14a1f7f1297 wheelchair.cpp --- a/wheelchair.cpp Mon Jul 23 20:17:37 2018 +0000 +++ b/wheelchair.cpp Wed Aug 01 22:39:22 2018 +0000 @@ -1,15 +1,35 @@ #include "wheelchair.h" + bool manual_drive = false; +volatile float north; +volatile float curr_yaw; +volatile float comp_yn; +void Wheelchair::compass_thread() { + //north = lowPass(imu->angle_north()); + curr_yaw = imu->yaw(); + north = boxcar(imu->angle_north()); + //out->printf("curr_yaw %f\n", curr_yaw); + comp_yn = complement(north,curr_yaw,.5); + out->printf("%f curr_yaw\n", curr_yaw); + out->printf("%f gyroz\n",imu->gyro_z()); + //out->printf("yaw is %f, north is %f, curr_yaw is %f\n", comp_yn, north, curr_yaw); + + //out->printf("Yaw is %f\n", imu->yaw()); + //out->printf("north is %f\n", imu->angle_north()); + } + 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, time); + imu = new chair_BNO055(pc, time); + //imu = new chair_MPU9250(pc, time); + Wheelchair::stop(); imu->setup(); out = pc; out->printf("wheelchair setup done \n"); + ti = time; } @@ -21,10 +41,17 @@ float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; + + // lowPass(scaled_x); + //lowPass(scaled_y); + x->write(scaled_x); y->write(scaled_y); + + //out->printf("yaw %f\n", imu->yaw()); } + void Wheelchair::forward() { x->write(high); @@ -40,13 +67,15 @@ void Wheelchair::right() { x->write(def); - y->write(high); + y->write(low); + //y->write(1.9); + } void Wheelchair::left() { x->write(def); - y->write(low); + y->write(high); } void Wheelchair::stop() @@ -54,76 +83,115 @@ x->write(def); y->write(def); } -// counter clockwise is + -// clockwise is - +// counter clockwise is - +// clockwise is + -void Wheelchair::turn_right() +double Wheelchair::turn_right(int deg) { bool overturn = false; out->printf("turning right\n"); - double start = imu->yaw(); - double final = start - 90; - - if(final < 0) { - final += 360; + double start = curr_yaw; + double final = start + deg; + + if(final > 360) { + final -= 360; overturn = true; } out->printf("start %f, final %f\n", start, final); - - double curr = 361; - while(curr >= final) { + + double curr = -1; + while(curr <= final) { Wheelchair::right(); if( out->readable()) { out->printf("stopped\n"); Wheelchair::stop(); return; } - curr = imu->yaw(); - - - if(overturn && curr >= 0 && curr <= start ) { - curr = 361; + curr = curr_yaw; + if(overturn && curr > (360 - deg) ) { + curr = 0; } } - - out->printf("done turning\n"); + + out->printf("done turning start %f final %f\n", start, final); Wheelchair::stop(); - + + return final; } -void Wheelchair::turn_left() +double Wheelchair::turn_left(int deg) { bool overturn = false; out->printf("turning left\n"); - double start = imu->yaw(); - double final = start + 90; + double start = comp_yn; + double final = start - deg; - if(final > 360) { - final -= 360; + if(final < 0) { + final += 360; overturn = true; } -out->printf("start %f, final %f\n", start, final); + out->printf("start %f, final %f\n", start, final); - double curr = -1; - while(curr <= final) { + double curr = 361; + 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; + curr = curr_yaw; + + if(overturn && curr >= 0 && curr <= start ) { + curr = 361; } } - out->printf("done turning\n"); + out->printf("done turning start %f final %f\n", start, final); Wheelchair::stop(); + + return final; +} + +void Wheelchair::turn(int deg) +{ + if(deg > 180) { + deg -= 360; + } + + else if(deg < -180) { + deg+=360; + } + + double finalpos; + int turnAmt = abs(deg); + //ti->reset(); + /* + if(deg >= 0){ + finalpos = Wheelchair::turn_right(turnAmt); + } + else { + finalpos = Wheelchair::turn_left(turnAmt); + } + */ + wait(2); + + float correction = finalpos - curr_yaw; + out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw); + + + //if(abs(correction) > turn_precision) { + out->printf("correcting %f\n", correction); + //ti->reset(); + Wheelchair::turn_left(curr_yaw - finalpos); + return; + //} + + }