55import com .pathplanner .lib .util .HolonomicPathFollowerConfig ;
66import com .pathplanner .lib .util .PathPlannerLogging ;
77import com .pathplanner .lib .util .ReplanningConfig ;
8+ import edu .wpi .first .math .VecBuilder ;
9+ import edu .wpi .first .math .estimator .SwerveDrivePoseEstimator ;
810import edu .wpi .first .math .geometry .Pose2d ;
911import edu .wpi .first .math .geometry .Rotation2d ;
1012import edu .wpi .first .math .geometry .Translation2d ;
11- import edu .wpi .first .math .geometry .Twist2d ;
1213import edu .wpi .first .math .kinematics .ChassisSpeeds ;
1314import edu .wpi .first .math .kinematics .SwerveDriveKinematics ;
1415import edu .wpi .first .math .kinematics .SwerveModulePosition ;
1819import org .team1540 .robot2024 .util .LocalADStarAK ;
1920import org .littletonrobotics .junction .AutoLogOutput ;
2021import org .littletonrobotics .junction .Logger ;
22+ import org .team1540 .robot2024 .util .vision .TimestampedVisionPose ;
2123
2224import static org .team1540 .robot2024 .Constants .Drivetrain .*;
2325
@@ -27,10 +29,11 @@ public class Drivetrain extends SubsystemBase {
2729 private final Module [] modules = new Module [4 ]; // FL, FR, BL, BR
2830
2931 private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics (getModuleTranslations ());
30- private Pose2d pose = new Pose2d ();
31- private Rotation2d lastGyroRotation = new Rotation2d ();
32+ private Rotation2d rawGyroRotation = new Rotation2d ();
3233 private boolean forceModuleAngleChange = false ;
3334
35+ private final SwerveDrivePoseEstimator poseEstimator ;
36+
3437 public Drivetrain (
3538 GyroIO gyroIO ,
3639 ModuleIO flModuleIO ,
@@ -43,6 +46,15 @@ public Drivetrain(
4346 modules [2 ] = new Module (blModuleIO , 2 );
4447 modules [3 ] = new Module (brModuleIO , 3 );
4548
49+ poseEstimator = new SwerveDrivePoseEstimator (
50+ kinematics ,
51+ rawGyroRotation ,
52+ getModulePositions (),
53+ new Pose2d (),
54+ // TODO: tune std devs (scale vision by distance?)
55+ VecBuilder .fill (0.1 , 0.1 , 0.1 ),
56+ VecBuilder .fill (0.5 , 0.5 , 5.0 )); // Trust the gyro more than the AprilTags
57+
4658 // Configure AutoBuilder for PathPlanner
4759 AutoBuilder .configureHolonomic (
4860 this ::getPose ,
@@ -60,44 +72,35 @@ public Drivetrain(
6072 (targetPose ) -> Logger .recordOutput ("Odometry/TrajectorySetpoint" , targetPose ));
6173 }
6274
75+ @ Override
6376 public void periodic () {
6477 gyroIO .updateInputs (gyroInputs );
6578 Logger .processInputs ("Drivetrain/Gyro" , gyroInputs );
6679 for (Module module : modules ) {
6780 module .periodic ();
6881 }
6982
70- // Stop moving when disabled
71- if (DriverStation .isDisabled ()) {
72- for (Module module : modules ) {
73- module .stop ();
74- }
75- }
76- // Log empty setpoint states when disabled
7783 if (DriverStation .isDisabled ()) {
84+ // Stop moving when disabled
85+ for (Module module : modules ) module .stop ();
86+
87+ // Log empty setpoint states when disabled
7888 Logger .recordOutput ("SwerveStates/Setpoints" , new SwerveModuleState []{});
7989 Logger .recordOutput ("SwerveStates/SetpointsOptimized" , new SwerveModuleState []{});
8090 }
8191
82- // Update odometry
92+ // Calculate module deltas
8393 SwerveModulePosition [] wheelDeltas = new SwerveModulePosition [4 ];
8494 for (int i = 0 ; i < 4 ; i ++) {
8595 wheelDeltas [i ] = modules [i ].getPositionDelta ();
8696 }
87- // The twist represents the motion of the robot since the last
88- // loop cycle in x, y, and theta based only on the modules,
89- // without the gyro. The gyro is always disconnected in simulation.
90- Twist2d twist = kinematics .toTwist2d (wheelDeltas );
91- if (gyroInputs .connected ) {
92- // If the gyro is connected, replace the theta component of the twist
93- // with the change in angle since the last loop cycle.
94- twist =
95- new Twist2d (
96- twist .dx , twist .dy , gyroInputs .yawPosition .minus (lastGyroRotation ).getRadians ());
97- lastGyroRotation = gyroInputs .yawPosition ;
98- }
99- // Apply the twist (change since last loop cycle) to the current pose
100- pose = pose .exp (twist );
97+ // Use gyro rotation if gyro is connected, otherwise use module deltas to calculate rotation delta
98+ rawGyroRotation =
99+ gyroInputs .connected ?
100+ gyroInputs .yawPosition
101+ : rawGyroRotation .plus (Rotation2d .fromRadians (kinematics .toTwist2d (wheelDeltas ).dtheta ));
102+ // Update odometry
103+ poseEstimator .update (rawGyroRotation , getModulePositions ());
101104 }
102105
103106 /**
@@ -164,6 +167,10 @@ public double getCharacterizationVelocity() {
164167 return driveVelocityAverage / 4.0 ;
165168 }
166169
170+ public ChassisSpeeds getChassisSpeeds () {
171+ return kinematics .toChassisSpeeds (getModuleStates ());
172+ }
173+
167174 /**
168175 * Returns the module states (turn angles and drive velocities) for all the modules.
169176 */
@@ -181,21 +188,34 @@ private SwerveModuleState[] getModuleStates() {
181188 */
182189 @ AutoLogOutput (key = "Odometry/Robot" )
183190 public Pose2d getPose () {
184- return pose ;
191+ return poseEstimator . getEstimatedPosition () ;
185192 }
186193
187194 /**
188195 * Returns the current odometry rotation.
189196 */
190197 public Rotation2d getRotation () {
191- return pose .getRotation ();
198+ return getPose () .getRotation ();
192199 }
193200
194201 /**
195202 * Resets the current odometry pose.
196203 */
197204 public void setPose (Pose2d pose ) {
198- this .pose = pose ;
205+ poseEstimator .resetPosition (rawGyroRotation , getModulePositions (), pose );
206+ }
207+
208+ public void addVisionMeasurement (TimestampedVisionPose visionPose ) {
209+ poseEstimator .addVisionMeasurement (visionPose .poseMeters (), visionPose .timestampSecs ());
210+ }
211+
212+ public SwerveModulePosition [] getModulePositions () {
213+ return new SwerveModulePosition []{
214+ modules [0 ].getPosition (),
215+ modules [1 ].getPosition (),
216+ modules [2 ].getPosition (),
217+ modules [3 ].getPosition (),
218+ };
199219 }
200220
201221 /**
0 commit comments