Advanced: Profiled Drive to Point
Profiled Path Following with Feedforward Control
The advanced DriveToPoint command combines motion profiling with feedforward control to achieve smooth, predictable autonomous movement. Instead of relying solely on PID feedback, this approach plans a velocity profile and proactively applies the forces needed to follow it.
Key Concept: Feedforward control anticipates what forces are needed based on the planned trajectory, while feedback (PID) handles small corrections for disturbances. Together, they create smooth, accurate autonomous navigation.
Motion Profiling Fundamentals
What is Motion Profiling?
Motion profiling generates smooth trajectories that respect your robot's physical capabilities. Instead of jerky movements from instant velocity changes, profiled paths smoothly accelerate, cruise at maximum speed, then decelerate to the target.
ā” Acceleration Phase
Ramps up from current velocity to maximum velocity using controlled acceleration (4 m/sĀ²)
š Cruise Phase
Maintains maximum velocity (4.3 m/s) for the bulk of the journey when possible
šÆ Deceleration Phase
Smoothly brakes to reach the target position with zero velocity
Key Constraints
- Max Velocity: 4.3 m/s (translation), Ļ rad/s (rotation)
- Max Acceleration: 4 m/sĀ² (translation), 2Ļ rad/sĀ² (rotation)
- Benefits: Prevents wheel slip, reduces mechanical stress, more predictable behavior
Tuning Max Velocity and Acceleration
Finding the Right Constraints
Setting appropriate velocity and acceleration constraints is crucial for reliable path following. These values need to be conservative enough that your robot can actually track them while still being fast enough for competitive performance.
šļø Maximum Velocity (kMaxV)
Set to ~90% of your robot's maximum speed
Why not 100%? This headroom allows your feedback controller to "catch up" if the robot falls behind the planned trajectory. Without this margin, you can't correct tracking errors.
Example: If max speed is 4.8 m/s
Set kMaxV = 4.3 m/s (90%)
ā” Maximum Acceleration (kMaxAccel)
Set to a value that gets you close to max speed in ~1 second
Start with kMaxV as your initial guess, then tune by graphing actual velocity against target velocity. Reduce if your robot can't track the trajectory.
Example: kMaxV = 4.3 m/s
Start with kMaxAccel = 4.0 m/sĀ²
š Validation Method: Graph Velocity Tracking
The best way to validate your constraints is to log and graph your robot's actual velocity against the planned trajectory velocity:
- Enable logging for both
setpoint.speeds(planned) and actual chassis speeds - Run your DriveToPoint command and capture data using AdvantageScope
- Graph both velocities over time - they should track closely
- If actual velocity lags significantly behind planned velocity, reduce kMaxAccel
- If actual velocity consistently exceeds planned velocity, you have headroom to increase constraints
Goal: Actual velocity should closely follow planned velocity with minimal lag
Feedforward vs Feedback Control
Understanding the Dual Control Strategy
The basic DriveToPoint command uses only feedback control (PID) - it measures error and reacts to it. The advanced version adds feedforward control - it anticipates what forces are needed before errors occur.
| Aspect | Basic (Feedback Only) | Advanced (Feedforward + Feedback) |
|---|---|---|
| Control Type | Reactive - responds after error occurs | Proactive + Reactive - anticipates needs and corrects disturbances |
| Path Planning | None - direct to target | Generates smooth trajectory with velocity profiles |
| PID Target | Final target position (large errors) | Moving setpoint on trajectory (small errors) |
| Force Application | Only from PID corrections | Planned forces + PID corrections |
| Motion Quality | Can be jerky, oscillation prone | Smooth, predictable, minimal oscillation |
š Car Driving Analogy
Feedback Only: Like driving by only looking at lane lines and correcting when you drift. You're always reacting to errors after they happen.
Feedforward: Like knowing you need to turn the steering wheel before the curve based on the road ahead. You're anticipating what's needed.
Combined Approach: You plan your steering based on the road ahead (feedforward), but still make small corrections if wind pushes you off course (feedback).
Setpoint Tracking
Following a Trajectory vs Targeting an Endpoint
The key innovation in the advanced approach is using moving setpoints from the trajectory instead of just the final target position.
ā Basic Approach
ā Advanced Approach
Why This Matters
- ā Smaller Errors: PID only needs to keep robot on the planned path, not correct for entire distance to target
- ā Smoother Control: Setpoint moves gradually along trajectory, creating gentle corrections
- ā Less Oscillation: Smaller errors mean less aggressive PID corrections and overshoot
- ā Predictable Behavior: Robot follows a planned path instead of taking unpredictable routes
Wheel Force Feedforwards
Understanding Force-Based Control
The advanced DriveToPoint calculates specific forces to apply to each swerve module. This allows the robot to anticipate and execute the accelerations needed to follow the planned trajectory.
What are Wheel Forces?
When you command a swerve drivetrain to accelerate or turn, each individual wheel module needs to apply specific forces. The WheelForceCalculator determines exactly how much force each wheel should generate based on:
- ā¢ The desired change in velocity (from trajectory)
- ā¢ Robot's mass and moment of inertia (MOI)
- ā¢ Each wheel's position relative to robot center
- ā¢ Required acceleration to stay on trajectory
How It Works
- Calculate velocity change needed (current setpoint - previous setpoint)
- Divide by time step (dt) to get required acceleration
- Account for robot mass and moment of inertia (MOI parameter)
- Distribute forces across all wheel modules
- Return X and Y force arrays for each wheel
Phoenix 6 Integration
CTRE's Phoenix 6 API accepts force feedforwards directly:
š” Tuning: The MOI Parameter
The moment of inertia (MOI) parameter represents your robot's resistance to rotation. Higher MOI means more force is needed for rotational changes.
For guidance on estimating your robot's MOI, see Choreo's MOI estimation guide.
Workshop Implementation
Profiled DriveToPoint Code
View the complete implementation in the workshop code repository. The pull request shows the profiled path following with feedforward control.
Loading file...
šÆ When to Use Profiled Paths
ā Best Use Cases
- ā¢ Autonomous Routines: Smooth, predictable movement without overshoot
- ā¢ High-Speed Navigation: Safely reach maximum velocity without wheel slip
- ā¢ Precise Positioning: Minimal oscillation when reaching scoring positions
- ā¢ Teleop Assists: Drive-to-position helpers that feel smooth to drivers
ā ļø When Basic Might Be Enough
- ā¢ Short Distances: Quick corrections where acceleration phase is minimal
- ā¢ Learning/Testing: Understanding PID fundamentals before adding complexity
- ā¢ Simple Teleop Assists: Driver-controlled movements with minimal autonomy