Merge pull request #12 from Iron-Panthers/feat/odometry

rough odometry, lacking advanced timing/measurement features

src/main/java/frc/robot/RobotContainer.java

@@ -7,9 +7,11 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.Mode;
 import frc.robot.subsystems.flywheels.Flywheels;
+import frc.robot.subsystems.flywheels.FlywheelsIO;
 import frc.robot.subsystems.flywheels.FlywheelsIOTalonFX;
 import frc.robot.subsystems.rollers.Rollers;
 import frc.robot.subsystems.rollers.intake.Intake;
+import frc.robot.subsystems.rollers.intake.IntakeIO;
 import frc.robot.subsystems.rollers.intake.IntakeIOTalonFX;
 import frc.robot.subsystems.swerve.Drive;
 import frc.robot.subsystems.swerve.DriveConstants;
@@ -25,6 +27,7 @@
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+  private final RobotState robotState = RobotState.getInstance();
 
   private final CommandXboxController driverA = new CommandXboxController(0);
   private final CommandXboxController driverB = new CommandXboxController(1);
@@ -83,6 +86,12 @@ public RobotContainer() {
               new ModuleIO() {},
               new ModuleIO() {});
     }
+    if (flywheels == null) {
+      flywheels = new Flywheels(new FlywheelsIO() {});
+    }
+    if (intake == null) {
+      intake = new Intake(new IntakeIO() {});
+    }
 
     rollers = new Rollers(intake);
 

src/main/java/frc/robot/RobotState.java

@@ -5,18 +5,85 @@
 package frc.robot;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import frc.robot.subsystems.swerve.DriveConstants;
+import org.littletonrobotics.junction.AutoLogOutput;
 
+/* based on wpimath/../PoseEstimator.java */
 public class RobotState {
-  private Pose2d estimatedPose;
+  public record OdometryMeasurement(
+      SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {}
+
+  public record VisionMeasurement(Pose2d visionPose, double timestamp) {}
+
+  private static final double poseBufferSizeSeconds = 2; // shorter?
+
+  private TimeInterpolatableBuffer<Pose2d> poseBuffer =
+      TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds);
+
+  private Pose2d odometryPose = new Pose2d(); // motion sensors
+  private Pose2d estimatedPose = new Pose2d(); // odometry + vision
+
+  private SwerveDriveWheelPositions lastWheelPositions =
+      new SwerveDriveWheelPositions(
+          new SwerveModulePosition[] {
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition()
+          });
+  private Rotation2d lastGyroAngle = new Rotation2d();
 
   private static RobotState instance;
 
   public static RobotState getInstance() {
+    if (instance == null) instance = new RobotState();
     return instance;
   }
 
-  public RobotState() {}
+  private RobotState() {}
+
+  /* update pose estimation based on odometry measurements, based on wpimath */
+  public void addOdometryMeasurement(OdometryMeasurement measurement) {
+    Twist2d twist =
+        DriveConstants.KINEMATICS.toTwist2d(lastWheelPositions, measurement.wheelPositions());
+    twist.dtheta = measurement.gyroAngle().minus(lastGyroAngle).getRadians();
+
+    lastWheelPositions = measurement.wheelPositions();
+    lastGyroAngle = measurement.gyroAngle();
+
+    // integrate to find difference in pose over time, add to pose estimate
+    odometryPose = odometryPose.exp(twist);
+
+    // add post estimate to buffer at timestamp; for vision
+    poseBuffer.addSample(measurement.timestamp(), odometryPose);
+  }
+
+  // FIXME TO DO
+  public void addVisionMeasurement(VisionMeasurement measurement) {
+    // if measurement is old enough to be outside buffer timespan, skip
+    if (poseBuffer.getInternalBuffer().isEmpty()
+        || poseBuffer.getInternalBuffer().lastKey() < poseBufferSizeSeconds) {
+      return;
+    }
+  }
+
+  public void resetPose(Pose2d pose) {
+    odometryPose = pose;
+    estimatedPose = pose;
+    poseBuffer.clear();
+  }
+
+  @AutoLogOutput(key = "RobotState/OdometryPose")
+  public Pose2d getOdometryPose() {
+    return odometryPose;
+  }
 
+  @AutoLogOutput(key = "RobotState/EstimatedPose")
   public Pose2d getEstimatedPose() {
     return estimatedPose;
   }

src/main/java/frc/robot/subsystems/swerve/Drive.java

@@ -6,12 +6,17 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.RobotState;
 import frc.robot.subsystems.swerve.controllers.TeleopController;
+import java.util.Arrays;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -62,6 +67,17 @@ public void periodic() {
       module.updateInputs();
     }
 
+    // pass odometry data to robotstate
+    SwerveDriveWheelPositions wheelPositions =
+        new SwerveDriveWheelPositions(
+            Arrays.stream(modules)
+                .map(module -> module.getModulePosition())
+                .toArray(SwerveModulePosition[]::new));
+    RobotState.getInstance()
+        .addOdometryMeasurement(
+            new RobotState.OdometryMeasurement(
+                wheelPositions, gyroInputs.yawPosition, Timer.getFPGATimestamp()));
+
     switch (driveMode) {
       case TELEOP -> {
         targetSpeeds = teleopController.update(arbitraryYaw);

src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java

@@ -18,7 +18,7 @@ public GyroIOPigeon2() {
     pigeon = new Pigeon2(DriveConstants.GYRO_ID);
 
     pigeon.getConfigurator().apply(new Pigeon2Configuration());
-    pigeon.setYaw(0);
+    pigeon.setYaw(0, 1.0);
 
     yaw = pigeon.getYaw();
     yawVelocity = pigeon.getAngularVelocityZWorld();

src/main/java/frc/robot/subsystems/swerve/Module.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.swerve;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import org.littletonrobotics.junction.Logger;
 
@@ -54,4 +55,8 @@ public void runToSetpoint(SwerveModuleState targetState) {
   public Rotation2d getSteerHeading() {
     return inputs.steerAbsolutePostion;
   }
+
+  public SwerveModulePosition getModulePosition() {
+    return new SwerveModulePosition(inputs.drivePositionMeters, inputs.steerAbsolutePostion);
+  }
 }

src/main/java/frc/robot/subsystems/swerve/ModuleIO.java

@@ -13,6 +13,7 @@ class ModuleIOInputs {
     public boolean steerMotorConnected = true;
 
     public double drivePositionRads = 0;
+    public double drivePositionMeters = 0;
     public double driveVelocityRadsPerSec = 0;
     public double driveAppliedVolts = 0;
     public double driveSupplyCurrent = 0;

src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems.swerve;
 
+import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG;
 import static frc.robot.subsystems.swerve.DriveConstants.MODULE_CONSTANTS;
 
 import com.ctre.phoenix6.BaseStatusSignal;
@@ -133,6 +134,8 @@ public void updateInputs(ModuleIOInputs inputs) {
                 driveStatorCurrent)
             .isOK();
     inputs.drivePositionRads = Units.rotationsToRadians(drivePosition.getValueAsDouble());
+    inputs.drivePositionMeters =
+        Units.rotationsToRadians(drivePosition.getValueAsDouble()) * DRIVE_CONFIG.wheelRadius();
     inputs.driveVelocityRadsPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble());
     inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
     inputs.driveSupplyCurrent = driveSupplyCurrent.getValueAsDouble();