diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 17d6f24..2ef6506 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -207,8 +207,15 @@ public final class Constants { public static final double SHOOTER_TURRET_MIN = -TURRET_SPEED_MULTIPLIER; //Gains for hood public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); - public static final double TURRET_FORWARD_LIMIT = 0.0; - public static final double TURRET_REVERSE_LIMIT = -95.0; //Find + + //#region test start + + //#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_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 2; + public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; //Shooter gains for actual Drum public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index fbb7cfa..3704dd7 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -204,7 +204,7 @@ public class Robot extends TimedRobot { LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } - m_robotContainer.m_robotVisionOdometry.setLEDs(true); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ebd9715..d9b8e08 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -166,10 +166,10 @@ public class RobotContainer { getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - m_robotStorage.setDefaultCommand( - new ManageStorage(m_robotStorage, - m_robotBoomBoom, - m_robotTurret).withName("Storage ManageStorage defaultCommand")); + // m_robotStorage.setDefaultCommand( + // new ManageStorage(m_robotStorage, + // m_robotBoomBoom, + // m_robotTurret).withName("Storage ManageStorage defaultCommand")); // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 7f8a8db..38a8888 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -69,7 +69,7 @@ public class AimToCenter extends CommandBase { if (angle == Double.NaN) { return false; } - return !((ShooterConstants.TURRET_REVERSE_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_LIMIT)); + return !((ShooterConstants.TURRET_REVERSE_SOFT_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_SOFT_LIMIT)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 34cd555..001208f 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -17,18 +17,18 @@ public class Extender extends SubsystemBase { private CANSparkMax m_extenderMotor; - private SparkMaxLimitSwitch m_inLimit; - private SparkMaxLimitSwitch m_outLimit; + // private SparkMaxLimitSwitch m_inLimit; + // private SparkMaxLimitSwitch m_outLimit; /** Creates a new Extender. */ public Extender(CANSparkMax extenderMotor) { m_extenderMotor = extenderMotor; - m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_inLimit.enableLimitSwitch(false); - m_outLimit.enableLimitSwitch(false); + // m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_inLimit.enableLimitSwitch(false); + // m_outLimit.enableLimitSwitch(false); m_extenderMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ExtenderConstants.EXTENDER_FORWARD_LIMIT); m_extenderMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ExtenderConstants.EXTENDER_REVERSE_LIMIT); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index d2dc34f..87e74d9 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -37,23 +37,35 @@ 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; + + long leftCurrentTime; + long rightCurrentTime; + long leftElapsedTime; + long rightElapsedTime; + public Turret(CANSparkMax boomBoomRotateMotor) { m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + 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); - // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); - // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); - - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); setTurretSoftLimits(true); - + setTurretPIDGains(); } @@ -68,19 +80,53 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone); m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput); } - + @Override 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.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()); - if (m_boomBoomLeftLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_LIMIT - 2); - if (m_boomBoomRightLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_LIMIT + 2); + // limit switch annoying time thing + leftState = m_boomBoomLeftLimit.isPressed(); + rightState = m_boomBoomRightLimit.isPressed(); + + hasLeftSwitchChanged = (leftState != leftPrevState); + hasRightSwitchChanged = (rightState != rightPrevState); + + if (leftState && hasLeftSwitchChanged) { + leftCurrentTime = System.currentTimeMillis(); + leftElapsedTime = 0; + } + + if (rightState && hasRightSwitchChanged) { + rightCurrentTime = System.currentTimeMillis(); + rightElapsedTime = 0; + } + + if (leftState && !hasLeftSwitchChanged) { + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } + + 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*/); + } + + leftPrevState = leftState; + rightPrevState = rightState; } /** @@ -110,7 +156,7 @@ public class Turret extends SubsystemBase { * @param input from -1.0 to 1.0, positive is clockwise */ public void runTurretWithInput(double input) { - m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * 0.5); } public void runShooterRotatePID(double targetAngle) {