diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 1d01271..cc96886 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -37,19 +37,13 @@ public class Turret extends SubsystemBase { SparkMaxLimitSwitch m_boomBoomLeftLimit; SparkMaxLimitSwitch m_boomBoomRightLimit; - boolean hasLeftSwitchChanged = false; - boolean hasRightSwitchChanged = false; - - boolean leftPrevState = false; - boolean rightPrevState = false; boolean leftState; - boolean rightState; boolean recentlyPressed; + boolean leftPrevState = false; + boolean hasLeftSwitchChanged = false; long leftCurrentTime; - long rightCurrentTime; long leftElapsedTime; - long rightElapsedTime; public Turret(CANSparkMax boomBoomRotateMotor) { @@ -120,22 +114,15 @@ public class Turret extends SubsystemBase { SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); // limit switch annoying time thing but actually worked first try wtf - leftState = m_boomBoomLeftLimit.isPressed(); + leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed). - hasLeftSwitchChanged = (leftState != leftPrevState); + hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state. - if (leftState && hasLeftSwitchChanged) { + if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time. leftCurrentTime = System.currentTimeMillis(); leftElapsedTime = 0; } - if (leftState && !hasLeftSwitchChanged) { - leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; - } - - if (leftState && (leftElapsedTime > 500)) { - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); - } if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false; if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){ @@ -144,8 +131,15 @@ public class Turret extends SubsystemBase { } SmartDashboard.putBoolean("Recently Pressed", recentlyPressed); - leftPrevState = leftState; - // rightPrevState = rightState; + if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time. + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } + + if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position. + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); + } + + leftPrevState = leftState; // * Update the state of the left limit switch. } /**