Rotating to a angle with PID

I need a way to turn to a angle with a motor.
I have a encoder returning angle connected to DIO.

1 Like

Here’s a starting point: Control System Basics — FIRST Robotics Competition documentation

There’s no magic solution here. You’ll have to learn/understand the math/logic that goes into closed-loop control systems to be able to do this well. The complexity will depend on what you’re trying to control (i.e. turning the output shaft of a motor with no load on it will be much simpler than using that motor to turn a long/heavy arm). That said, the math involved in PID control isn’t particularly complex.


Obviously this will depend on ur robot and drive train but we just fixed our turn to angle function for our 2022 robot FRC2022/ at main · Frc5572/FRC2022 · GitHub. this uses a holonomic drive controller to rotate a swerve drive to a certain angle, either absolute or relative.

The concept for a single motor and encoder is the same, using a PID controller and sensor to get u where I want to be.

There are some programming language agnostic training presentations is a sub-directory of this github repository. One of them tals about position control. GitHub - jsimpso81/FRC_Secret_Book_Of_FRC_LabVIEW_2: Secret book of FRC LabVIEW version 2.x

Generally you don’t really need a PID to do position control. The training presents a simple algorithm to set the motor output based on the position error.

1 Like

Here’s some pseudocode I guess
For simple P control

Kp = 0.005 // P gain (may be tuned)
currentPosition Rotation2d.fromDegrees(encoder angle) // Get current encoder angle
desiredPosition = Rotation2d.fromDegrees(desired encoder angle); // Desired encoder angle
error = desiredPosition.minus(currentPosition).getDegrees(); // Calculate error
command = error * Kp // Error times P = P command

What you describe is still PID (when used as a generic term for the superset of P/PD/etc controllers at least). Basing your output on the error (difference between “here” and “there”), scaled linearly, IS proportional closed-loop control (a P-controller).

Unrelated: I’ll be at both of your team’s events in an orange hat. Come say hi!

Additionally, bump for the interactive tuning examples - they’ll help you build an intuition for what the different gains within a PID controller do.


Yes – it is a P only PID controller, but with a characterized error term, a bump added to the output term, and a limited output. i find this easier to teach, implement, and tune than a P only PID. (While I’d have to look again I don’t think the WPILIB implementation of PID has a P term output that doesn’t intersect at 0,0 and an error deadband. This has to be created externally.) Usually after we do this then we move to discussing PID control. New for WPILIB either this year or last is a Function Generator (although they called it something different) that implements a piecewise linear function of a single input. This can be used to implement the position control without writing any new code.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.