1
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: wheelchairControlSumer2019
Diff: wheelchair.cpp
- Revision:
- 12:921488918749
- Parent:
- 11:d14a1f7f1297
- Child:
- 14:9caca9fde9b0
diff -r d14a1f7f1297 -r 921488918749 wheelchair.cpp --- a/wheelchair.cpp Wed Aug 01 22:39:22 2018 +0000 +++ b/wheelchair.cpp Thu Aug 09 16:36:46 2018 +0000 @@ -2,21 +2,25 @@ bool manual_drive = false; volatile float north; -volatile float curr_yaw; -volatile float comp_yn; +//volatile double curr_yaw; +double curr_yaw; +double Setpoint, Output; +PID myPID(&curr_yaw, &Output, &Setpoint, .1, .1, 5, DIRECT); void Wheelchair::compass_thread() { + //north = lowPass(imu->angle_north()); + //Input = (double)curr_yaw; 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("curr_yaw %f\n", curr_yaw); + //out->printf("%f\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 ) @@ -30,7 +34,8 @@ out = pc; out->printf("wheelchair setup done \n"); ti = time; - + wheel = new QEI(Encoder1, Encoder1, NC, EncoderReadRate); + myPID.SetMode(AUTOMATIC); } /* @@ -68,8 +73,6 @@ { x->write(def); y->write(low); - //y->write(1.9); - } void Wheelchair::left() @@ -85,7 +88,71 @@ } // counter clockwise is - // clockwise is + +void Wheelchair::pid_right(int deg) +{ + bool overturn = false; + + out->printf("pid right\n"); + x->write(def); + Setpoint = curr_yaw + deg; + + if(Setpoint > 360) { + Setpoint -= 360; + overturn = true; + } + + myPID.SetOutputLimits(low, def); + myPID.SetControllerDirection(REVERSE); + out->printf("setpoint %f curr_yaw %f input %f output %i %i same address\n", Setpoint, curr_yaw, *(myPID.myInput), *(myPID.myOutput), (myPID.myInput==&curr_yaw), (myPID.myOutput == &Output) ); + out->printf("addresses %i %i\n", myPID.myInput, &curr_yaw); + while(myPID.Compute()) { + y->write(Output); + out->printf("curr_yaw %f\n", curr_yaw); + } + Wheelchair::stop(); + out->printf("done \n"); + } +void Wheelchair::pid_left(int deg) +{ + bool overturn = false; + + x->write(def); + Setpoint = curr_yaw - deg; + + if(Setpoint < 0) { + Setpoint += 360; + overturn = true; + } + myPID.SetOutputLimits(def,high); + myPID.SetControllerDirection(DIRECT); + while(myPID.Compute()) { + y->write(Output); + out->printf("curr_yaw %f\n", curr_yaw); + } + Wheelchair::stop(); + } + +void Wheelchair::pid_turn(int deg) { + if(deg > 180) { + deg -= 360; + } + + else if(deg < -180) { + deg+=360; + } + + int turnAmt = abs(deg); + ti->reset(); + + if(deg >= 0){ + Wheelchair::pid_right(turnAmt); + } + else { + Wheelchair::pid_left(turnAmt); + } + } + double Wheelchair::turn_right(int deg) { bool overturn = false; @@ -102,7 +169,7 @@ out->printf("start %f, final %f\n", start, final); double curr = -1; - while(curr <= final) { + while(curr <= final - 30) { Wheelchair::right(); if( out->readable()) { out->printf("stopped\n"); @@ -118,6 +185,16 @@ out->printf("done turning start %f final %f\n", start, final); Wheelchair::stop(); + //delete me + wait(5); + + float correction = final - curr_yaw; + out->printf("final pos %f actual pos %f\n", final, curr_yaw); + Wheelchair::turn_left(abs(correction)); + Wheelchair::stop(); + + wait(5); + out->printf("curr_yaw %f\n", curr_yaw); return final; } @@ -126,7 +203,7 @@ bool overturn = false; out->printf("turning left\n"); - double start = comp_yn; + double start = curr_yaw; double final = start - deg; if(final < 0) { @@ -153,7 +230,15 @@ out->printf("done turning start %f final %f\n", start, final); Wheelchair::stop(); - + + //delete me + wait(2); + /* + float correction = final - curr_yaw; + out->printf("final pos %f actual pos %f\n", final, curr_yaw); + Wheelchair::turn_right(abs(correction)); + Wheelchair::stop(); +*/ return final; } @@ -191,7 +276,14 @@ return; //} - } +float Wheelchair::getDistance() { + return wheel->getDistance(Diameter); + } + +void Wheelchair::resetDistance(){ + wheel->reset(); + } +