# Turn to angle

We are using a switch statement and telling the robot to turn to angle. When the robot reaches 90 it goes to the next step but robot momentum carries it past 90 by a few degrees. What’s the best way to keep it in that step without adding a long delay so it can correct back to 90 degrees.

My team does this by treating the angle as a set point and using PID to orient to the angle.

Here’s an example from last year that orients the robot to a target detected in our vision system.

3 Likes

You can add a threshold for velocity in addition to position. Also, you might want to look into a Profiled PID controller, so you can constrain the acceleration of the robot.

2 Likes

There are many solutions here. The underlying theme for all of them is “work with how the hardware behaves”.

Setting the threshold to something less than 90 is a quick and dirty way to fix it. It may not be reliable though- depending on how fast the robot was moving when it hits "almost 90”, how quickly you start doing the next thing, and how consistent your mechanical system is, you may still see overshoot or undershoot.

Ramping up and ramping down the motor command near the start and end of the turn is a big gain- it helps make sure the robot isn’t asked to do something physical implausible (instantly stop).

Using some form of closed loop controller would be the optimal solution. The ProfiledPID controller is probably the golden standard for cases like this.

2 Likes

We borrowed this from another team and has worked really well for our swerve drive, https://github.com/Frc5572/FRC2022/blob/main/src/main/java/frc/robot/commands/TurnToAngle.java

First, how are you measuring the 90-degree turn? Do you use encoders, something else, or simply have the robot turn for a set amount of time?

As others have stated, PID is probably the way to go. The advantage of PID control is that if it is properly calibrated, it will not overshoot the target angle.

Here’s a great example of using PID for angle control: