Design for Autonomous

Griffin Della Grotte

Special Thanks to Gautam and Krish from 5940

BREAD 5940

Design for Autonomous

About Me

• Advisor, Technical Mentor, and Drive Coach for 5940
• System Engineer with Apple’s Satellite Connectivity Group (SOS via Satellite)
• Security testing enthusiast
• Ex Race Car driver
• New cat owner!

Championship FSAE autocross drive 2018

Einstein 2022

Lovely kitty

Behind the fence-line of some datacenter

BREAD 5940

Design for Autonomous

About this slide deck

Walk through the process of going from nothing to a high-performance autonomous mode.

​

In other words: there’s more to a good auto than fancy code.

BREAD 5940

Design for Autonomous

Game release

How should we think about the autonomous mode? What advantage can we gain?

• Bonus points available only during auto
• Remember it’s about the delta (mostly)
• Getting an early lead
• Are there more?

BREAD 5940

Design for Autonomous

Game release

Short cycle for you, removal of short cycle for your opponent

BREAD 5940

Design for Autonomous

Game release

Good field positioning going into teleop

BREAD 5940

Design for Autonomous

Game release

Get a lead in the tiebreakers

2023 Qualification Ranking

2023 Playoff Tiebreaker

Remember the pre-champs rules!!

Full-grid + Triple = TIE

Clean match = TIE

Better auto???

Unlikely to matter

Without new rules, champs 2023 would have been decided on fouls or auto

BREAD 5940

Design for Autonomous

Field sketches + time estimates

Estimate how much is feasible to do in those 15 seconds. When in doubt, play it out!

​

This should let you know what the most anyone could do in auto will be.

Auto path in Path Planner

2023 Chezy Champs “Human Match”

BREAD 5940

Design for Autonomous

Down-select your options

Which feasible auto routines do we want the most? Which don’t matter much? What do you need to best serve your alliance?

​

​

Quals

• Most matches, not much help
• Maximize score with partners on the field
• Reasonable to work around your partners

Playoffs

• Playing with another very good robot
• Who does what? How do these modes complement each other?

BREAD 5940

Design for Autonomous

Down-select your options (2023)

1. 3 Balance: Help enough to reach RP threshold, other partners can score a preload
2. 3 No Balance over Bump: Playing with someone that has a good smooth side auto
3. 4 Toss + Balance: No help, but also likely win, just trying to reach RP threshold
4. 3 No Balance: Playing with a team that has only a center balance, or balance works well
5. Only Preload: Backup plan in case of mechanical issues or miscommunication
6. 2 Over Charge Station + Balance: Playing with two very good teams

Remember, FRC is a game of alliances. The more flexible you are, the better you can make your whole alliance!

​

BREAD 5940

Design for Autonomous

Down-select your options (2022)

• 5 Piece Right: Lots of points, leaves space for your alliance members
• 2 Piece Left: Secondary if playing with a team with a good 5 piece.
• 3 Piece Steal Left: Gotta get some extra points somewhere
• Only Preload: Backup plan in case of mechanical issues or miscommunication
• 6 Piece: Most points, but too hard on the alliance.

BREAD 5940

Design for Autonomous

Consider your robot architecture

Certain architectures help or hurt your chances.

BREAD 5940

Design for Autonomous

Consider your robot architecture

Certain architectures help or hurt your chances.

BREAD 5940

Design for Autonomous

Consider your robot architecture

Certain architectures help or hurt your chances.

BREAD 5940

Design for Autonomous

Consider your robot architecture

Certain architectures help or hurt your chances.

BREAD 5940

Design for Autonomous

Consider your robot architecture

These choices might make your autos easier to achieve. Are the strictly necessary? Sometimes not, sometimes yes.

BREAD 5940

Design for Autonomous

Implementation

BREAD 5940

Design for Autonomous

Layers of control

Many layers of automation to make a great auto happen!

Every layer needs to do its job...

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Absolute Location PID Control

Robot State Machine Implementation

Robot Localization Fusion

Drivetrain Odometry Reverse Kinematics

AprilTag Detection

Camera Calibration

Effective Wheel Radius Calibration

Autonomous Routine Script

BREAD 5940

Design for Autonomous

Layers of control

Many layers of automation to make a great auto happen!

Every layer needs to do its job...

Absolute Location PID Control

Robot State Machine Implementation

Robot Localization Fusion

Drivetrain Odometry Reverse Kinematics

AprilTag Detection

Camera Calibration

Effective Wheel Radius Calibration

Autonomous Routine Script

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

BREAD 5940

Design for Autonomous

Motor control

Many places to find pre-tuned values for your swerve drive control loops. Other team’s code, module supplier example code, CTRE’s new Swerve wizard.

​

Worth checking when you get driving! Are the swerve modules actually achieving the setpoints they’re given?

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Suggested tests

• Check the commanded position closely matches the achieved position of the swerve rotation while driving.
• Run the robot in an automated forward path some distance. Check the achieved velocity of the modules closely matches the command.

BREAD 5940

Design for Autonomous

Control Loop Options

Many different ways to implement PID control on your motors. A couple notes / recommendations:

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

On TalonFX

• 1000Hz PID for Pos. or Vel.
• Motion Profiling w/ Position PID

On SPARK

• 1000Hz PID for Pos. or Vel.
• Motion Profiling w/ Velocity PID

On Rio

• Main loop at 50Hz, can do ~faster thread
• Arbitrary PID / profile / model

MotionMagic is 👍 and Phoenix 6 is 👍

PID control works! Beware low encoder count on NEOs for position. For motion profiling, use Rio + SPARK’s position PID.

Avoid if you can. Better to avoid sensor latency and offload fast processing. Can often use higher gains with the faster loop speeds.

BREAD 5940

Design for Autonomous

Control Loop Tuning

Depending on the performance you’re looking for, this can be easy or hard. Seconds (or less) count!

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

BREAD 5940

Design for Autonomous

Control Loop Tuning

kG: Add fixed voltage to offset gravity in one direction

kS: Add fixed voltage to offset static friction in the direction of rotation

kV: Add voltage proportional to the desired profile velocity

kA: Add voltage proportional to the desired profile acceleration

kP: Add voltage proportional to the closed-loop error

kI: Add voltage proportional to the accumulated closed-loop error

kD: Add voltage proportional to the rate-of-change of the closed-loop error

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Feedforward

Feedback

Output Voltage = kG + kS * sign(v) + kV * v + kA * a + kP * err + kI * accum-err-sec + kD * err/sec

BREAD 5940

Design for Autonomous

Control Loop Tuning

kG: Add fixed voltage to offset gravity in one direction

kS: Add fixed voltage to offset static friction in the direction of rotation

kV: Add voltage proportional to the desired profile velocity

kA: Add voltage proportional to the desired profile acceleration

kP: Add voltage proportional to the closed-loop error

kI: Add voltage proportional to the accumulated closed-loop error

kD: Add voltage proportional to the rate-of-change of the closed-loop error

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Available PID contents in Phoenix 6 (Units for MotionMagicVoltage)

Volts / rps/s

Volts / Δ error-rotations

Volts

Volts / accumulated-error-rotations

Volts / error-rotations

Volts

Volts / rps

BREAD 5940

Design for Autonomous

Control Loop Tuning (kG & kS)

1. Find your gravity offset (if needed, like for an arm)
1. Increase kG until your mechanism supports its own weight without moving
2. Increase kG more until the mechanism accelerates against gravity
3. Center of that range is kG, half the width of that range is kS.
2. If no gravity offset is needed, can find kS on it’s own in a similar way.

​

Starting point? Do a hand-calc to find the voltage needed to sustain the torque required.

Take care of the input and outputs on these functions. In Phoenix 6, we can choose a couple options. Recommend start with MotionMagicVoltage so the outputs are “voltage compensated” for variable robot battery.

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Available PID contents in Phoenix 6 (Units for MotionMagicVoltage)

Volts / rps/s

Volts / Δ error-rotations

Volts

Volts / accumulated-error-rotations

Volts / error-rotations

Volts

Volts / rps

BREAD 5940

Design for Autonomous

Control Loop Tuning (kV)

• Find the velocity coefficient
• Adjust until the motion profile roughly matches the setpoint

​

The mechanism will not actually drive to its end position at this point, but when traversing the profile, it should be close!

Starting point? Direct translation from motor free speed per volt.

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Available PID contents in Phoenix 6 (Units for MotionMagicVoltage)

​

Volts / rps/s

Volts / Δ error-rotations

Volts

Volts / accumulated-error-rotations

Volts / error-rotations

Volts

Volts / rps

BREAD 5940

Design for Autonomous

Control Loop Tuning (kV)

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Looking to get the cruise portion (the linear increase portion) of the blue line and red line parallel.

BREAD 5940

Design for Autonomous

Control Loop Tuning (kV)

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Line lags behind blue until overshooting at the end, this is OK for now!

BREAD 5940

Design for Autonomous

Control Loop Tuning (kA)

• Find the acceleration coefficient (if needed)
• Add to the coefficient in order to help the mechanism accelerate and decelerate at the start and end of the profile

​

The mechanism will not actually drive to its end position at this point, but when traversing the profile, it should be close!

Many mechanisms will not need this. Only comes into play when you’re really pushing limits. We implemented this manually last year.

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Available PID contents in Phoenix 6 (Units for MotionMagicVoltage)

​

Volts / rps/s

Volts / Δ error-rotations

Volts

Volts / accumulated-error-rotations

Volts / error-rotations

Volts

Volts / rps

BREAD 5940

Design for Autonomous

Control Loop Tuning (kA)

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

kA brings overshoot and lag into check, but some position error remains

BREAD 5940

Design for Autonomous

Control Loop Tuning (kP)

• Find the proportional gain
• Add more P until you see oscillations, back off.
• The control will already look quite good if you did the previous steps well!

Motion profiled control loops can handle higher P terms because the error values ever presented should be smaller.

​

Golden rule of controls: If you know something about the system, tell the controller!

This is the “magic” of motion profiles. The mechanism cannot accelerate instantaneously, or go arbitrarily fast.

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Available PID contents in Phoenix 6 (Units for MotionMagicVoltage)

​

Volts / rps/s

Volts / Δ error-rotations

Volts

Volts / accumulated-error-rotations

Volts / error-rotations

Volts

Volts / rps

BREAD 5940

Design for Autonomous

Control Loop Tuning (kP)

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

kP handles remaining position error

BREAD 5940

Design for Autonomous

Control Loop Tuning (kI & kD)

• Find the I and D terms
• Set them to zero. Done 🙂

Haven’t used a non-zero I term on an FRC mechanism for as long as I can remember. We use D sometimes, but it’s usually for non-profiled PID controllers.

​

Our swerve module drive motors have a kD that is two orders of magnitude lower than the kP. Steer motors are running no kD, and that works just fine.

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Available PID contents in Phoenix 6 (Units for MotionMagicVoltage)

​

Volts / rps/s

Volts / Δ error-rotations

Volts

Volts / accumulated-error-rotations

Volts / error-rotations

Volts

Volts / rps

BREAD 5940

Design for Autonomous

Control Loop Tuning

All of this is done visually, plotting achievement vs. setpoint

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

BREAD 5940

Design for Autonomous

Layers of control

Absolute Location PID Control

Robot State Machine Implementation

Robot Localization Fusion

Drivetrain Odometry Reverse Kinematics

Effective Wheel Radius Calibration

Autonomous Routine Script

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

AprilTag Detection

Camera Calibration

BREAD 5940

Design for Autonomous

Cameras

Lens calibration and positional calibration can be easy to miss. Lens first!

AprilTag Detection

Camera Calibration

Positional calibration

• Start with the true offsets
• Try close to your tags and far from your tags
• Verify the detected camera position (and robot position by proxy) is correct. Adjust if not.
• Camera angle has a large effect only at far distances.
• Bad lens cal → wacky pos. cal

BREAD 5940

Design for Autonomous

Layers of control

Absolute Location PID Control

Robot State Machine Implementation

Robot Localization Fusion

Drivetrain Odometry Reverse Kinematics

AprilTag Detection

Camera Calibration

Effective Wheel Radius Calibration

Autonomous Routine Script

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

BREAD 5940

Design for Autonomous

Carpet Calibration

Determine what the rolling radius of your wheels are

AprilTag Detection

Effective Wheel Radius Calibration

Test Procedure

• Autonomously drive a fixed distance at low acceleration
• Record the distance travelled reported by the sensors
• Measure the true distance travelled
• Calculate rolling radius of wheels

BREAD 5940

Design for Autonomous

Layers of control

Robot State Machine Implementation

Robot Localization Fusion

Drivetrain Odometry Reverse Kinematics

AprilTag Detection

Camera Calibration

Effective Wheel Radius Calibration

Autonomous Routine Script

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Absolute Location PID Control

BREAD 5940

Design for Autonomous

Path Follower PID

Path following usually implemented with a separate PID controller for X, Y, and rotation, yielding velocity commands for the swerve system.

​

We usually just give it as high a P term as can be sustained without inducing oscillations. Look to other team’s code base for their choice!

AprilTag Detection

Absolute Location PID Control

Test Procedure

• Increase P term until jitter occurs during path following
• Do this before adding in vision to the robot localization, as that can also introduce oscillations

BREAD 5940

Design for Autonomous

Layers of control

Absolute Location PID Control

Robot State Machine Implementation

Drivetrain Odometry Reverse Kinematics

AprilTag Detection

Camera Calibration

Effective Wheel Radius Calibration

Autonomous Routine Script

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Robot Localization Fusion

BREAD 5940

Design for Autonomous

Sensor Fusion

Tune how the robot handles combining swerve odometry data with camera measurements. Really just tuning which data is “trusted” more.

​

Can do more scientific measurements on the actual StdDev in your vision measurements, but we got better performance just tweaking it, so we’ll probably skip that part completely now.

AprilTag Detection

Robot Localization Fusion

Test Procedure

• Observe the “jitter” in your pose estimation. More trust in vision will lead to more jitter. Less trust will lead to less accuracy.
• Increase your trust in your vision measurements until the jitter becomes unacceptable for path following

BREAD 5940

Design for Autonomous

Layers of control

Absolute Location PID Control

Robot State Machine Implementation

Robot Localization Fusion

Drivetrain Odometry Reverse Kinematics

AprilTag Detection

Camera Calibration

Effective Wheel Radius Calibration

Autonomous Routine Script

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

BREAD 5940

Design for Autonomous

“Application Code”

Many different ways to script the robot actions that take place during an autonomous routine, see what works best for you.

​

This isn’t “performance” code. You can write the same auto many different ways at this point. Looking for something that is reasonable to read, and reasonable to tweak.

Robot State Machine

Autonomous Routine Script

BREAD 5940

Design for Autonomous

State Machine

Complex robot procedures defined through sequence of states.

​

“Signals” provide input to how the driver or autonomous routine wants the robot to change action

​

Can only transition from certain states into other states

Autonomous Routine Script

Robot State Machine

BREAD 5940

Design for Autonomous

State Machine

Autonomous Routine Script

FLOOR_INTAKE_CONE_A

FLOOR_INTAKE_CONE_B

FLOOR_INTAKE_CONE_C

IDLE

Robot State Machine

BREAD 5940

Design for Autonomous

State Machine

Autonomous Routine Script

FLOOR_INTAKE_CONE_A

FLOOR_INTAKE_CONE_B

FLOOR_INTAKE_CONE_C

IDLE

Start lifting the intake if the current suggests a cone is in it

Transition once the intake is lifted and roller jammed by a cone

Robot State Machine

BREAD 5940

Design for Autonomous

State Machine

Autonomous Routine Script

FLOOR_INTAKE_CONE_A

FLOOR_INTAKE_CONE_B

FLOOR_INTAKE_CONE_C

IDLE

Extend elevator out, rotate arm in

Transition once elevator + arm reaches end of travel

Different arm rotation depending on the position of the elevator

Robot State Machine

BREAD 5940

Design for Autonomous

State Machine

Autonomous Routine Script

FLOOR_INTAKE_CONE_A

FLOOR_INTAKE_CONE_B

FLOOR_INTAKE_CONE_C

IDLE

Start spitting out the cone only once the arm is fully in position

Do not transition unless the tusks signal there is actually a cone in them

Timing logic for how long the tusk beam-break has been triggered

Robot State Machine

BREAD 5940

Design for Autonomous

State Machine

Autonomous Routine Script

FLOOR_INTAKE_CONE_A

FLOOR_INTAKE_CONE_B

FLOOR_INTAKE_CONE_C

IDLE

Different current limit for different gamepiece

Big transition statement out of IDLE

Robot State Machine

BREAD 5940

Design for Autonomous

State Machine

Autonomous Routine Script

FLOOR_INTAKE_CONE_A

FLOOR_INTAKE_CONE_B

FLOOR_INTAKE_CONE_C

IDLE

Robot State Machine

BREAD 5940

Design for Autonomous

Routine Script

Autonomous Routine Script

BREAD 5940

Design for Autonomous

Routine Script

Autonomous Routine Script

Intake a cone while driving out and back

Score a cone high

Intake a cube while driving out and back

Score a cone mid w/ vision

Score cube

Drive to midfield

BREAD 5940

Design for Autonomous

Routine Script

Autonomous Routine Script

BREAD 5940

Design for Autonomous

Layers of control

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Absolute Location PID Control

Robot State Machine Implementation

Robot Localization Fusion

Drivetrain Odometry Reverse Kinematics

AprilTag Detection

Camera Calibration

Effective Wheel Radius Calibration

Autonomous Routine Script

BREAD 5940

Design for Autonomous

Layers of control

Almost all of this can be done before the season starts!

TalonFX Voltage Compensation

Drive/Steer TalonFX PID Control

Absolute Location PID Control

Robot State Machine Implementation

Robot Localization Fusion

Drivetrain Odometry Reverse Kinematics

AprilTag Detection

Camera Calibration

Effective Wheel Radius Calibration

Autonomous Routine Script

BREAD 5940

Design for Autonomous

Strategies for Success

• If it’s possible to do it before the season, do it before the season.
• Think critically about what you should do in auto
• Resist the temptation to move higher in the layer stack.
• Tune graphically. You don’t need a PhD to control a mechanism well.
• If it doesn’t work every time in your shop, it probably won’t work half the time on the field. Test unforgivingly.
• Be skeptical of hardware problems. Code rarely “just stops working” if it wasn’t updated.

BREAD 5940

Design for Autonomous

Discussion!

BREAD 5940

Design for Autonomous