Hi. Currently, I’m implementing a p-loop on my High Stake Mech.
void liftTo(float angle, float liftkP, float liftkD, float liftkI, float integralThresh, float lift_settle_error, float lift_settle_time, float lifttimeout){
liftAngle.setPosition(359, degrees);
Brain.Screen.clearScreen();
PID liftPID(liftAngle.angle(degrees) - angle, liftkP, liftkD, liftkI, integralThresh, lift_settle_error, lift_settle_time, lifttimeout);
while(liftPID.is_settled() == false){
float liftError = liftAngle.angle(degrees) - angle;
printf("liftError: %f \n", liftError);
Brain.Screen.printAt(1, 150, "liftPID: %f", liftPID);
float liftOutput = liftPID.calculate(liftError);
lift.spin(fwd, liftOutput, volt);
task::sleep(10);
}
lift.stop(hold);
Brain.Screen.printAt(1, 170, "Ending Theta: %f", liftAngle.angle(degrees));
Brain.Screen.printAt(1, 200, "Done!");
}
Above it may seem like a PID but i’m only changing the kP variable here while leaving the kI and kD variables as 0.
Here it is in the auton function.
liftTo(340, 0.4, 0, 0, 0, 1.5, 50, 1000);
When I run the code, most of the time, the robot somewhat reaches the target of 340 degrees (I say somewhat as I wasn’t able to fully tune the p-loop yet). YET, there were some instances in which the mech begins to rapidly oscillate at the beginning of the motion to which I’m very worried about as it’s breaking my motor and of course, doesn’t reach the target angle. Please someone help.