for ros
Dependencies: QEI chair_BNO055 pid ros_lib_kinetic
Dependents: wheelchaircontrolrealtimeROS
Fork of wheelchaircontrol by
Diff: wheelchair.cpp
- Revision:
- 17:93a636b16b9e
- Parent:
- 16:53d912c86c7f
- Child:
- 18:3eadf01ec1b0
--- a/wheelchair.cpp Fri Aug 17 20:46:53 2018 +0000 +++ b/wheelchair.cpp Fri Aug 24 22:41:55 2018 +0000 @@ -5,7 +5,7 @@ //volatile double curr_yaw; double curr_yaw; double Setpoint, Output; -//PID myPID(&curr_yaw, &Output, &Setpoint, .1, .1, 5, DIRECT); +PID myPID(&curr_yaw, &Output, &Setpoint, 5.5, 0, .0036, P_ON_E, DIRECT); void Wheelchair::compass_thread() { curr_yaw = imu->yaw(); @@ -78,48 +78,69 @@ // clockwise is + void Wheelchair::pid_right(int deg) { + float pid_yaw; bool overturn = false; - out->printf("pid right\n"); + out->printf("pid right\r\r\n"); x->write(def); Setpoint = curr_yaw + deg; - + pid_yaw = curr_yaw; if(Setpoint > 360) { - Setpoint -= 360; + // Setpoint -= 360; overturn = true; } - + myPID.SetTunings(5.5,0, 0.00345); myPID.SetOutputLimits(0, def-low); myPID.SetControllerDirection(REVERSE); - - while(Setpoint <= curr_yaw){ + while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) { + if(overturn && curr_yaw < Setpoint-deg-1) + { + pid_yaw = curr_yaw + 360; + } + else + pid_yaw = curr_yaw; myPID.Compute(); - y->write(Output+low); - out->printf("curr_yaw %f\n", curr_yaw); - wait(process); + double tempor = Output+low; + y->write(tempor); + out->printf("curr_yaw %f\r\r\n", curr_yaw); + out->printf("Setpoint = %f \r\n", Setpoint); + + wait(.05); } Wheelchair::stop(); - out->printf("done \n"); + out->printf("done \r\n"); } - void Wheelchair::pid_left(int deg) { + float pid_yaw; bool overturn = false; + out->printf("pid Left\r\r\n"); x->write(def); Setpoint = curr_yaw - deg; - + pid_yaw = curr_yaw; if(Setpoint < 0) { - Setpoint += 360; + // 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); + myPID.SetTunings(5,0, 0.004); + myPID.SetOutputLimits(0,high-def); + myPID.SetControllerDirection(REVERSE); + while(pid_yaw > Setpoint + 3){//pid_yaw < Setpoint + 2) { + myPID.Compute(); + if(overturn && curr_yaw > Setpoint+deg+1) + { + pid_yaw = curr_yaw - 360; + } + else + pid_yaw = curr_yaw; + double tempor = Output+def; + + y->write(tempor); + out->printf("curr_yaw %f\r\n", curr_yaw); + wait(.05); } - Wheelchair::stop(); + Wheelchair::stop(); } void Wheelchair::pid_turn(int deg) { @@ -220,14 +241,7 @@ 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; } @@ -243,16 +257,7 @@ 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);