Cleanup drive subsystem

.vscode/settings.json

@@ -52,6 +52,7 @@
     "sparkmax",
     "subwoofer",
     "teleop",
+    "teleoperated",
     "traj",
     "Uper",
     "wpiblue",

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

@@ -23,8 +23,8 @@
 import frc.robot.cmdGroup.IntakeNote;
 import frc.robot.cmdGroup.ShootAndReturnHome;
 import frc.robot.drive.DefaultDrive;
+import frc.robot.drive.DriveCommands;
 import frc.robot.drive.DriveSubsystem;
-import frc.robot.drive.TurnToAngle;
 import frc.robot.notification.ContextualColor;
 import frc.robot.notification.LEDSubsystem;
 import frc.robot.poseScheduler.PoseScheduler;
@@ -60,7 +60,7 @@ public RobotContainer() {
         DataLogManager.log("Data log started.");
 
         poseScheduler = new PoseScheduler();
-        drive = new DriveSubsystem(poseScheduler);
+        drive = new DriveSubsystem();
 
         // visionSubsystem = VisionSubsystem.create();
         ledSubsystem = LEDSubsystem.create();
@@ -93,11 +93,8 @@ private void configureBindings() {
                 .onTrue(new ShootAndReturnHome(shooter, arm, () -> robotContext.getShooterSpeed()[0], () -> robotContext
                         .getShooterSpeed()[1]));
 
-        driverController.a().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 0 : 180));
-        driverController
-                .b()
-                .onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 150 : -30.5)); // TODO alliance switching
-        driverController.x().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 90 : -90));
+        driverController.a().onTrue(DriveCommands.turnToShootCommand(drive));
+        driverController.x().onTrue(DriveCommands.turnToAmpCommand(drive));
 
         operatorController.leftTrigger().onTrue(new MoveToPosition(arm, Constants.Arm.AMP_POSITION));
         operatorController.rightTrigger().onTrue(new MoveToHome(arm));

src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java

@@ -85,7 +85,7 @@ public PathPlannerAutoGenerator(
         for (String s : autos) {
             autoSelector.addOption(s, s);
         }
-        autoSelector.setDefaultOption("SubC-MidDC", "SubC-MidDC");
+        autoSelector.setDefaultOption("None", "None");
 
         autoSelector.onChange((cmd) -> {
             if (cmd != null) {

src/main/java/frc/robot/drive/DriveCommands.java

@@ -0,0 +1,22 @@
+package frc.robot.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.util.AllianceUtil;
+
+public class DriveCommands {
+
+    public static Command turnToShootCommand(DriveSubsystem drive) {
+        return new InstantCommand(
+                () -> drive.setRotationTarget(Rotation2d.fromDegrees(AllianceUtil.isRedAlliance() ? 0 : 180)), drive);
+    }
+
+    public static Command turnToAmpCommand(DriveSubsystem drive) {
+        return new InstantCommand(() -> drive.setRotationTarget(Rotation2d.fromDegrees(-90)), drive);
+    }
+
+    public static Command zeroGyroscope(DriveSubsystem drive) {
+        return new InstantCommand(drive::zeroGyroscope, drive);
+    }
+}

src/main/java/frc/robot/drive/DriveSubsystem.java

@@ -4,17 +4,16 @@
 
 package frc.robot.drive;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
-import frc.robot.poseScheduler.PoseScheduler;
 import java.io.File;
 import java.io.IOException;
 import swervelib.SwerveDrive;
@@ -26,21 +25,16 @@
 
 public class DriveSubsystem extends SubsystemBase {
 
-    private SwerveDrive swerve;
+    private final SwerveDrive swerve;
 
-    private final PoseScheduler poseScheduler;
+    private final PIDController rotationController =
+            new PIDController(Constants.Drive.ROTATION_P, Constants.Drive.ROTATION_I, Constants.Drive.ROTATION_D);
+    private Rotation2d rotationTarget = null;
 
-    private ShuffleboardTab dashboard;
+    private ChassisSpeeds inputSpeeds = new ChassisSpeeds();
+    private boolean isFieldRelative = true;
 
-    private final PIDController rotationController;
-    private Double targetAngle = null;
-    private int rotationDirection =
-            1; // Used to make sure the rotation PID continues working while the gyroscope is inverted.
-
-    public DriveSubsystem(PoseScheduler poseScheduler) {
-        this.poseScheduler = poseScheduler;
-        rotationController =
-                new PIDController(Constants.Drive.ROTATION_P, Constants.Drive.ROTATION_I, Constants.Drive.ROTATION_D);
+    public DriveSubsystem() {
         rotationController.enableContinuousInput(-180, 180);
         rotationController.setTolerance(Constants.Drive.ROTATION_TARGET_RANGE);
 
@@ -56,24 +50,26 @@ public DriveSubsystem(PoseScheduler poseScheduler) {
                                     Constants.Drive.DRIVE_GEAR_RATIO,
                                     Constants.Drive.PULSE_PER_ROTATION));
         } catch (IOException e) {
-            System.out.println("Swerve drive configuration file could not be found at "
-                    + Filesystem.getDeployDirectory()
-                    + "/swerve");
             e.printStackTrace();
+            throw new Error(
+                    "Swerve drive configuration could not be loaded from " + Filesystem.getDeployDirectory()
+                            + "/swerve",
+                    e);
         }
 
-        swerve.chassisVelocityCorrection = false;
+        swerve.chassisVelocityCorrection = false; // TODO test out setting this
         swerve.setHeadingCorrection(true);
         initDashboard();
     }
 
     private void initDashboard() {
-        dashboard = Shuffleboard.getTab("Drive");
+        ShuffleboardTab dashboard = Shuffleboard.getTab("Drive");
         dashboard.addString("Current Command", this::getCommandName);
 
         dashboard.add("Rotation PID", rotationController);
         dashboard.addDouble("Yaw", () -> swerve.getYaw().getDegrees());
-        dashboard.addDouble("Target angle", () -> targetAngle == null ? -1 : targetAngle);
+        dashboard.addDouble("Target angle", () -> rotationTarget == null ? -1 : rotationTarget.getDegrees());
+        dashboard.addBoolean("Has rotation target?", () -> rotationTarget == null);
 
         dashboard.addBoolean("Rotation On Target?", () -> rotationController.atSetpoint());
 
@@ -112,69 +108,89 @@ public ChassisSpeeds getRobotRelativeSpeeds() {
         return swerve.getRobotVelocity();
     }
 
-    /** Used for PathPlanner autonomous */
-    public void setRobotRelativeChassisSpeeds(ChassisSpeeds speeds) {
-        swerve.drive(
-                new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond),
-                speeds.omegaRadiansPerSecond,
-                false,
-                false);
+    /**
+     * Used for PathPlanner autonomous
+     *
+     * @param override new pose
+     */
+    public void resetOdometry(Pose2d override) {
+        swerve.resetOdometry(override);
     }
 
-    public void drive(double x, double y, double rotation) {
-        if (rotation != 0 || (targetAngle != null && rotationController.atSetpoint())) {
-            targetAngle = null;
-        }
-
-        if (targetAngle != null) {
-            rotation = rotationDirection
-                    * rotationController.calculate(swerve.getYaw().getDegrees(), targetAngle);
-        } else {
-            rotation = rotation * Constants.Drive.MAXIMUM_ANGULAR_VELOCITY;
-        }
-
-        swerve.drive(
-                new Translation2d(x * Constants.Drive.MAXIMUM_VELOCITY, y * Constants.Drive.MAXIMUM_VELOCITY),
-                rotation,
-                true,
-                false);
+    /**
+     * Used for PathPlanner autonomous
+     *
+     * @param speeds robot relative chassis speeds
+     */
+    public void setRobotRelativeChassisSpeeds(ChassisSpeeds speeds) {
+        inputSpeeds = speeds;
+        isFieldRelative = false;
     }
 
+    /**
+     * Get the current robot pose as measured by the odometry.
+     * Odometry may include vision based measurements.
+     * @return the current pose of the robot
+     */
     public Pose2d getPose() {
         return swerve.getPose();
     }
 
-    public void addVisionMeasurement(Pose2d visionMeasuredPose) {
+    /**
+     * Drive the robot in field relative mode.
+     * This is the primary method of teleoperated robot control.
+     *
+     * @param x percent translational speed in the x direction
+     * @param y percent translational speed in the y direction
+     * @param rotation percent rotation speed
+     */
+    public void drive(double x, double y, double rotation) {
+        inputSpeeds = new ChassisSpeeds(
+                x * Constants.Drive.MAXIMUM_VELOCITY,
+                y * Constants.Drive.MAXIMUM_VELOCITY,
+                rotation * Constants.Drive.MAXIMUM_ANGULAR_VELOCITY);
+
+        isFieldRelative = true;
+    }
+
+    /**
+     * Add a vision measurement to the pose estimate.
+     * @param visionMeasuredPose The pose the vision measured.
+     * @param measurementTimeSeconds The time the given pose was measured in seconds
+     */
+    public void addVisionMeasurement(Pose2d visionMeasuredPose, double measurementTimeSeconds) {
         // Per recommendation from lib authors, discard poses which are
         // too far away from current pose.
         double distance =
                 Math.sqrt(Math.pow((visionMeasuredPose.getX() - swerve.getPose().getX()), 2)
                         + Math.pow((visionMeasuredPose.getY() - swerve.getPose().getY()), 2));
         if (distance <= Constants.Drive.MAXIMUM_VISION_POSE_OVERRIDE_DISTANCE) {
-            swerve.swerveDrivePoseEstimator.addVisionMeasurement(visionMeasuredPose, Timer.getFPGATimestamp());
+            swerve.swerveDrivePoseEstimator.addVisionMeasurement(visionMeasuredPose, measurementTimeSeconds);
         }
     }
 
     /**
-     * Used for autonomous
-     *
-     * @param override new pose
+     * Set a rotation target. May be null.
+     * @param target Rotation target or null to disable target
      */
-    public void resetOdometry(Pose2d override) {
-        swerve.resetOdometry(override);
-    }
-
-    public void setTargetAngle(double angleDeg) {
-        rotationController.reset();
-        targetAngle = angleDeg;
-    }
-
-    public void unsetTargetAngle() {
-        targetAngle = null;
+    public void setRotationTarget(Rotation2d target) {
+        rotationTarget = target;
     }
 
     @Override
     public void periodic() {
-        poseScheduler.updatePose(getPose());
+        if (inputSpeeds.omegaRadiansPerSecond != 0) {
+            rotationTarget = null;
+        } else if (rotationTarget != null) {
+            inputSpeeds.omegaRadiansPerSecond = rotationController.calculate(
+                    MathUtil.inputModulus(swerve.getYaw().getDegrees(), -180, 180),
+                    MathUtil.inputModulus(rotationTarget.getDegrees(), -180, 180));
+        }
+
+        if (isFieldRelative) {
+            swerve.driveFieldOriented(inputSpeeds);
+        } else {
+            swerve.drive(inputSpeeds);
+        }
     }
 }

src/main/java/frc/robot/util/AllianceUtil.java

@@ -2,15 +2,10 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import frc.robot.Robot;
 
 public class AllianceUtil {
 
     public static boolean isRedAlliance() {
-        if (Robot.isSimulation()) {
-            return false;
-        }
-
         if (DriverStation.getAlliance().isEmpty()) {
             return false;
         }