From 5149c3c52b30b43f1851b0a135e52ef1547af227 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Tue, 20 Feb 2024 19:21:17 -0500 Subject: [PATCH 1/9] use w[pilib approach to custom centers of rotation --- src/main/java/frc/robot/Constants.java | 10 +++----- src/main/java/frc/robot/RobotContainer.java | 7 +----- .../java/frc/robot/drivetrain/Drivetrain.java | 20 ++++++++++++---- .../drivetrain/commands/DriveCommand.java | 17 ++++++------- .../drivetrain/commands/XBoxDriveCommand.java | 13 ++++++++-- .../commands/ZorroDriveCommand.java | 24 +++++++++++++++---- 6 files changed, 59 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 518ca2c..c5ceb28 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -86,13 +86,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; @@ -208,6 +201,9 @@ public static final class ArmConstants { 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 class ClimberConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26224a4..8f7a228 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.ClimberConstants; -import frc.robot.Constants.DriveConstants; import frc.robot.Constants.OIConstants; import frc.robot.LEDs.LEDs; import frc.robot.arm.Arm; @@ -234,8 +233,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.createStopIntakeCommand()); m_climber.setDefaultCommand(m_climber.createStopCommand()); m_LEDs.setDefaultCommand(m_LEDs.createDefaultLEDCommand()); @@ -249,9 +247,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))); - new JoystickButton(m_driver, OIConstants.kZorroDIn) .whileTrue(m_intake.createSetVoltageCommand(12.0) .onlyIf(m_arm.isArmRaised())); diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index 97feb60..bacd6c6 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/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,11 +119,17 @@ 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) { + public void setChassisSpeeds( + ChassisSpeeds chassisSpeeds, Optional rotationCenterOptional) { + Translation2d rotationCenter = + rotationCenterOptional.isPresent() ? rotationCenterOptional.get() : new Translation2d(); + var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod)); + ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod), rotationCenter); + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); @@ -129,10 +137,12 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { m_rearRight.setDesiredState(swerveModuleStates[3]); } - // uses kinematics type to determine robot center - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, SwerveDriveKinematics kinematicsType) { + /** + * @param chassisSpeeds Robot-relative chassis speeds (x, y, theta) + */ + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { var swerveModuleStates = - kinematicsType.toSwerveModuleStates( + DriveConstants.kDriveKinematics.toSwerveModuleStates( ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod)); SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); m_frontLeft.setDesiredState(swerveModuleStates[0]); diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java index ba9dcb3..5507ae4 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java @@ -3,12 +3,13 @@ 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.drivetrain.Drivetrain; +import java.util.Optional; public abstract class DriveCommand extends Command { @@ -16,17 +17,12 @@ public abstract class DriveCommand extends Command { 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 @@ -43,7 +39,7 @@ public void execute() { drivetrain.setChassisSpeeds( fieldRelative() ? getFieldRelativeChassisSpeeds() : getRobotRelativeChassisSpeeds(), - kinematicsType); + Optional.of(getSteeringCenter())); } private ChassisSpeeds getRobotRelativeChassisSpeeds() { @@ -79,4 +75,9 @@ private ChassisSpeeds getFieldRelativeChassisSpeeds() { * @return Boolean representing whether to drive in field-relative mode */ public abstract boolean fieldRelative(); + + /** + * @return Transform2d representing the vector between chassis center and desired steering center + */ + public abstract Translation2d getSteeringCenter(); } diff --git a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java index a0706b6..436986e 100644 --- a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java +++ b/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.OIConstants; 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; } @@ -41,4 +43,11 @@ public double getTheta() { public boolean fieldRelative() { return m_controller.getLeftBumper(); } + + @Override + public Translation2d getSteeringCenter() { + return m_controller.getRawButton(OIConstants.kZorroAIn) + ? ArmConstants.kChassisCentroidToArmCentroid + : new Translation2d(); + } } diff --git a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java index e41ba32..ab3fe17 100644 --- a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java @@ -3,18 +3,21 @@ 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.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; } @@ -37,4 +40,17 @@ public double getTheta() { public boolean fieldRelative() { return m_controller.getRawButton(OIConstants.kZorroEUp); } + + @Override + public Translation2d getSteeringCenter() { + // offset steering angle whenever arm is raised + // return m_arm.isArmRaised().getAsBoolean() + // ? ArmConstants.kChassisCentroidToArmCentroid + // : new Translation2d(); + + // offset steering angle when Zorro A button is pressed + return m_controller.getRawButton(OIConstants.kZorroAIn) + ? ArmConstants.kChassisCentroidToArmCentroid + : new Translation2d(); + } } From f80b4c2cdcc5c73f35a568e573be2014d71ad257 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Tue, 20 Feb 2024 20:43:47 -0500 Subject: [PATCH 2/9] return null when no translation is desired --- .../java/frc/robot/drivetrain/commands/XBoxDriveCommand.java | 2 +- .../java/frc/robot/drivetrain/commands/ZorroDriveCommand.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java index 436986e..72a06c8 100644 --- a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java @@ -48,6 +48,6 @@ public boolean fieldRelative() { public Translation2d getSteeringCenter() { return m_controller.getRawButton(OIConstants.kZorroAIn) ? ArmConstants.kChassisCentroidToArmCentroid - : new Translation2d(); + : null; } } diff --git a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java index ab3fe17..32b7b72 100644 --- a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java @@ -46,11 +46,11 @@ public Translation2d getSteeringCenter() { // offset steering angle whenever arm is raised // return m_arm.isArmRaised().getAsBoolean() // ? ArmConstants.kChassisCentroidToArmCentroid - // : new Translation2d(); + // : null; // offset steering angle when Zorro A button is pressed return m_controller.getRawButton(OIConstants.kZorroAIn) ? ArmConstants.kChassisCentroidToArmCentroid - : new Translation2d(); + : null; } } From fe66e17d492dfabf730253c2475dd7aa153012f3 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Tue, 20 Feb 2024 21:45:11 -0500 Subject: [PATCH 3/9] xboxcontroller should only reference xbox buttons --- .../java/frc/robot/drivetrain/commands/XBoxDriveCommand.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java index 72a06c8..bfc36cf 100644 --- a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.OIConstants; import frc.robot.drivetrain.Drivetrain; public class XBoxDriveCommand extends DriveCommand { @@ -46,7 +45,7 @@ public boolean fieldRelative() { @Override public Translation2d getSteeringCenter() { - return m_controller.getRawButton(OIConstants.kZorroAIn) + return m_controller.getRightBumper() ? ArmConstants.kChassisCentroidToArmCentroid : null; } From 406b388b96a99dfd8000518b6277de4312ff8b54 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Tue, 20 Feb 2024 22:05:13 -0500 Subject: [PATCH 4/9] spotless --- .../java/frc/robot/drivetrain/commands/XBoxDriveCommand.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java index bfc36cf..f2b4f19 100644 --- a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java @@ -45,8 +45,6 @@ public boolean fieldRelative() { @Override public Translation2d getSteeringCenter() { - return m_controller.getRightBumper() - ? ArmConstants.kChassisCentroidToArmCentroid - : null; + return m_controller.getRightBumper() ? ArmConstants.kChassisCentroidToArmCentroid : null; } } From 6450217013455395e3f273db253dc424991bc620 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sat, 24 Feb 2024 09:36:39 -0500 Subject: [PATCH 5/9] use arm state checker --- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/robot/drivetrain/commands/ZorroDriveCommand.java | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9014967..ef39887 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -217,7 +217,7 @@ public static enum ArmState { public static final Translation2d kChassisCentroidToArmCentroid = new Translation2d(-0.4572, 0.0); - + public static final int kFlapForwardChannel = 4; public static final int kFlapReverseChannel = 5; } diff --git a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java index 32b7b72..f382742 100644 --- a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java @@ -6,6 +6,7 @@ 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.OIConstants; import frc.robot.arm.Arm; import frc.robot.drivetrain.Drivetrain; @@ -44,7 +45,7 @@ public boolean fieldRelative() { @Override public Translation2d getSteeringCenter() { // offset steering angle whenever arm is raised - // return m_arm.isArmRaised().getAsBoolean() + // return m_arm.stateChecker(ArmState.DEPLOYED).getAsBoolean() // ? ArmConstants.kChassisCentroidToArmCentroid // : null; From c6fdee297df2e99872612ea7fa239ab474a0ec21 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 17 Mar 2024 10:33:54 -0400 Subject: [PATCH 6/9] enumerated teleop drive modes --- src/main/java/frc/robot/Constants.java | 7 +++ .../drivetrain/commands/DriveCommand.java | 43 +++++++++---------- .../drivetrain/commands/XBoxDriveCommand.java | 7 ++- .../commands/ZorroDriveCommand.java | 12 +++++- 4 files changed, 43 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e219836..69db7c2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java index 8c00100..56d1a94 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java @@ -3,11 +3,13 @@ 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; public abstract class DriveCommand extends Command { @@ -38,32 +40,29 @@ 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(), kinematicsType); } - 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 +79,7 @@ private ChassisSpeeds getFieldRelativeChassisSpeeds() { public abstract double getTheta(); /** - * @return Boolean representing whether to drive in field-relative mode + * @return The desired drive mode */ - public abstract boolean fieldRelative(); + public abstract DriveMode getDriveMode(); } diff --git a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java index a0706b6..84b0ba1 100644 --- a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.XboxController; import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.DriveConstants.DriveMode; import frc.robot.drivetrain.Drivetrain; public class XBoxDriveCommand extends DriveCommand { @@ -38,7 +39,9 @@ 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; } } diff --git a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java index e41ba32..ffde67d 100644 --- a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj.Joystick; +import frc.robot.Constants.DriveConstants.DriveMode; import frc.robot.Constants.OIConstants; import frc.robot.drivetrain.Drivetrain; @@ -34,7 +35,14 @@ 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; } } From 2d0df1df1a2df1a31ea791d47f90d44376fc9b0f Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 17 Mar 2024 10:57:28 -0400 Subject: [PATCH 7/9] deduplicate setChassisSpeeds method --- .../java/frc/robot/drivetrain/Drivetrain.java | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index bacd6c6..1abe62d 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -122,13 +122,12 @@ public void periodic() { * @param rotationCenter Vector from the chassis center to the desired center of rotation */ public void setChassisSpeeds( - ChassisSpeeds chassisSpeeds, Optional rotationCenterOptional) { - Translation2d rotationCenter = - rotationCenterOptional.isPresent() ? rotationCenterOptional.get() : new Translation2d(); + ChassisSpeeds chassisSpeeds, Optional rotationCenter) { + Translation2d offset = rotationCenter.isPresent() ? rotationCenter.get() : new Translation2d(); var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod), rotationCenter); + ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod), offset); SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); m_frontLeft.setDesiredState(swerveModuleStates[0]); @@ -141,14 +140,7 @@ public void setChassisSpeeds( * @param chassisSpeeds Robot-relative chassis speeds (x, y, theta) */ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { - var 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]); + setChassisSpeeds(chassisSpeeds, null); } /** Updates the field relative position of the robot. */ From 3ecdb90322ecd6b8bb2367e9a6d4579475e7eb37 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 17 Mar 2024 11:06:26 -0400 Subject: [PATCH 8/9] documentation --- src/main/java/frc/robot/drivetrain/commands/DriveCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java index 3c641f5..7f1d684 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java @@ -80,7 +80,7 @@ private ChassisSpeeds getChassisSpeeds() { public abstract DriveMode getDriveMode(); /** - * @return Transform2d representing the vector between chassis center and desired steering center + * @return The vector between chassis center and desired steering center */ public abstract Translation2d getSteeringCenter(); } From 05f6bf4c2419eb62c45a28534ef902b75fce118b Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 17 Mar 2024 11:35:56 -0400 Subject: [PATCH 9/9] don't create Translation2d when offset is null --- .../java/frc/robot/drivetrain/Drivetrain.java | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index 1abe62d..e81aa4d 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -123,11 +123,19 @@ public void periodic() { */ public void setChassisSpeeds( ChassisSpeeds chassisSpeeds, Optional rotationCenter) { - Translation2d offset = rotationCenter.isPresent() ? rotationCenter.get() : new Translation2d(); - var swerveModuleStates = - DriveConstants.kDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod), offset); + 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]);