From 700667cb7c93c4e6267a13bfd0960ab06868a2d4 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Fri, 26 Jul 2024 20:32:28 -0400 Subject: [PATCH 1/3] gives extra time before arm automatically goes back to feed after amping --- .../robot/commands/shooter/ShooterScoreAmp.java | 2 +- .../com/stuypulse/robot/constants/Settings.java | 15 ++++++++------- .../stuypulse/robot/subsystems/arm/ArmImpl.java | 15 +++++++++++++-- 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java index 2019f603..b64e5773 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java @@ -12,7 +12,7 @@ public ShooterScoreAmp() { addCommands( new ShooterFeederDeacquire(), new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()), - new WaitUntilCommand(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME.get()), + new WaitUntilCommand(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME), new ShooterFeederStop() ); } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 88658606..6a5916ad 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -56,22 +56,23 @@ public static RobotType fromString(String serialNum) { public interface Arm { double LENGTH = Units.inchesToMeters(16.5); - SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", 425); - SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", 450); + SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", 400); + SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", 425); SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 100); SmartNumber MIN_ANGLE = new SmartNumber("Arm/Min Angle (deg)", -90 + 12.25); - SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2); + SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2.5); SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 70); SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", -50); SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50); SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80); - SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.getAsDouble() + 16); + SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.getAsDouble() + 17); SmartNumber PODIUM_SHOT_ANGLE = new SmartNumber("Arm/Podium Shot Angle", -60); - SmartNumber SHOULD_RETURN_TO_FEED_TIME = new SmartNumber("Arm/Return To Feed Time", 1.0); + double SHOULD_RETURN_TO_FEED_TIME = 1.0; + double EXTRA_TIME_BEFORE_RETURNING_TO_FEED_FOR_AMP = 2.5; SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02); @@ -331,7 +332,7 @@ public interface Driver { double DOUBLE_CLICK_TIME_BETWEEN_CLICKS = 0.5; public interface Drive { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.015); SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.01); SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); @@ -341,7 +342,7 @@ public interface Drive { } public interface Turn { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.03); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.015); SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 5d342af6..26b8c0d4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -26,7 +26,6 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandScheduler; public class ArmImpl extends Arm { @@ -41,6 +40,7 @@ public class ArmImpl extends Arm { private final MotionProfile motionProfile; private final BStream shouldGoBackToFeed; + private final BStream shouldGoBackToFeedFromAmp; protected ArmImpl() { super(); @@ -69,6 +69,11 @@ protected ArmImpl() { shouldGoBackToFeed = BStream.create(() -> !Shooter.getInstance().hasNote()) .filtered(new BDebounce.Rising(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME)); + + shouldGoBackToFeedFromAmp = BStream.create(shouldGoBackToFeed) + .filtered(new BDebounce.Rising(Settings.Arm.EXTRA_TIME_BEFORE_RETURNING_TO_FEED_FOR_AMP)) + .and(BStream.create(() -> !overriding) + .filtered(new BDebounce.Rising(Settings.Arm.EXTRA_TIME_BEFORE_RETURNING_TO_FEED_FOR_AMP))); } @Override @@ -184,7 +189,13 @@ public void periodic() { this.actualState = this.requestedState; } else if (shouldGoBackToFeed.get()) { - if (actualState != State.FEED) { + if (actualState == State.AMP) { + if (shouldGoBackToFeedFromAmp.get()) { + requestedState = State.FEED; + actualState = State.FEED; + } + } + else if (actualState != State.FEED) { requestedState = State.FEED; actualState = State.FEED; } From 52d42e4165621265872bb93d5092bbf162e5fdd7 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Fri, 26 Jul 2024 20:32:52 -0400 Subject: [PATCH 2/3] buzz controller to indicate no note detected, add deadband for swervedrivedrive --- src/main/java/com/stuypulse/robot/RobotContainer.java | 9 ++++++--- .../robot/commands/swerve/SwerveDriveDrive.java | 5 ++++- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b41d3163..96453de6 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -123,7 +123,8 @@ private void configureButtonBindings() { .onFalse(new ShooterSetRPM(new ShooterSpeeds()).onlyIf(() -> !Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); driver.getTopButton() - .onTrue(new ArmToSpeaker()); + .onTrue(new ArmToSpeaker()) + .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); driver.getTopButton() .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) @@ -131,7 +132,8 @@ private void configureButtonBindings() { .onFalse(new ArmDisableOverride()); driver.getLeftButton() - .onTrue(new ArmToAmp()); + .onTrue(new ArmToAmp()) + .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); driver.getLeftButton() .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) @@ -139,7 +141,8 @@ private void configureButtonBindings() { .onFalse(new ArmDisableOverride()); driver.getRightButton() - .whileTrue(new ArmToFerry()); + .whileTrue(new ArmToFerry()) + .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); driver.getRightButton() .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index d1a50fc5..32bca12b 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -49,7 +49,10 @@ public SwerveDriveDrive(Gamepad driver) { this.driver = driver; - drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + drive = new SwerveRequest.FieldCentric() + .withDeadband(Settings.Swerve.MAX_LINEAR_VELOCITY * Settings.Driver.Drive.DEADBAND.get()) + .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); addRequirements(swerve); } From fdcee0c5dc70ad73ee7da3819266262d2c4201e7 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 27 Jul 2024 00:38:35 -0400 Subject: [PATCH 3/3] interpolated lob ferry --- .../util/ShooterLobFerryInterpolation.java | 36 +++++++++++++++++-- 1 file changed, 33 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java index c2a2250a..06ddca65 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java @@ -14,9 +14,39 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { // fake data im just putting here so it doesnt crash lol - {1000, 55.5}, - {1000, 42.5}, - {1000, 57}, + {3100,382}, + {3100,292}, + {3100,337}, + {3100,337}, + {3100,290}, + {3100,300}, + {3200,335}, + {3200,349}, + {3200,337}, + {3200,339}, + {3300,379}, + {3300,384}, + {3300,389}, + {3300,373}, + {3300,354}, + {3500,389}, + {3500,331}, + {2500,238}, + {2500,233}, + {2500,259}, + {2000,169}, + {2500,288}, + {2000,164}, + {2000,191}, + {1500,109}, + {1500,109}, + {1500,108}, + {1500,105}, + {1200,79}, + {1200,82}, + {1200,67}, + {1200,69}, + {1200,64}, }; static {