Merge pull request #23 from Robocubs/robot-state

Replaced pose estimator with robot state

src/main/java/com/team1701/robot/Configuration.java

@@ -34,8 +34,12 @@ public static Mode getMode() {
         }
     }
 
-    public static Alliance getAlliance() {
-        return DriverStation.getAlliance().orElse(Alliance.Blue);
+    public static boolean isBlueAlliance() {
+        return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue;
+    }
+
+    public static boolean isRedAlliance() {
+        return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;
     }
 
     public static enum RobotType {

src/main/java/com/team1701/robot/FieldConstants.java

@@ -4,11 +4,15 @@
 import edu.wpi.first.math.util.Units;
 
 public final class FieldConstants {
-    // blue locations use blue field origin, red locations use red origin
-    // all units are in meters and radians
     public static double kFieldLongLengthMeters = (Units.inchesToMeters((54 * 12) + 3.25));
     public static double kFieldShortLengthMeters = (Units.inchesToMeters((26 * 12) + 11.25));
+    public static double kCenterLine = kFieldLongLengthMeters / 2.0;
+    public static double kWingLength = Units.inchesToMeters(231.2);
 
-    public static Translation3d kBlueSpeakerOpeningCenter = new Translation3d(0, Units.inchesToMeters(218.42), 204.5);
-    public static Translation3d kRedSpeakerOpeningCenter = new Translation3d(0, Units.inchesToMeters(651.23), 204.5);
+    public static Translation3d kBlueSpeakerOpeningCenter =
+            new Translation3d(Units.inchesToMeters(0.9), Units.inchesToMeters(218.42), 204.5);
+    public static Translation3d kRedSpeakerOpeningCenter = new Translation3d(
+            kFieldLongLengthMeters - kBlueSpeakerOpeningCenter.getX(),
+            kBlueSpeakerOpeningCenter.getY(),
+            kBlueSpeakerOpeningCenter.getZ());
 }

src/main/java/com/team1701/robot/RobotContainer.java

@@ -20,7 +20,7 @@
 import com.team1701.lib.util.GeometryUtil;
 import com.team1701.robot.Configuration.Mode;
 import com.team1701.robot.commands.AutonomousCommands;
-import com.team1701.robot.estimation.PoseEstimator;
+import com.team1701.robot.states.RobotState;
 import com.team1701.robot.subsystems.drive.Drive;
 import com.team1701.robot.subsystems.drive.DriveMotorFactory;
 import com.team1701.robot.subsystems.drive.SwerveModule.SwerveModuleIO;
@@ -29,7 +29,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -40,6 +39,7 @@
 import static edu.wpi.first.wpilibj2.command.Commands.*;
 
 public class RobotContainer {
+    private final RobotState mRobotState = new RobotState();
     public final Drive mDrive;
     public final Shooter mShooter;
     // public final Vision mVision;
@@ -55,37 +55,40 @@ public RobotContainer() {
         if (Configuration.getMode() != Mode.REPLAY) {
             switch (Configuration.getRobot()) {
                 case COMPETITION_BOT:
-                    drive = Optional.of(new Drive(new GyroIOPigeon2(10), new SwerveModuleIO[] {
-                        new SwerveModuleIO(
-                                DriveMotorFactory.createDriveMotorIOSparkMax(10),
-                                DriveMotorFactory.createSteerMotorIOSparkMax(11),
-                                new EncoderIOAnalog(0)),
-                        new SwerveModuleIO(
-                                DriveMotorFactory.createDriveMotorIOSparkMax(12),
-                                DriveMotorFactory.createSteerMotorIOSparkMax(13),
-                                new EncoderIOAnalog(1)),
-                        new SwerveModuleIO(
-                                DriveMotorFactory.createDriveMotorIOSparkMax(16),
-                                DriveMotorFactory.createSteerMotorIOSparkMax(17),
-                                new EncoderIOAnalog(3)),
-                        new SwerveModuleIO(
-                                DriveMotorFactory.createDriveMotorIOSparkMax(14),
-                                DriveMotorFactory.createSteerMotorIOSparkMax(15),
-                                new EncoderIOAnalog(2)),
-                    }));
+                    drive = Optional.of(new Drive(
+                            new GyroIOPigeon2(10),
+                            new SwerveModuleIO[] {
+                                new SwerveModuleIO(
+                                        DriveMotorFactory.createDriveMotorIOSparkMax(10),
+                                        DriveMotorFactory.createSteerMotorIOSparkMax(11),
+                                        new EncoderIOAnalog(0)),
+                                new SwerveModuleIO(
+                                        DriveMotorFactory.createDriveMotorIOSparkMax(12),
+                                        DriveMotorFactory.createSteerMotorIOSparkMax(13),
+                                        new EncoderIOAnalog(1)),
+                                new SwerveModuleIO(
+                                        DriveMotorFactory.createDriveMotorIOSparkMax(16),
+                                        DriveMotorFactory.createSteerMotorIOSparkMax(17),
+                                        new EncoderIOAnalog(3)),
+                                new SwerveModuleIO(
+                                        DriveMotorFactory.createDriveMotorIOSparkMax(14),
+                                        DriveMotorFactory.createSteerMotorIOSparkMax(15),
+                                        new EncoderIOAnalog(2)),
+                            },
+                            mRobotState));
 
                     // TODO: update ID
                     shooter = Optional.of(new Shooter(
                             ShooterMotorFactory.createDriveMotorIOSparkFlex(Constants.Shooter.kShooterDeviceId)));
                     break;
                 case SIMULATION_BOT:
-                    var gyroIO = new GyroIOSim(
-                            () -> PoseEstimator.getInstance().getPose2d().getRotation());
+                    var gyroIO = new GyroIOSim(mRobotState::getHeading);
                     var simDrive = new Drive(
                             gyroIO,
                             Stream.generate(() -> SwerveModuleIO.createSim(DCMotor.getKrakenX60(1), DCMotor.getNEO(1)))
                                     .limit(Constants.Drive.kNumModules)
-                                    .toArray(SwerveModuleIO[]::new));
+                                    .toArray(SwerveModuleIO[]::new),
+                            mRobotState);
                     gyroIO.setYawSupplier(
                             () -> simDrive.getVelocity().omegaRadiansPerSecond, Constants.kLoopPeriodSeconds);
 
@@ -107,7 +110,8 @@ public RobotContainer() {
                 new GyroIO() {},
                 Stream.generate(() -> new SwerveModuleIO(new MotorIO() {}, new MotorIO() {}, new EncoderIO() {}))
                         .limit(Constants.Drive.kNumModules)
-                        .toArray(SwerveModuleIO[]::new)));
+                        .toArray(SwerveModuleIO[]::new),
+                mRobotState));
 
         /*  this.mVision = vision.orElseGet(() -> new Vision(
         new AprilTagCameraIO() {},
@@ -142,7 +146,7 @@ private void setupControllerBindings() {
         mDriverController
                 .x()
                 .onTrue(runOnce(() -> mDrive.zeroGyroscope(
-                                Configuration.getAlliance().equals(Alliance.Blue)
+                                Configuration.isBlueAlliance()
                                         ? GeometryUtil.kRotationIdentity
                                         : GeometryUtil.kRotationPi))
                         .withName("ZeroGyroscopeToHeading"));
@@ -153,32 +157,29 @@ private void setupControllerBindings() {
     }
 
     private void setupAutonomous() {
-        var poseEstimator = PoseEstimator.getInstance();
-
         AutoBuilder.configureHolonomic(
-                poseEstimator::getPose2d,
-                poseEstimator::setPose,
+                mRobotState::getPose2d,
+                mRobotState::resetPose,
                 mDrive::getVelocity,
                 mDrive::setVelocity,
                 Constants.Drive.kPathFollowerConfig,
-                () -> Configuration.getAlliance() == Alliance.Red,
+                Configuration::isRedAlliance,
                 mDrive);
 
         PathPlannerLogging.setLogTargetPoseCallback(pose -> Logger.recordOutput("PathPlanner/TargetPose", pose));
         PathPlannerLogging.setLogActivePathCallback(
                 poses -> Logger.recordOutput("PathPlanner/Path", poses.toArray(Pose2d[]::new)));
 
-        var commands = new AutonomousCommands(mDrive);
+        var commands = new AutonomousCommands(mRobotState, mDrive);
         autonomousModeChooser.addDefaultOption("Demo", commands.demo());
         autonomousModeChooser.addOption("Four Piece", commands.fourPiece());
         autonomousModeChooser.addOption("Four Piece Planner", new PathPlannerAuto("FourPiece"));
     }
 
     private void setupStateTriggers() {
         var teleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
-        teleopTrigger.onTrue(runOnce(() -> mDrive.zeroGyroscope(
-                        PoseEstimator.getInstance().getPose2d().getRotation()))
-                .withName("ZeroGyroscopeToPose"));
+        teleopTrigger.onTrue(
+                runOnce(() -> mDrive.zeroGyroscope(mRobotState.getHeading())).withName("ZeroGyroscopeToPose"));
     }
 
     public Optional<Command> getAutonomousCommand() {

src/main/java/com/team1701/robot/commands/AutonomousCommands.java

@@ -11,20 +11,21 @@
 import com.team1701.robot.Configuration;
 import com.team1701.robot.Constants;
 import com.team1701.robot.FieldConstants;
-import com.team1701.robot.estimation.PoseEstimator;
+import com.team1701.robot.states.RobotState;
 import com.team1701.robot.subsystems.drive.Drive;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 
 import static com.team1701.lib.commands.LoggedCommands.*;
 import static edu.wpi.first.wpilibj2.command.Commands.*;
 
 public class AutonomousCommands {
+    private final RobotState mRobotState;
     private final Drive mDrive;
 
-    public AutonomousCommands(Drive drive) {
+    public AutonomousCommands(RobotState robotState, Drive drive) {
+        mRobotState = robotState;
         mDrive = drive;
 
         NamedCommands.registerCommand("printHello", print("Hello from autonomous path"));
@@ -35,15 +36,15 @@ private Pose2d autoFlipPose(Pose2d pose) {
                 FieldConstants.kFieldLongLengthMeters - pose.getX(),
                 pose.getY(),
                 GeometryUtil.kRotationPi.minus(pose.getRotation()));
-        return Configuration.getAlliance() == Alliance.Red ? flippedPose : pose;
+        return Configuration.isRedAlliance() ? flippedPose : pose;
     }
 
     private Command resetPose(Pose2d pose) {
         return resetPose(() -> autoFlipPose(pose));
     }
 
     private Command resetPose(Supplier<Pose2d> pose) {
-        return runOnce(() -> PoseEstimator.getInstance().setPose(pose.get())).withName("ResetPose");
+        return runOnce(() -> mRobotState.resetPose(pose.get())).withName("ResetPose");
     }
 
     private Command driveToPose(Pose2d pose) {
@@ -59,7 +60,8 @@ private Command driveToPose(Pose2d pose, KinematicLimits kinematicLimits) {
     }
 
     private Command driveToPose(Pose2d pose, KinematicLimits kinematicLimits, boolean finishAtPose) {
-        return DriveCommands.driveToPose(mDrive, () -> autoFlipPose(pose), kinematicLimits, finishAtPose);
+        return DriveCommands.driveToPose(
+                mDrive, () -> autoFlipPose(pose), mRobotState::getPose2d, kinematicLimits, finishAtPose);
     }
 
     private Command followPath(String pathName) {
@@ -91,7 +93,7 @@ private Command followChoreoPath(String pathName, boolean resetPose) {
             return idle(); // Prevent auto mode from continuing if trajectory failed to load
         }
 
-        return new DriveChoreoTrajectory(mDrive, trajectory, resetPose);
+        return new DriveChoreoTrajectory(mDrive, trajectory, mRobotState, resetPose);
     }
 
     public Command demo() {

src/main/java/com/team1701/robot/commands/DriveChoreoTrajectory.java

@@ -5,25 +5,26 @@
 import com.choreo.lib.ChoreoTrajectory;
 import com.team1701.robot.Configuration;
 import com.team1701.robot.Constants;
-import com.team1701.robot.estimation.PoseEstimator;
+import com.team1701.robot.states.RobotState;
 import com.team1701.robot.subsystems.drive.Drive;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import org.littletonrobotics.junction.Logger;
 
 public class DriveChoreoTrajectory extends Command {
     private final Drive mDrive;
+    private final RobotState mRobotState;
     private final ChoreoTrajectory mTrajectory;
     private final ChoreoTrajectory mFlippedTrajectory;
     private final ChoreoControlFunction mControlFunction;
     private final Timer mTimer = new Timer();
     private final boolean mResetPose;
 
-    DriveChoreoTrajectory(Drive drive, ChoreoTrajectory trajectory, boolean resetPose) {
+    DriveChoreoTrajectory(Drive drive, ChoreoTrajectory trajectory, RobotState robotState, boolean resetPose) {
         mDrive = drive;
+        mRobotState = robotState;
         mTrajectory = trajectory;
         mFlippedTrajectory = trajectory.flipped();
         mControlFunction = Choreo.choreoSwerveController(
@@ -41,7 +42,7 @@ public void initialize() {
 
         var trajectory = shouldFlip() ? mFlippedTrajectory : mTrajectory;
         if (mResetPose) {
-            PoseEstimator.getInstance().setPose(trajectory.getInitialPose());
+            mRobotState.resetPose(trajectory.getInitialPose());
         }
 
         mDrive.setKinematicLimits(Constants.Drive.kUncappedKinematicLimits);
@@ -54,7 +55,7 @@ public void initialize() {
     public void execute() {
         var trajectory = shouldFlip() ? mFlippedTrajectory : mTrajectory;
         var state = trajectory.sample(mTimer.get());
-        var chassisSpeeds = mControlFunction.apply(PoseEstimator.getInstance().getPose2d(), state);
+        var chassisSpeeds = mControlFunction.apply(mRobotState.getPose2d(), state);
         mDrive.setVelocity(chassisSpeeds);
 
         Logger.recordOutput("Choreo/TargetPose", state.getPose());
@@ -79,6 +80,6 @@ public boolean isFinished() {
     }
 
     private boolean shouldFlip() {
-        return Configuration.getAlliance() == Alliance.Red;
+        return Configuration.isRedAlliance();
     }
 }

src/main/java/com/team1701/robot/commands/DriveCommands.java

@@ -20,8 +20,12 @@ public static Command driveWithJoysticks(
     }
 
     public static Command driveToPose(
-            Drive drive, Supplier<Pose2d> poseSupplier, KinematicLimits kinematicLimits, boolean finishAtPose) {
-        return new DriveToPose(drive, poseSupplier, kinematicLimits, finishAtPose);
+            Drive drive,
+            Supplier<Pose2d> poseSupplier,
+            Supplier<Pose2d> robotPoseSupplier,
+            KinematicLimits kinematicLimits,
+            boolean finishAtPose) {
+        return new DriveToPose(drive, poseSupplier, robotPoseSupplier, kinematicLimits, finishAtPose);
     }
 
     public static Command swerveLock(Drive drive) {

src/main/java/com/team1701/robot/commands/DriveToPose.java

@@ -7,7 +7,6 @@
 import com.team1701.lib.util.LoggedTunableNumber;
 import com.team1701.lib.util.Util;
 import com.team1701.robot.Constants;
-import com.team1701.robot.estimation.PoseEstimator;
 import com.team1701.robot.subsystems.drive.Drive;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
@@ -48,6 +47,7 @@ public class DriveToPose extends Command {
 
     private final Drive mDrive;
     private final Supplier<Pose2d> mTargetPoseSupplier;
+    private final Supplier<Pose2d> mRobotPoseSupplier;
     private final KinematicLimits mKinematicLimits;
     private final boolean mFinishAtPose;
     private final PIDController mTranslationController;
@@ -60,9 +60,15 @@ public class DriveToPose extends Command {
     private TrapezoidProfile.State mTranslationState = new TrapezoidProfile.State();
     private TrapezoidProfile.State mRotationState = new TrapezoidProfile.State();
 
-    DriveToPose(Drive drive, Supplier<Pose2d> poseSupplier, KinematicLimits kinematicLimits, boolean finishAtPose) {
+    DriveToPose(
+            Drive drive,
+            Supplier<Pose2d> poseSupplier,
+            Supplier<Pose2d> robotPoseSupplier,
+            KinematicLimits kinematicLimits,
+            boolean finishAtPose) {
         mDrive = drive;
         mTargetPoseSupplier = poseSupplier;
+        mRobotPoseSupplier = robotPoseSupplier;
         mKinematicLimits = kinematicLimits;
         mFinishAtPose = finishAtPose;
 
@@ -84,12 +90,12 @@ public class DriveToPose extends Command {
     public void initialize() {
         mDrive.setKinematicLimits(Constants.Drive.kFastKinematicLimits);
         mTargetPose = mTargetPoseSupplier.get();
-        mSetpoint = PoseEstimator.getInstance().getPose2d();
+        mSetpoint = mRobotPoseSupplier.get();
 
         mTranslationController.reset();
         mRotationController.reset();
 
-        var currentPose = PoseEstimator.getInstance().getPose2d();
+        var currentPose = mRobotPoseSupplier.get();
         var translationToTarget = mTargetPose.getTranslation().minus(currentPose.getTranslation());
         var rotationToTarget = translationToTarget.getAngle();
         var fieldRelativeChassisSpeeds = mDrive.getFieldRelativeVelocity();
@@ -129,7 +135,7 @@ public void execute() {
             mRotationController.setPID(kRotationKp.get(), kRotationKi.get(), kRotationKd.get());
         }
 
-        var currentPose = PoseEstimator.getInstance().getPose2d();
+        var currentPose = mRobotPoseSupplier.get();
         var translationToTarget = mTargetPose.getTranslation().minus(currentPose.getTranslation());
         var distanceToTarget = translationToTarget.getNorm();
         var headingToTarget = translationToTarget.getAngle();
@@ -183,7 +189,7 @@ public boolean isFinished() {
     }
 
     private boolean atTargetPose() {
-        var currentPose = PoseEstimator.getInstance().getPose2d();
+        var currentPose = mRobotPoseSupplier.get();
         var translationError =
                 mTargetPose.getTranslation().minus(currentPose.getTranslation()).getNorm();
         return Util.inRange(translationError, kTranslationToleranceMeters.get())

src/main/java/com/team1701/robot/commands/DriveWithJoysticks.java

@@ -9,7 +9,6 @@
 import com.team1701.robot.subsystems.drive.Drive;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class DriveWithJoysticks extends Command {
@@ -38,7 +37,7 @@ public void execute() {
         var kinematicLimits = mKinematicLimits.get();
         mDrive.setKinematicLimits(kinematicLimits);
 
-        var translationSign = Configuration.getAlliance().equals(Alliance.Blue) ? 1.0 : -1.0;
+        var translationSign = Configuration.isBlueAlliance() ? 1.0 : -1.0;
         var throttle = mThrottle.getAsDouble() * translationSign;
         var strafe = mStrafe.getAsDouble() * translationSign;
         var magnitude = Math.hypot(throttle, strafe);