Cleanup teleop driving

src/main/java/frc/robot/Constants.java

@@ -16,6 +16,13 @@ public static final class RobotConstants {
   }
 
   public static final class DriveConstants {
+
+    public static enum DriveMode {
+      FIELD_CENTRIC,
+      ROBOT_CENTRIC_FORWARD_FACING,
+      ROBOT_CENTRIC_AFT_FACING
+    }
+
     // Define the conventional order of our modules when putting them into arrays
     public static final int FRONT_LEFT = 0;
     public static final int FRONT_RIGHT = 1;
@@ -86,14 +93,6 @@ public static final class AbsoluteEncoders {
             new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0) // rear right
             );
 
-    public static final SwerveDriveKinematics kDriveKinematicsDriveFromArm =
-        new SwerveDriveKinematics(
-            new Translation2d(kWheelBase, kTrackWidth / 2.0), // front left
-            new Translation2d(kWheelBase, -kTrackWidth / 2.0), // front right
-            new Translation2d(0.0, kTrackWidth / 2.0), // rear left
-            new Translation2d(0.0, -kTrackWidth / 2.0) // rear right
-            );
-
     // public static final boolean kGyroReversed = false;
     // public static final double ksVolts = 1.0;
     // public static final double kvVoltSecondsPerMeter = 0.8;
@@ -244,6 +243,9 @@ public static enum ArmState {
     public static final int kHardStopperForwardChannel = 3;
     public static final int kHardStopperReverseChannel = 2;
 
+    public static final Translation2d kChassisCentroidToArmCentroid =
+        new Translation2d(-0.4572, 0.0);
+
     public static final int kFlapForwardChannel = 4;
     public static final int kFlapReverseChannel = 5;
   }

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

@@ -27,7 +27,6 @@
 import frc.robot.Constants.ArmConstants.ArmState;
 import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.ClimberConstants;
-import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.IntakeConstants.IntakeState;
 import frc.robot.Constants.OIConstants;
@@ -262,8 +261,7 @@ private void createNamedCommands() {
   // spotless:on
 
   private void setDefaultCommands() {
-    m_swerve.setDefaultCommand(
-        new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematics, m_driver));
+    m_swerve.setDefaultCommand(new ZorroDriveCommand(m_swerve, m_arm, m_driver));
     m_intake.setDefaultCommand(m_intake.createSetVelocityCommand(0));
     m_climber.setDefaultCommand(m_climber.createStopCommand());
   }
@@ -276,10 +274,6 @@ private void configureDriverButtonBindings() {
         .onTrue(new InstantCommand(() -> m_swerve.resetHeading())
         .ignoringDisable(true));
 
-    new JoystickButton(m_driver,OIConstants.kZorroAIn)
-    .whileTrue((new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematicsDriveFromArm, m_driver)));
-
-
     Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED));
     JoystickButton D_Button = new JoystickButton(m_driver, OIConstants.kZorroDIn);
 

src/main/java/frc/robot/drivetrain/Drivetrain.java

@@ -8,6 +8,7 @@
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
@@ -21,6 +22,7 @@
 import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.RobotConstants;
+import java.util.Optional;
 import java.util.function.BooleanSupplier;
 
 /** Constructs a swerve drive style drivetrain. */
@@ -117,28 +119,36 @@ public void periodic() {
 
   /**
    * @param chassisSpeeds Robot-relative chassis speeds (x, y, theta)
+   * @param rotationCenter Vector from the chassis center to the desired center of rotation
    */
-  public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
-    var swerveModuleStates =
-        DriveConstants.kDriveKinematics.toSwerveModuleStates(
-            ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod));
+  public void setChassisSpeeds(
+      ChassisSpeeds chassisSpeeds, Optional<Translation2d> rotationCenter) {
+
+    SwerveModuleState[] swerveModuleStates;
+
+    if (rotationCenter.isPresent()) {
+      swerveModuleStates =
+          DriveConstants.kDriveKinematics.toSwerveModuleStates(
+              ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod),
+              rotationCenter.get());
+    } else {
+      swerveModuleStates =
+          DriveConstants.kDriveKinematics.toSwerveModuleStates(
+              ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod));
+    }
+
     SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
     m_frontLeft.setDesiredState(swerveModuleStates[0]);
     m_frontRight.setDesiredState(swerveModuleStates[1]);
     m_rearLeft.setDesiredState(swerveModuleStates[2]);
     m_rearRight.setDesiredState(swerveModuleStates[3]);
   }
 
-  // uses kinematics type to determine robot center
-  public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, SwerveDriveKinematics kinematicsType) {
-    var swerveModuleStates =
-        kinematicsType.toSwerveModuleStates(
-            ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod));
-    SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
-    m_frontLeft.setDesiredState(swerveModuleStates[0]);
-    m_frontRight.setDesiredState(swerveModuleStates[1]);
-    m_rearLeft.setDesiredState(swerveModuleStates[2]);
-    m_rearRight.setDesiredState(swerveModuleStates[3]);
+  /**
+   * @param chassisSpeeds Robot-relative chassis speeds (x, y, theta)
+   */
+  public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
+    setChassisSpeeds(chassisSpeeds, null);
   }
 
   /** Updates the field relative position of the robot. */

src/main/java/frc/robot/drivetrain/commands/DriveCommand.java

@@ -3,30 +3,27 @@
 package frc.robot.drivetrain.commands;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.DriveConstants.DriveMode;
 import frc.robot.drivetrain.Drivetrain;
+import java.util.Optional;
 
 public abstract class DriveCommand extends Command {
 
   private double xDot;
   private double yDot;
   private double thetaDot;
 
-  // used to swap control locations
-  SwerveDriveKinematics kinematicsType;
-
   // The subsystem the command runs on
   public final Drivetrain drivetrain;
 
-  public DriveCommand(Drivetrain subsystem, SwerveDriveKinematics kinematicsType) {
+  public DriveCommand(Drivetrain subsystem) {
     drivetrain = subsystem;
     addRequirements(drivetrain);
-
-    this.kinematicsType = kinematicsType;
   }
 
   @Override
@@ -38,32 +35,30 @@ public void execute() {
     yDot = getY() * DriveConstants.kMaxTranslationalVelocity;
     thetaDot = getTheta() * DriveConstants.kMaxRotationalVelocity;
 
-    SmartDashboard.putBoolean("fieldRelative", fieldRelative());
+    SmartDashboard.putBoolean("fieldRelative", getDriveMode() == DriveMode.FIELD_CENTRIC);
     SmartDashboard.putBoolean("rotateField", drivetrain.fieldRotatedSupplier().getAsBoolean());
 
-    drivetrain.setChassisSpeeds(
-        fieldRelative() ? getFieldRelativeChassisSpeeds() : getRobotRelativeChassisSpeedsReversed(),
-        kinematicsType);
+    drivetrain.setChassisSpeeds(getChassisSpeeds(), Optional.of(getSteeringCenter()));
   }
 
-  private ChassisSpeeds getRobotRelativeChassisSpeedsForward() {
-    return new ChassisSpeeds(xDot, yDot, thetaDot);
+  private ChassisSpeeds getChassisSpeeds() {
+    switch (getDriveMode()) {
+      case FIELD_CENTRIC:
+        var robotAngle = drivetrain.getHeading();
+        if (drivetrain.fieldRotatedSupplier().getAsBoolean())
+          robotAngle.rotateBy(new Rotation2d(Math.PI));
+        return ChassisSpeeds.fromFieldRelativeSpeeds(xDot, yDot, thetaDot, robotAngle);
+
+      case ROBOT_CENTRIC_AFT_FACING:
+        var rotated = new Translation2d(xDot, yDot).rotateBy(new Rotation2d(Math.PI));
+        return new ChassisSpeeds(rotated.getX(), rotated.getY(), thetaDot);
+
+      case ROBOT_CENTRIC_FORWARD_FACING:
+      default:
+        return new ChassisSpeeds(xDot, yDot, thetaDot);
+    }
   }
 
-  private ChassisSpeeds getRobotRelativeChassisSpeedsReversed() {
-    return new ChassisSpeeds(-xDot, -yDot, thetaDot);
-  }
-
-  // spotless:off
-  private ChassisSpeeds getFieldRelativeChassisSpeeds() {
-    return drivetrain.fieldRotatedSupplier().getAsBoolean()
-        ? ChassisSpeeds.fromFieldRelativeSpeeds(
-            xDot, yDot, thetaDot, drivetrain.getHeading().rotateBy(new Rotation2d(Math.PI)))
-        : ChassisSpeeds.fromFieldRelativeSpeeds(
-            xDot, yDot, thetaDot, drivetrain.getHeading());
-  }
-  // spotless:on
-
   /**
    * @return The input to the drive controller in the x axis, range [-1, 1]
    */
@@ -80,7 +75,12 @@ private ChassisSpeeds getFieldRelativeChassisSpeeds() {
   public abstract double getTheta();
 
   /**
-   * @return Boolean representing whether to drive in field-relative mode
+   * @return The desired drive mode
+   */
+  public abstract DriveMode getDriveMode();
+
+  /**
+   * @return The vector between chassis center and desired steering center
    */
-  public abstract boolean fieldRelative();
+  public abstract Translation2d getSteeringCenter();
 }

src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java

@@ -4,8 +4,10 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.XboxController;
-import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.ArmConstants;
+import frc.robot.Constants.DriveConstants.DriveMode;
 import frc.robot.drivetrain.Drivetrain;
 
 public class XBoxDriveCommand extends DriveCommand {
@@ -18,7 +20,7 @@ public class XBoxDriveCommand extends DriveCommand {
   private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
 
   public XBoxDriveCommand(Drivetrain subsystem, XboxController joysticks) {
-    super(subsystem, DriveConstants.kDriveKinematics);
+    super(subsystem);
     this.m_controller = joysticks;
   }
 
@@ -38,7 +40,14 @@ public double getTheta() {
   }
 
   @Override
-  public boolean fieldRelative() {
-    return m_controller.getLeftBumper();
+  public DriveMode getDriveMode() {
+    return m_controller.getLeftBumper()
+        ? DriveMode.FIELD_CENTRIC
+        : DriveMode.ROBOT_CENTRIC_AFT_FACING;
+  }
+
+  @Override
+  public Translation2d getSteeringCenter() {
+    return m_controller.getRightBumper() ? ArmConstants.kChassisCentroidToArmCentroid : null;
   }
 }

src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java

@@ -3,18 +3,23 @@
 package frc.robot.drivetrain.commands;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.Joystick;
+import frc.robot.Constants.ArmConstants;
+// import frc.robot.Constants.ArmConstants.ArmState;
+import frc.robot.Constants.DriveConstants.DriveMode;
 import frc.robot.Constants.OIConstants;
+import frc.robot.arm.Arm;
 import frc.robot.drivetrain.Drivetrain;
 
 public class ZorroDriveCommand extends DriveCommand {
 
+  Arm m_arm;
   Joystick m_controller;
 
-  public ZorroDriveCommand(
-      Drivetrain subsystem, SwerveDriveKinematics kinematicsType, Joystick joysticks) {
-    super(subsystem, kinematicsType);
+  public ZorroDriveCommand(Drivetrain subsystem, Arm arm, Joystick joysticks) {
+    super(subsystem);
+    this.m_arm = arm;
     this.m_controller = joysticks;
   }
 
@@ -34,7 +39,27 @@ public double getTheta() {
   }
 
   @Override
-  public boolean fieldRelative() {
-    return m_controller.getRawButton(OIConstants.kZorroEUp);
+  public DriveMode getDriveMode() {
+    DriveMode mode;
+
+    if (m_controller.getRawButton(OIConstants.kZorroEUp)) mode = DriveMode.FIELD_CENTRIC;
+    else if (m_controller.getRawButton(OIConstants.kZorroEDown))
+      mode = DriveMode.ROBOT_CENTRIC_AFT_FACING;
+    else mode = DriveMode.FIELD_CENTRIC;
+
+    return mode;
+  }
+
+  @Override
+  public Translation2d getSteeringCenter() {
+    // offset steering angle whenever arm is raised
+    // return m_arm.stateChecker(ArmState.DEPLOYED).getAsBoolean()
+    //   ? ArmConstants.kChassisCentroidToArmCentroid
+    //   : null;
+
+    // offset steering angle when Zorro A button is pressed
+    return m_controller.getRawButton(OIConstants.kZorroAIn)
+        ? ArmConstants.kChassisCentroidToArmCentroid
+        : null;
   }
 }