diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5e559e0..f87a507 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -191,6 +191,7 @@ public final class Constants { public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; public static final double TURRET_SPEED_MULTIPLIER = 0.6; + public static final double TURRET_CALIBRATION_MULTIPLIER = 0.5; public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; @@ -212,7 +213,7 @@ public final class Constants { //#endregion test end public static final double TURRET_FORWARD_HARD_LIMIT = 0.0; - public static final double TURRET_REVERSE_HARD_LIMIT = -85.0; + public static final double TURRET_REVERSE_HARD_LIMIT = -123.0; public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 2; public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ce9f0c1..d5967b9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -359,6 +359,10 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + .whileHeld(new RunCommand(() -> m_robotTurret.turnOnLeftLimitSwitch(), m_robotTurret)) + .whenReleased(new RunCommand(() -> m_robotTurret.turnOffLeftLimitSwitch(), m_robotTurret)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 87e74d9..f94b44d 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -56,19 +56,42 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - m_boomBoomLeftLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_boomBoomRightLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomLeftLimit.enableLimitSwitch(true); - // m_boomBoomRightLimit.enableLimitSwitch(true); - setTurretLimitSwitches(true); + // m_boomBoomLeftLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_boomBoomRightLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_boomBoomLeftLimit.enableLimitSwitch(false); + // m_boomBoomRightLimit.enableLimitSwitch(false); + // setTurretLimitSwitches(true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - setTurretSoftLimits(true); + setTurretSoftLimits(false); setTurretPIDGains(); } + + public void toggleLeftLimitSwitch() { + // TODO: find better way to do this, but im in a hurry + + + // if (leftSwitch.isLimitSwitchEnabled()) { + // leftSwitch.enableLimitSwitch(false); + // } else { + // leftSwitch.enableLimitSwitch(true); + } + // } + public void turnOnLeftLimitSwitch() { + SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + System.out.println("Left Switch ENABLED"); + leftSwitch.enableLimitSwitch(true); + } + + public void turnOffLeftLimitSwitch() { + SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + System.out.println("Left Switch DISABLED"); + leftSwitch.enableLimitSwitch(false); + } + /** * Set gains for turret PIDs. */ @@ -85,48 +108,48 @@ public class Turret extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); - SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); - SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); - SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); + // SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); + // SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); - // limit switch annoying time thing - leftState = m_boomBoomLeftLimit.isPressed(); - rightState = m_boomBoomRightLimit.isPressed(); + // limit switch annoying time thing but actually worked first try wtf + // leftState = m_boomBoomLeftLimit.isPressed(); + // rightState = m_boomBoomRightLimit.isPressed(); - hasLeftSwitchChanged = (leftState != leftPrevState); - hasRightSwitchChanged = (rightState != rightPrevState); + // hasLeftSwitchChanged = (leftState != leftPrevState); + // hasRightSwitchChanged = (rightState != rightPrevState); - if (leftState && hasLeftSwitchChanged) { - leftCurrentTime = System.currentTimeMillis(); - leftElapsedTime = 0; - } + // if (leftState && hasLeftSwitchChanged) { + // leftCurrentTime = System.currentTimeMillis(); + // leftElapsedTime = 0; + // } - if (rightState && hasRightSwitchChanged) { - rightCurrentTime = System.currentTimeMillis(); - rightElapsedTime = 0; - } + // if (rightState && hasRightSwitchChanged) { + // rightCurrentTime = System.currentTimeMillis(); + // rightElapsedTime = 0; + // } - if (leftState && !hasLeftSwitchChanged) { - leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; - } + // if (leftState && !hasLeftSwitchChanged) { + // leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + // } - if (rightState && !hasRightSwitchChanged) { - rightElapsedTime = System.currentTimeMillis() - rightCurrentTime; - } + // if (rightState && !hasRightSwitchChanged) { + // rightElapsedTime = System.currentTimeMillis() - rightCurrentTime; + // } - if (leftState && (leftElapsedTime > 500)) { - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// -95/*ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - 2*/); - } - if (rightState && (rightElapsedTime > 500)) { - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); - } + // if (leftState && (leftElapsedTime > 500)) { + // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// -95/*ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - 2*/); + // } + // if (rightState && (rightElapsedTime > 500)) { + // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); + // } - leftPrevState = leftState; - rightPrevState = rightState; + // leftPrevState = leftState; + // rightPrevState = rightState; } /**