diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 915335b..d01a5be 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -34,35 +34,37 @@ public final class Constants { public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; - + + public static final double[] GEARS = {0.25, 0.5, 1.0}; + public static final double SLOW_SPEED = 0.25; public static final double FAST_SPEED = 0.5; public static final double TURBO_SPEED = 1.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 +0.5 ;//+ 0.5; - public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 +0.5 ;//+ 0.5; - public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875+0.5 ; - public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 +0.5 ; + public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 + 0.5; + public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 + 0.5; + public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875 + 0.5; + public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 + 0.5; } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 2; - public static final int RIGHT_FRONT_STEER_ID = 3; - public static final int RIGHT_FRONT_ENCODER_ID = 10; - - public static final int LEFT_FRONT_WHEEL_ID = 4; - public static final int LEFT_FRONT_STEER_ID = 5; - public static final int LEFT_FRONT_ENCODER_ID = 11; - - public static final int LEFT_BACK_WHEEL_ID = 6; - public static final int LEFT_BACK_STEER_ID = 7; - public static final int LEFT_BACK_ENCODER_ID = 12; - - public static final int RIGHT_BACK_WHEEL_ID = 8; - public static final int RIGHT_BACK_STEER_ID = 9; - public static final int RIGHT_BACK_ENCODER_ID = 13; + public static final int RIGHT_FRONT_WHEEL_ID = 2; + public static final int RIGHT_FRONT_STEER_ID = 3; + public static final int RIGHT_FRONT_ENCODER_ID = 10; + + public static final int LEFT_FRONT_WHEEL_ID = 4; + public static final int LEFT_FRONT_STEER_ID = 5; + public static final int LEFT_FRONT_ENCODER_ID = 11; + + public static final int LEFT_BACK_WHEEL_ID = 6; + public static final int LEFT_BACK_STEER_ID = 7; + public static final int LEFT_BACK_ENCODER_ID = 12; + + public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_STEER_ID = 9; + public static final int RIGHT_BACK_ENCODER_ID = 13; } public static final class PIDConstants { @@ -85,8 +87,6 @@ public final class Constants { } public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 1; - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5; @@ -107,13 +107,13 @@ public final class Constants { } public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + public static final double OPEN_LOOP_RAMP_RATE = 0.2; + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; + public static final double NEUTRAL_DEADBAND = 0.04; } - public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; // TODO: find the actual value - public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value + public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // dimensions public static final double WIDTH = 18.5; @@ -127,23 +127,6 @@ public final class Constants { } public static final class VisionConstants { - // public static final String NAME = "photonCamera"; - - // public static final int LIME_HIXELS = 640; - // public static final int LIME_VIXELS = 480; - - // public static final double H_FOV = 59.6; - // public static final double V_FOV = 45.7; - - // public static final double LIME_HEIGHT = 6.0; - // public static final double LIME_ANGLE = 55.0; - - // // public static final double HIGH_TARGET_HEIGHT = 46.0; - // public static final double HIGH_TAPE_HEIGHT = 44.0; - - // // public static final double MID_TARGET_HEIGHT = 34.0; - // public static final double MID_TAPE_HEIGHT = 24.0; - // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); @@ -153,11 +136,6 @@ public final class Constants { public static final double targetPosDistance = 1.5; } - - public static final class AutoAlignConstants { - public static final double MoveSpeed = 0.0; - public static final double RotSpeed = 0.0; - } public static final class DriveConstants { public static final int DRIVE_PIGEON_ID = 14; @@ -202,6 +180,7 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; + public static final int XBOX_PROGRAMMER_ID = 2; public static final double LEFT_AXIS_DEADBAND = 0.1; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 262a593..5a178f5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -69,7 +69,7 @@ public class RobotContainer { /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); + private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID); private final Limelight limelight = new Limelight(); @@ -77,7 +77,6 @@ public class RobotContainer { private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); - /* Virtual Controllers */ private final VirtualController m_virtualDriver = new VirtualController(0); private final VirtualController m_virtualOperator = new VirtualController(1); @@ -123,9 +122,9 @@ public class RobotContainer { private String lastAutoName = "four_note_taxi_kracken.auto"; private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, - lastAutoName, // () -> autoplaybackName.get(), // lastAutoName + () -> autoplaybackName.get(), // lastAutoName new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, true); + true, false); private neoJoystickPlayback amp_shoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto", new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, @@ -195,11 +194,10 @@ public class RobotContainer { // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); @@ -207,7 +205,6 @@ public class RobotContainer { new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - // ? /* Operator Buttons */ DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 70f05b6..bd35742 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -33,10 +33,13 @@ public class SwerveDrive extends SubsystemBase { private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - + private RobotGyro gyro; + + private int gear_index; + private boolean stopped = false; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; @@ -52,7 +55,7 @@ public class SwerveDrive extends SubsystemBase { this.rightBack = rightBack; this.gyro = gyro; - + reset_index(); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } @@ -68,7 +71,6 @@ public class SwerveDrive extends SubsystemBase { module.setDesiredState(state); } - boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; @@ -256,15 +258,29 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putNumber("RotTartget", rotTarget); } - public void shiftDown() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; - } else { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + private void reset_index() { + gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?) + } - } + public void shiftDown() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index - 1; + if (i == -1) i = 0; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void shiftUp() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index + 1; + if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void setPercentOutput(double speed) { + speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed; + gear_index = -1; } public void setToSlow() { @@ -294,16 +310,6 @@ public class SwerveDrive extends SubsystemBase { System.out.println("TURBO"); } - public void shiftUp() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; - } else { - - } - } - public void toggleGear(double angle) { if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;