Drive to Point
Autonomous Point Navigation with Odometry
Use your swerve drivetrain's odometry to autonomously navigate to specific field coordinates with PID control.
Key Concept: Combine odometry tracking with PID controllers to command your robot to drive to any (x, y, rotation) position on the field.
Drive to point functionality enables your robot to move to precise field positions automatically, forming the foundation for advanced autonomous routines and teleop assists.
Understanding Field Coordinates
The FRC field uses a coordinate system where positions are defined as Pose2d objects containing X position, Y position, and rotation (heading). Your swerve drivetrain's odometry continuously tracks your robot's current pose, allowing you to navigate to any target pose using feedback control.
Pose2d Structure
A Pose2d represents a position and orientation on the field.
Pose2d Components:
PID Control for Position Tracking
To drive to a point, we use three separate PID controllers—one for X position, one for Y position, and one for rotation. Each controller calculates the required velocity by comparing the current value to the target value.
X Controller
Controls forward/backward velocity based on X position error
Y Controller
Controls left/right velocity based on Y position error
Theta Controller
Controls rotation rate based on heading error
Why Three Controllers?
Swerve drivetrains can move in X, Y, and rotate independently. By using separate PID controllers for each degree of freedom, the robot can simultaneously drive to the target position while rotating to the target heading.
DriveToPoint Command Implementation
Create PID Controllers
Initialize three PID controllers with appropriate gains. The theta controller uses continuous input to handle angle wrapping.
Continuous Input
enableContinuousInput(-Math.PI, Math.PI) tells the controller that angles wrap around. This ensures the robot rotates via the shortest path (e.g., from 350° to 10° goes clockwise through 0°, not counterclockwise 340°).
Calculate Velocities in Execute
Each execute cycle, get the current pose and calculate the required velocities to reach the target.
How It Works
The command runs continuously, recalculating velocities every 20ms (50Hz). As the robot gets closer to the target, the error decreases, and the PID controllers automatically reduce the velocity until the robot reaches the setpoint.
Stop When Command Ends
When the command is interrupted or finished, stop the drivetrain to prevent unwanted movement.
Binding to Controller Buttons
Bind the DriveToPoint command to buttons for easy testing and teleop use. Hold the button to drive to the point; release to stop.
Testing Safety & Field Requirements
Start with conservative PID gains (kP = 1-2) and test in a clear area. The robot will move automatically when you press the button. Make sure you have a way to disable the robot quickly if needed.
No full field? No problem! You don't need a full FRC field to test this. You can pick any point just remember to use the robot's starting position as the origin (0, 0, 0°).
Workshop Implementation: DriveToPoint
See the complete implementation in the Workshop-Code repository. The 5-DriveToPoint branch shows the full command structure and button bindings.
Loading file...
Practical Applications
🎮 Teleop Assists
Bind preset positions to buttons to help drivers quickly position the robot:
- •Amp scoring position: Drive to the precise position for scoring in the amp
- •Speaker shooting position: Auto-position for optimal shooting angle
- •Source pickup position: Navigate to game piece source quickly
- •Defensive positions: Move to strategic blocking locations
🤖 Autonomous Routines
Use DriveToPoint as building blocks for autonomous sequences:
This forms the foundation for more complex autonomous navigation before adding vision or PathPlanner.
⚙️ Tuning Tips
Start with low gains
The values provided (kP = 10) are just starting points. Every robot is different! If the robot oscillates, reduce gains. If it's too slow, increase gradually.
Pro Tip: Graph the Target Position vs Actual Position in AdvantageScope to visualize how well your PID controller is tracking.
Theta controller typically needs different gains
Rotation usually requires different tuning than translation. Start with kP around 5-7 for theta.
Add velocity limits for safety
Clamp the output velocities to prevent the robot from moving too fast:
Consider adding tolerance checking
Make the command finish automatically when close enough to the target: