Hi, my team’s robot keeps leaning to the right when we test our autonomous pid loop. It also seems to not stop driving. When we change the kI, kP, or kD it doesn’t change the movement of the robot.
Can you show us your entire drivePID() function?
First, I’d recommend having separate functions for turn PID and drive PID, as it makes everything much clearer. That being said, your drive PID looks fine, but your turnError is only dependent on the left and right motor position, which both get reset to 0. The fact that your bot turns at all means there are likely some friction issues in the drivetrain. Since you said it turns right, that means the left side travels further, than the right, so the right side has more friction than the left. As for the algorithm, since you calculate turnError as 0.5(leftMotorPosition - rightMotorPosition) and the left motor always travels more, this means your turnError will always be increasing, which means the power you apply for turning always increases, and since you add it to the left side and subtract it from the right side, this makes the left travel even faster than the right, making the problem worse. You are correct that calculating the change in angle is done by finding \frac{\Delta L-\Delta R}{2}, but you have to subtract that from an arbitrary target.
Also, you never change enableDrivePID in your while loop, so it runs forever
By ‘subtract that by an arbitrary target’ do you mean I need to subtract the (L-R/2) by the setpoint or the error? Or do I need to subtract the change in angle by the setpoint or error? Also what do you mean by ‘never change enableDrivePID’? Do you mean that I need to create a code to disable it after it reaches the setpoint?
You need to subtract it from desiredTurnValue. The point of PID is to bring a system to a desired target, but you can’t actually do that if your calculations aren’t based at all on that target.
Your PID loop is contained inside a while loop with the condition that enableDrivePID is true. But inside the while loop, there is no way for enableDrivePID to be set to false. So you have a while(true) loop that will run forever. If you take my suggestion and separate your turn PID and drive PID, you can simply make the while loop dependent on whether or not your error is 0, or ideally, within a small range of 0. Otherwise, just add something like enableDrivePID = error != 0; at the end of the while loop
So it would be
int turnDifference = (leftMotorPosition-rightMotorPosition)-desiredTurnValue;
No, (leftMotorPosition - rightMotorPosition) / 2) is how much you’ve turned, and you need to subtract that from desiredTurnValue to get your turnError. If you have an inertial sensor though, that will be better at measuring how much you’ve turned