diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e219836..dc41acb 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; @@ -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; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 860a3f3..ebcb2f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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); diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index 97feb60..e81aa4d 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,24 @@ 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 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]); @@ -129,16 +144,11 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { 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. */ diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java index 8c00100..7f1d684 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java @@ -3,12 +3,14 @@ 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 { @@ -16,17 +18,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 @@ -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(); } diff --git a/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/XBoxDriveCommand.java index a0706b6..a838458 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.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; } } diff --git a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java index e41ba32..aba74d7 100644 --- a/src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java +++ b/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; } }