Closed-Loop Control Basics

A Closed-Loop Control System in its most basic form is a process that uses feedback to improve the accuracy of its outputs. Closed-Loop Control Systems, sometimes referred to as Feedback Controllers, are frequently used when maintaining or reaching a steady output is important or if the system may have outside influences that could affect the system's output.

A simple example using this type of Control is an automatic coffee maker. In its Closed-Loop Control System, the output is hot coffee and the process we are getting feedback on is the heating of the water. If the coffee maker receives feedback that the water is cold, it will start to heat the pot. When the water is almost hot enough to brew the coffee, the control algorithm will continue to heat the water until the correct goal temperature has been reached. Once the water reaches it's goal temperature, or if it gets too hot, the system will stop heating the water and wait until it receives feedback that the heater needs to begin again.

Closed-Loop Control with SPARK Motor Controllers

Closed-Loop Control is a staple of complex FRC mechanism programming. WPILib offers several libraries to allow teams to run PID loops on the roboRIO, but they require manual setup in your team's code, need additional configuration to run at high frequencies, and may require specifically-configured feedback devices for fast responses.

With a PID loop onboard a SPARK Motor Controller, the setup is simple, doesn't clutter your code, and the loop is updated every 1ms, increasing the responsiveness and precision of the controller. Even when using a more complex control algorithm on the roboRIO, it's still recommended to put as much processing on the motor controller as possible. The PID controller onboard the SPARK can also be configured and tuned with the REV Hardware Client, allowing for a much faster tuning process that doesn't rely on your other subsystems.

Configuring SPARK PID with REVLib can be done in a couple of lines and fits right into the configuration of the motor controller.

SparkFlexConfig config = new SparkFlexConfig()
    .closedLoop.pid(0.01, 0, 0.001);
spark.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);

Setting a setpoint for the PID is just as easy, whether you want to set a position or velocity or even use a motion profile.

SparkClosedLoopController closedLoopController = spark.getClosedLoopController();
closedLoopController.setSetpoint(10, ControlType.kVelocity); // 10 RPM

Both the SPARK MAX and SPARK Flex can operate in several closed-loop control modes, using sensor input to tightly control the motor velocity, position, or current. The internal control loop follows a standard PID algorithm and incorporates several feedforward terms to account for known system dynamics. This allows the motor to follow precise and repeatable motions, useful for complex mechanisms.

Additionally, an arbitrary feedforward signal is added to the output of the control loop after all calculations are done. The units for this signal can be selected as either voltage or duty cycle. This feature allows more advanced feedforward calculations to be performed by the controller. This can be useful for systems with more complex dynamics than can be represented by the SPARK feedforward.

Current Control uses a PID controller to run the motor at a consistent current, providing a consistent torque. The PID controller is run using the setpoint, in Amps, and the internally measured current draw.

It is called as shown below:

Setting up Closed-Loop Control

Closed-loop control in REVLib is accessed through the SPARK's closed loop controller object. This object is specific to each motor and contains all the methods needed to control your motor with closed-loop control. It can be accessed as shown below:

// Initialize the motor (Flex/MAX are setup the same way)
SparkFlex m_motor = 

using namespace rev::spark;

To drive your motor in a closed-loop control mode, address the closed loop controller object and give it a set point (a target in whatever units are required by your control mode: position, velocity, or current) and a control mode as shown below:

The provided example above runs the motor in position control mode, which is just a conventional PID loop reading the motor's current position from the configured encoder and taking a setpoint in rotations.

To run a PID loop, several constants are required. More advanced controllers require additional parameters to be set and tuned.

A PID controller has 3 core parameters or gains. For more information on these gains and how to tune them, see .

These gains can be configured on the with the closedLoop member of a SparkFlexConfigor SparkMaxConfig object as seen below:

There are several Feedforward parameters that can be used to model your system and help support the PID controller, resulting in more precise and consistent motions. These are explained on the .

MAXMotion has parameters that allow you to configure and tune the motion profiles generated by MAXMotion. The parameters can be set through the maxMotion member of the closedLoop config.

The SPARK MAX and SPARK Flex each have 4 closed-loop slots, each with their own set of constants. These slots are numbered 0-3. You can pass the desired as an argument to each of the applicable configurations.

When applying the setpoint, pass the slot number and the motor controller will switch to the appropriate config.

For a detailed technical and mathematical description of each term and its effect, the WPILib docs page on PID is a good resource.

FRC Usage

In FRC, PID loops are used in many types of mechanisms, from flywheel shooters to vertical arms. These need to be tuned to different constants, depending on the units they use and the physical design of the mechanism, however the process to find these constants is roughly the same.

Most teams find success using controllers tuned primarily with P and D, using a Feedforward to account for steady-state error.

P, the proportional gain, is the primary factor of the control loop. This is multiplied by the error and that gain is added to the output. This does the heavy lifting of the motion, pushing the motor in the direction it needs to go.

I, the integral gain, is not often recommended in FRC. It is useful for eliminating steady-state error, or error that the other gains leave behind and cannot address. It accumulates the error over time and multiplies it by the I gain, gradually increasing the power it supplies until that has evened out. If it is needed, it's recommended to use a limited to prevent I windup. For FRC purposes, Feedforward gains are recommended to eliminate steady-state error instead.

The derivative gain, D, is used to tune out oscillation and dampen the motion. It resists motion, decreasing power when the mechanism is moving. A good balance of P and D is needed to make a smooth motion with no oscillation.

Several guides for PID tuning are available, such as . It may be useful to consult multiple, especially those available that reference your specific mechanism.

Any method for PID tuning will start with the same concept, however, regardless of mechanism. Before you can tune your mechanism, you should setup a graph of the setpoint and that measured value, either through the or a similar utility. This will allow you to analyze each test and properly evaluate the changes to make.

To then tune a basic PID loop, follow the steps below:

1. Set all constants (P, I, D, etc) to 0

2. Ensure the mechanism is safe to actuate. This process will spin the motor, potentially at unexpected speeds and in unexpected directions

3. Check the direction of the motor, and invert it if needed so that positive output is in the desired direction

Velocity Control uses the PID controller to run the motor at a set speed in RPM (or configured conversion factor units).

It is called in the same way as Position Control:

m_controller.setSetpoint(setPoint, ControlType.

using namespace rev::spark;

MAXMotion Velocity Control utilizes the MAXMotion parameters to improve upon velocity control. Honoring the maximum acceleration, MAXMotion Velocity Control will speed up your flywheel or rotary mechanism in a controlled way, reducing power draw and increasing consistency.

MAXMotion Velocity Control utilizes an internal velocity closed-loop controller, so transitioning from Velocity Control mode to MAXMotion Velocity Control is as simple as setting a maximum acceleration and changing the setSetpoint call.

It is called as seen below:

m_controller.setSetpoint(setPoint, ControlType.

using namespace rev::spark;

Tips for Smooth Motions

• The Static, Velocity, and Acceleration constants are super helpful in making your motion smooth and consistent. You should be able to get decent performance with only kV/kA and no PID at all

• If your motion seems jittery, try reducing your PID constants, especially P. If the underlying velocity PID outruns the acceleration target, the motion may seem jittery and the velocity will not increase smoothly.

• Make sure your units are correct: maximum velocity is set in RPM by default and maximum acceleration is set in RPM per second by default.

• At low speeds, the acceleration may seem wobbly or inconsistent if the loop has been tuned for higher speeds or vice versa. If both are needed, try tuning separate PIDs and switching between slots when needed. This may be easier than finding those perfect constants that work beautifully across the board.

Position Control is used to point the motor in a specific direction. It takes a setpoint in rotations (or whatever unit your selected encoder's position conversion factor is in) and uses the PID loop to move to that position. The Position control mode pipes directly into a PID controller with the configured encoder.

A properly tuned Position control loop should respond quickly and accurately to a setpoint change and should not oscillate around the target.

To run the motor in Position control mode, set the PID Controller setpoint as shown below.

Default Units

There are two configurable Conversion Factors on each Encoder type that can be used to account for gear ratios and unit conversions in the motor control logic. These are applied independently and the velocity factor does not rely on the position factor, so different units can be used for each.

Positions read from the feedback encoder are multiplied by the Position Conversion Factor before being processed by the closed-loop controller.

Velocities read from the feedback encoder are multiplied by the Velocity Conversion Factor before being processed by the closed-loop controller.

The velocity conversion factor is completely independent of the position conversion factor, so both need to be set to change both units.

All accelerations on the SPARK controllers are in terms of velocity per second, where the velocity is in units specified by the Velocity Conversion Factor.

MAXMotion Position Control is a second-degree closed loop controller, allowing for smooth and consistent motions from one position to another by limiting both the velocity and acceleration of the motor. These can be configured via the MAXMotion Parameters, setting a target acceleration and a "cruise" velocity. The motor will spin up, honoring the acceleration target, hold speed at the cruise velocity, and then slow down honoring the acceleration target to arrive at the setpoint. MAXMotion updates its motion profile every 10ms and the underlying PID controller every 1ms, which makes it extremely fast and responsive.

How it Works

MAXMotion generates a profile containing all the key transition points between the current position and the setpoint and uses that to calculate intermediate positions for the PID controller to follow.

Each point along the profile is a target for the PID controller at the point in time it corresponds to. If, at some point in time, the actual measured position is more than the configured Allowed Profile Error away from the profile, the profile will be regenerated from the current position and velocity. While the mechanism is within that margin, it will continue to track the same profile. This makes tuning easy and makes motions consistent and accurate.

Configuring MAXMotion

Feedforwards

The SPARK Feedforward system was designed with MAXMotion in mind, and MAXMotion can take advantage of all of its features.

The first step of setting up MAXMotion is to configure the PID feedforwards, as explained on . The kV and kA values from a calculator, converted to appropriate units, or from a tool like SysID are perfect starting points for tuning.

There are 3 primary constants to configure for MAXMotion:

• Cruise Velocity: this is the speed you want the motion to hold through the middle of its path

• Maximum Acceleration: this is the acceleration you want to use to speed up and slow down the motion

• Allowed Profile Error: this is the amount of position deviation from the profile that is allowed before the profile is regenerated

1. Ensure the mechanism is free to move and note any mechanical limits

2. Set up the for the mechanism. Note that these may not provide expected results until other values are setup

3. Set P to a very small number, relative to your position units. For the default units, kP = 0.01 is a good starting point. Keep in mind that kP will be multiplied by your position error and then become duty cycle percent output, so pick a "small" value relative to what your position error is expected to be

A well-tuned MAXMotion controller will:

• Track position, velocity, and acceleration closely and accurately

• Respond quickly to a change in setpoint

• Not stutter or reset

• Move smoothly and in a controllable way

After tuning your constants, calling MAXMotion is as simple as passing in the setpoint to the controller.

Smart Motion used a different method for smooth second-degree motion control, but MAXMotion can be applied anywhere Smart Motion was previously. Maximum velocity and acceleration constants may be transferable, but should be tested with caution. All other constants will need re-tuned from scratch, including all PIDs.

MAXMotion has several improvements over Smart Motion, and should offer better consistency, a better tuning experience, better position retention, and an all-around better user experience. It is highly recommended to migrate all systems using Smart Motion to MAXMotion.

Closed loop PID control and MAXMotion motion profiled control are excellent tools for precisely and reactively controlling mechanisms on your robot, but the effectiveness of these tools can be increased further with the introduction of Feed Forward terms. A feed forward (or feedforward) controller is an additional calculation that helps factor system dynamics like gravity and resistance into your closed loop movements, which can be especially helpful on heavy systems.

WPILib offers classes for Feedforward control that behave similarly to the SPARK motor controllers internal calculations and also have an explanation of the math behind DC motor feedforward control. The SPARK feed forward system is designed to drop-in to many of the use cases of these utilities, so much of the information on them is transferable, though you may need to watch your units.

The SPARK feed forward system has the added benefits of directly integrating with MAXMotion, being able to use high feedback frequencies without increased CAN bus traffic or additional configuration, being easy to setup and use, and conserving processing resources on your robot controller.

Feed Forward Constant Quick Reference

For more information on these terms, see their descriptions below

The SPARK Feed Forward system includes 5 terms and one additional constant, each of which apply to some control modes but not others. The compatibility of these is listed in the chart below:

Each term can be set per closed loop slot in the config, as seen below.

The Static Gain is used to counteract any resistance in your motor or mechanism, and is applied in the direction of desired velocity.

To find this value, find the smallest output that causes the mechanism to move slightly, then decrease it slightly so that it doesn't move on it's own, but has no resistance in that direction. See for how to experimentally find this value for an elevator or for the equivalent on an arm. Note that kS is input in Volts.

This should allow the motor/mechanism to move as soon as any other output is applied, eliminating any "dead zone" of output because of resistance. This can be measured using SysID.

The Velocity Gain is used to help your motor and mechanism maintain the desired velocity, and is multiplied by the velocity setpoint. The units are Volts per velocity as measured by the feedback sensor, after the conversion factor. By default, the units are Volts per RPM, prior to any gear ratio.

Many calculators will estimate this in terms of the mechanism's movement, so be sure to account for gear ratios or velocity unit conversions. This can be estimated with a tool like (note the units) or measured with SysID.

The Acceleration Gain is used to accelerate your motor to the desired acceleration, and is multiplied by the acceleration setpoint. The units are Volts per velocity unit per second, with the same caveats on the velocity units as the velocity gain. By default, the units are Volts per RPM per second, prior to any gear ratio.

This can be estimated with a tool like (note the units) or measured with SysID.

The Static Gravity Gain, for elevators and mass moving straight up and down, is simply added to the output and serves to hold the mechanism's position against gravity. The units are Volts.

kG can be can be estimated with a tool like , or it can be measured with SysID.

As kG and kCos are both different types of gravity feedforward gains, they shouldn't be used together. If your mechanism is an elevator, use kG. If your mechanism is an arm, use kCos.

The Cosine Gravity Gain, for arms and mechanisms that fight gravity in a rotary way, is the most complicated but also the most useful of the feed forward gains. It is multiplied by the cosine of the absolute position of your mechanism, which means it pushes the most when the mechanism is horizontal and the least when it's vertical.

This can be easily accomplished by setting up an absolute encoder or limit switch to reset the position of the arm and then using an initialization or homing sequence to zero the position correctly. Once the zero position is set, make sure to also set up the kCosRatio constant to ensure the calculations are done correctly.

The Units are Volts and kCosRatio needs to be set to convert position to absolute mechanism rotations.

This gain can be estimated with a tool like or measured with SysID, but is referred to as kG in these systems and may need unit conversions.

As kCos and kG are both different types of gravity feedforward gains, they shouldn't be used together. If your mechanism is an arm, use kCos. If your mechanism is an elevator, use kG.

Once your arm is zeroed correctly as explained above in the kCos section, the kCosRatio also needs to be configured so that your mechanism's absolute position can be calculated correctly. This ratio should convert from the units of your setpoint (selected feedback sensor's conversion factor) to absolute rotations of your mechanism, and is multiplied by the selected sensor's read position (in units set by your position conversion factor).

This must convert your motor's selected feedback sensor's position into Rotations of the mechanism for the calculation to work.

If your conversion factor is 1 (default), this should simply be any gear reduction between your motor and the actual motion of the arm. If your conversion factor is set, it'll need to be factored into this ratio to properly determine the absolute position of your arm.

For more complex feedforward models, there is also a means of applying an arbitrary voltage which can be calculated in your team code and passed to the API.

It can be applied with the setpoint as seen below: