Week 6

Week 6: Open-Loop and Closed-Loop Control

This week, we'll continue our journey into FRC programming by exploring the fundamental concepts of robot control. We will cover everything from simple open-loop methods to sophisticated closed-loop systems that use sensor feedback for precision and accuracy.

Goals for this Module:

Understand the difference between open-loop and closed-loop control.
Learn about various control methods: duty cycle, voltage, feedforward, and PID.
Understand how to use sensors like encoders and gyros to get feedback.
Implement different drive styles like arcade and tank drive.
Structure control logic using the command-based framework.
Learn systematic methods for tuning controllers.
(Returning Members) Explore advanced topics like motion profiling.

Pre-reading: Advanced Controls Introduction

Start by reading through the WPILib docs “Advanced Controls Introduction” (all articles in that section, not just Control System Basics). You don’t need to fully digest the math or jargon—focus on the ideas behind PID and Feedforward and the tradeoffs of each. Try the interactive models in those pages and play with the tuners.

Choosing a control strategy

We typically choose between three levels of control based on precision and effort:

Simple feedforward (duty cycle or voltage)
- Set a duty cycle or a fixed voltage when you don’t need exact speed/position. Great for intakes or routing wheels where “spin to pull” is enough.
Simple feedback
- Use built-in position/velocity control on your motor controller (e.g., TalonFX) when you want sensible units (RPM, degrees) but can accept modest precision or have an overpowered mechanism.
Combined feedforward + feedback
- Preferred for precise primary mechanisms. Feedforward gets you close based on a model; PID corrects the residual error. For example, our 2023 ElevatorSubsystem added PID output to a WPILib feedforward calculation. WPILib provides feedforward helpers for common mechanisms, so you can avoid most of the math while still getting accuracy.

1. The Building Blocks: Sensors & Commands

Before diving into control theory, let's review the tools we'll be using.

Using Sensors

To have any form of advanced control, we need feedback from the real world.

Encoders: Encoders are sensors that measure the rotation of a shaft. They are built into our motors and can be used to measure distance and velocity.

// In your Drivetrain subsystem, you can get the position and velocity from the motor controllers.

// For SparkMax with NEO motors, the integrated encoder is used by default.

// Get position (in rotations)

double position = leftFront.getEncoder().getPosition();

// Get velocity (in RPM)

double velocity = leftFront.getEncoder().getVelocity();

Gyro: A gyro (or gyroscope) is a sensor that measures the robot's angle of rotation. This is crucial for turning accurately and driving straight. While the roboRIO has a built-in accelerometer, we often use an external gyro like the ADXRS450_Gyro for better performance.

// In your Drivetrain subsystem (using Pigeon2)

private final Pigeon2 gyro = new Pigeon2(0);

public double getAngle() {

return gyro.getYaw().getValue();

}

public void resetGyro() {

gyro.setYaw(0.0);

}

Teleoperated Drivetrain Control

We use the DifferentialDrive class to control our drivetrain during the teleoperated period.

Arcade Drive: differentialDrive.arcadeDrive(fwd, rot)

fwd: The forward/backward speed (-1 to 1).
rot: The rotation speed (-1 to 1).

Tank Drive: differentialDrive.tankDrive(leftSpeed, rightSpeed)

leftSpeed: The speed of the left side of the drivetrain (-1 to 1).
rightSpeed: The speed of the right side of the drivetrain (-1 to 1).

Exercise: Modify your DefaultDrive command to allow the driver to switch between arcade and tank drive using a button on the controller.

Structuring with Commands

The command-based framework provides tools to organize our control logic.

InstantCommand: Executes once and then finishes immediately. Useful for simple actions like resetting a sensor.

RunCommand: Runs a given action continuously. Our DefaultDrive is a RunCommand.

SequentialCommandGroup: Runs a series of commands one after another.

ParallelCommandGroup: Runs a series of commands at the same time.

Example: A command to drive forward for 2 seconds, then turn 90 degrees.

// In RobotContainer.java

Command autoCommand = new SequentialCommandGroup(

new RunCommand(() -> drivetrain.arcadeDrive(0.5, 0), drivetrain).withTimeout(2),

new TurnToAngle(drivetrain, 90) // Assuming you have a TurnToAngle command

);

2. Open-Loop Control: A Simple Start

Open-loop control systems do not use sensor feedback to adjust their output. The robot simply executes the requested action without regard for the environment, assuming it will always behave the same way given the same command. This is simple but not always precise.

Method 1: Duty Cycle & Voltage Control

Duty Cycle: Telling a motor to run at a certain percentage of its maximum speed (from -1.0 to 1.0). The actual speed can vary greatly with battery voltage and mechanism load.
Voltage Control: Commanding a specific voltage (e.g., 6 Volts). This is more repeatable than duty cycle, but its consistency is still affected by the main battery voltage drop.

Implementation Example

You can use these control methods with SparkMax motor controllers using .set() for duty cycle and .setVoltage() for voltage.

import com.revrobotics.CANSparkMax;

import com.revrobotics.CANSparkMaxLowLevel.MotorType;

CANSparkMax motor = new CANSparkMax(1, MotorType.kBrushless);

// Run the motor at 50% speed (duty cycle)

motor.set(0.5);

// Run the motor at 6 volts

motor.setVoltage(6.0);

Method 2: Feedforward Control

Feedforward is a more advanced form of open-loop control. It uses a model of your system to predict the voltage needed to achieve a desired velocity or acceleration. This accounts for physical characteristics like friction.

The Math

The basic formula for a simple motor feedforward is: [ V(t) = k_S \cdot \text{sgn}(\text{vel}(t)) + k_V \cdot \text{vel}(t) + k_A \cdot \text{accel}(t) ]

( V(t) ) is the calculated voltage to apply to the motor.
( k_S ) (Static Gain): The voltage required to overcome static friction.
( k_V ) (Velocity Gain): The voltage needed to sustain a certain velocity.
( k_A ) (Acceleration Gain): The voltage needed to produce a certain acceleration.
( \text{vel}(t) ) and ( \text{accel}(t) ) are the desired velocity and acceleration.

How to Find the Gains

For a simple motor, you can find kS and kV manually by plotting motor velocity at different steady voltages. The y-intercept is kS and the slope is kV. kA is harder to find. For best results, use a characterization tool like WPILib's SysId.

Implementation Example

WPILib provides a SimpleMotorFeedforward class to handle the calculation.

import com.revrobotics.CANSparkMax;

import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.math.controller.SimpleMotorFeedforward;

CANSparkMax motor = new CANSparkMax(1, MotorType.kBrushless);

// The constructor is SimpleMotorFeedforward(kS, kV, kA)

// These values are just examples and must be tuned for your specific robot.

SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.05, 0.1, 0.02);

double desiredVelocity = 5.0; // Desired velocity in meters per second

double desiredAcceleration = 2.0; // Desired acceleration in meters per second squared

double voltage = feedforward.calculate(desiredVelocity, desiredAcceleration);

motor.setVoltage(voltage);

3. Closed-Loop Control: Precision and Adaptability

This is where we use sensors to create a "feedback loop". We tell the robot to go to a certain state (the setpoint), measure its actual state using sensors (the process variable), and use a controller to calculate the error and adjust the motor output to correct for it. This is essential for accuracy and autonomous routines.

What is a PID Controller?

A PID controller calculates an output by summing three terms:

Proportional (P): Proportional to the current error. This does the bulk of the work, driving the mechanism towards the setpoint.
Integral (I): Proportional to the sum of all past errors. It's used to correct for small, steady-state errors that the P term alone can't fix (like fighting gravity on an incline).
Derivative (D): Proportional to the rate of change of the error. It's used to predict future errors and dampen the system's response, preventing overshoot and oscillations.

Track 1 (Rookies): Implementing a Simple PID Controller

WPILib's PIDController class makes this easy. Let's write a command to drive to a specific distance.

// In a new file: commands/DriveDistance.java

public class DriveDistance extends CommandBase {

private final Drivetrain drivetrain;

private final double distance;

private final PIDController pidController;

public DriveDistance(Drivetrain drivetrain, double distance) {

this.drivetrain = drivetrain;

this.distance = distance;

// For now, we'll just use a P term.

this.pidController = new PIDController(1.0, 0.0, 0.0);

addRequirements(drivetrain);

}

@Override

public void initialize() {

drivetrain.resetEncoders();

pidController.setSetpoint(distance);

pidController.setTolerance(0.02); // 2 cm tolerance; tune per robot

}

@Override

public void execute() {

double output = pidController.calculate(drivetrain.getAverageDistance());

drivetrain.arcadeDrive(output, 0);

}

@Override

public boolean isFinished() {

return pidController.atSetpoint();

}

@Override

public void end(boolean interrupted) {

drivetrain.arcadeDrive(0, 0);

}

Tuning a P Controller

Tuning is the process of finding the right P, I, and D values. Let's just focus on P for now.

Set I and D to zero.
Start with a small value for P.
Run your command. If the robot is too slow, increase P.
If the robot overshoots or oscillates, decrease P.
Repeat until you get a fast but stable response.

Pro-tip: Use a data visualization tool like AdvantageScope or Glass to plot the setpoint and the process variable. This makes it much easier to see what's happening.

Track 2 (Returning Members): Advanced Control Systems

Feedforward + Feedback (PID)

The most effective control systems use both.

Feedforward gets you most of the way there based on a model.
Feedback (PID) corrects for any errors the model didn't account for.

When using the Phoenix 6 library, this is easy. The VelocityVoltage and PositionVoltage control requests have a withFeedForward() method, and the PID gains are configured on the motor controller itself.

// Configure PID gains on the motor controller

configs.Slot0.kP = 0.1;

configs.Slot0.kI = 0.0;

configs.Slot0.kD = 0.001;

leftMotor.getConfigurator().apply(configs);

// In your command

double targetVelocity = 10.0; // rps

double feedforwardVoltage = feedforward.calculate(targetVelocity);

// The PID is handled on the TalonFX, so we just provide the target velocity and feedforward

VelocityVoltage request = new VelocityVoltage(targetVelocity).withFeedForward(feedforwardVoltage);

io.setControl(request);

// Tip: Apply motor configuration once in your IO constructor, not every loop.

A Systematic Approach to PID Tuning

A practical approach for FRC:

Tune P: Set I and D to zero. Increase P until you get a steady oscillation around the setpoint. Your final kP is then half of that value.
Tune D: With your new kP, start increasing D to dampen the oscillations. You want to minimize overshoot without making the system sluggish. A good starting point for kD is kP / 10.
Tune I: If you have a steady-state error, start increasing I. Be careful, as too much I can cause instability. A good starting point for kI is very small, like kP / 1000.

Always tune on the real robot, and be prepared to disable it quickly if it becomes unstable!

Motion Profiling & Motion Magic

Instead of just telling a mechanism to "go to position X", motion profiling generates a smooth "profile" of position, velocity, and acceleration over time. The most common type is a trapezoidal motion profile.

The Phoenix 6 library has a built-in feature called Motion Magic. You provide the desired position, and the TalonFX takes care of generating and following the profile.

// Configure Motion Magic parameters in your IO layer

configs.MotionMagic.MotionMagicCruiseVelocity = 15; // rps

configs.MotionMagic.MotionMagicAcceleration = 30; // rps/s

leftMotor.getConfigurator().apply(configs);

// In your command, use a MotionMagicVoltage control request

MotionMagicVoltage request = new MotionMagicVoltage(20.0); // Target position of 20 rotations

io.setControl(request);

You can even combine Motion Magic with a feedforward term for even greater accuracy, especially for mechanisms that have to fight gravity.

Exercises

Complete all interactive WPILib tutorials you explored in the pre-reading and demonstrate tuning one of them to a lead or mentor.

Resources

What's Next?

Next, we'll shift our focus to the tools and processes that are essential for an efficient and successful software team, including advanced Git workflows and debugging techniques.

Page updated

Google Sites

Report abuse