From 2c18973dfaad0ffea193804a89c5c79810fed3f9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 11:18:08 -0600 Subject: [PATCH] fixed --- src/main/java/frc4388/robot/Constants.java | 6 +- src/main/java/frc4388/robot/Robot.java | 6 +- .../java/frc4388/robot/RobotContainer.java | 6 +- .../java/frc4388/robot/subsystems/Turret.java | 94 +++++++++---------- 4 files changed, 54 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f87a507..0273228 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -190,7 +190,7 @@ public final class Constants { true, 27, 0, 0); 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_SPEED_MULTIPLIER = 0.8; 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; @@ -212,8 +212,8 @@ public final class Constants { //#region test start //#endregion test end - public static final double TURRET_FORWARD_HARD_LIMIT = 0.0; - public static final double TURRET_REVERSE_HARD_LIMIT = -123.0; + public static final double TURRET_FORWARD_HARD_LIMIT = 18.4; + public static final double TURRET_REVERSE_HARD_LIMIT = -104.3; 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/Robot.java b/src/main/java/frc4388/robot/Robot.java index 3704dd7..1d2b91f 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -142,15 +142,15 @@ public class Robot extends TimedRobot { // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); current = - // m_robotContainer.m_robotBoomBoom.getCurrent() + + m_robotContainer.m_robotBoomBoom.getCurrent() + m_robotContainer.m_robotClimber.getCurrent() + - // m_robotContainer.m_robotHood.getCurrent() + + m_robotContainer.m_robotHood.getCurrent() + m_robotContainer.m_robotIntake.getCurrent() + m_robotContainer.m_robotExtender.getCurrent() + m_robotContainer.m_robotSerializer.getCurrent() + m_robotContainer.m_robotStorage.getCurrent() + m_robotContainer.m_robotSwerveDrive.getCurrent(); - // m_robotContainer.m_robotTurret.getCurrent(); + m_robotContainer.m_robotTurret.getCurrent(); SmartDashboard.putNumber("Total Robot Current Draw", current); SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d5967b9..7253a31 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -359,9 +359,9 @@ 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.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)) diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index f94b44d..1d01271 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -44,6 +44,7 @@ public class Turret extends SubsystemBase { boolean rightPrevState = false; boolean leftState; boolean rightState; + boolean recentlyPressed; long leftCurrentTime; long rightCurrentTime; @@ -56,20 +57,22 @@ 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(false); - // m_boomBoomRightLimit.enableLimitSwitch(false); - // setTurretLimitSwitches(true); - + // setTurretSoftLimits(true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - setTurretSoftLimits(false); + + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // setTurretLimitSwitches(true); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); setTurretPIDGains(); } - public void toggleLeftLimitSwitch() { + // public void toggleLeftLimitSwitch() { // TODO: find better way to do this, but im in a hurry @@ -77,20 +80,20 @@ public class Turret extends SubsystemBase { // 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 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); - } + // 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. @@ -113,42 +116,35 @@ public class Turret extends SubsystemBase { 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 but actually worked first try wtf - // leftState = m_boomBoomLeftLimit.isPressed(); - // rightState = m_boomBoomRightLimit.isPressed(); + leftState = m_boomBoomLeftLimit.isPressed(); - // hasLeftSwitchChanged = (leftState != leftPrevState); - // hasRightSwitchChanged = (rightState != rightPrevState); + hasLeftSwitchChanged = (leftState != leftPrevState); - // if (leftState && hasLeftSwitchChanged) { - // leftCurrentTime = System.currentTimeMillis(); - // leftElapsedTime = 0; - // } - - // if (rightState && hasRightSwitchChanged) { - // rightCurrentTime = System.currentTimeMillis(); - // rightElapsedTime = 0; - // } + if (leftState && hasLeftSwitchChanged) { + leftCurrentTime = System.currentTimeMillis(); + leftElapsedTime = 0; + } - // if (leftState && !hasLeftSwitchChanged) { - // leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; - // } - - // if (rightState && !hasRightSwitchChanged) { - // rightElapsedTime = System.currentTimeMillis() - rightCurrentTime; - // } + 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 (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(m_boomBoomRightLimit.isPressed() && !recentlyPressed){ + recentlyPressed = true; + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); + } + SmartDashboard.putBoolean("Recently Pressed", recentlyPressed); - // leftPrevState = leftState; + leftPrevState = leftState; // rightPrevState = rightState; } @@ -179,7 +175,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 * 0.5); + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); } public void runShooterRotatePID(double targetAngle) {