From 9814c181f22d5fe5bd586a4d37196e8c20710a4d Mon Sep 17 00:00:00 2001 From: Ian Shi <69531533+IanShiii@users.noreply.github.com> Date: Mon, 2 Sep 2024 18:57:12 -0400 Subject: [PATCH] 8/28 (#37) (#38) * fix camera rotations * get rid of outlier data * move automatically scheduled commands to triggers inside robot container * potential fix for having to double click to shoot * feeder runs backwards on deacquire, dont retry intake * tune amp angle, fix intake led colors * switch ferry buttons * added arm climb commands * clean up climb commands and default led instruction * use old low ferry data as lob ferry data, fix arm angles for ferrying * sysid swerve --------- Co-authored-by: Tmanxyz --- src/main/java/com/stuypulse/robot/Robot.java | 27 ----- .../com/stuypulse/robot/RobotContainer.java | 71 ++++++++---- .../robot/commands/leds/LEDDefaultMode.java | 4 + .../shooter/ShooterAcquireFromIntake.java | 25 ----- .../ShooterAcquireFromIntakeWithRetry.java | 64 +++++++++++ .../stuypulse/robot/constants/Cameras.java | 4 +- .../robot/constants/LEDInstructions.java | 6 +- .../stuypulse/robot/constants/Settings.java | 14 +-- .../util/ShooterLobFerryInterpolation.java | 106 ++++++++---------- .../util/ShooterLowFerryInterpolation.java | 82 +------------- 10 files changed, 182 insertions(+), 221 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 490a5d6..e175561 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -57,33 +57,6 @@ public void robotInit() { @Override public void robotPeriodic() { - if (Arm.getInstance().getState() == Arm.State.FEED - && Arm.getInstance().atTarget() - && !Shooter.getInstance().hasNote() - && Intake.getInstance().hasNote() - && Intake.getInstance().getState() != Intake.State.DEACQUIRING - ) { - CommandScheduler.getInstance().schedule(new ShooterAcquireFromIntake() - .andThen(new BuzzController(robot.driver))); - } - - if (Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED - && Arm.getInstance().atIntakeShouldShootAngle() - ) { - CommandScheduler.getInstance().schedule(new IntakeShoot() - .until( - () -> Arm.getInstance().getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED - || !Arm.getInstance().atIntakeShouldShootAngle() - )); - } - - if (Arm.getInstance().getState() == Arm.State.AMP - && !Shooter.getInstance().hasNote() - && Shooter.getInstance().getFeederState() != Shooter.FeederState.DEACQUIRING - ) { - CommandScheduler.getInstance().schedule(new ShooterManualIntake().until(() -> Arm.getInstance().getState() != Arm.State.AMP)); - } - CommandScheduler.getInstance().run(); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1c1534a..241f2a7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,6 +24,7 @@ import com.stuypulse.robot.commands.leds.LEDReset; import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; +import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntakeWithRetry; import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; @@ -71,6 +72,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class RobotContainer { @@ -124,10 +126,18 @@ private void configureDefaultCommands() { private void configureButtonBindings() { configureOperatorBindings(); configureDriverBindings(); + configureAutomaticCommandScheduling(); } private void configureDriverBindings() { - driver.getDPadUp().onTrue(new SwerveDriveSeedFieldRelative()); + driver.getDPadRight().onTrue(new SwerveDriveSeedFieldRelative()); + + driver.getDPadUp() + .onTrue(new ArmToPreClimb()) + .onTrue(new ShooterStop()) + .onTrue(new IntakeStop()); + + driver.getDPadDown().onTrue(new ArmToClimbing()); // intake field relative driver.getRightTriggerButton() @@ -150,23 +160,22 @@ private void configureDriverBindings() { // deacquire driver.getDPadLeft() .whileTrue(new IntakeDeacquire()) + .whileTrue(new ShooterFeederDeacquire()) .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING)); // speaker align and score // score amp driver.getRightBumper() .whileTrue(new ConditionalCommand( - new ArmWaitUntilAtTarget() - .withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) - .andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE))), + new SwerveDriveDrive(driver) + .alongWith(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) + .andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE)))), new SwerveDriveDriveAlignedSpeaker(driver) .alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)), () -> Arm.getInstance().getState() == Arm.State.AMP)) @@ -182,9 +191,7 @@ private void configureDriverBindings() { .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)) ) @@ -201,9 +208,7 @@ private void configureDriverBindings() { .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) ) @@ -220,9 +225,7 @@ private void configureDriverBindings() { .whileTrue(new ArmToSubwooferShot().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL)) .onFalse(new ConditionalCommand( @@ -237,9 +240,7 @@ private void configureDriverBindings() { .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLobFerry())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)) ) @@ -274,6 +275,38 @@ private void configureOperatorBindings() { } + private void configureAutomaticCommandScheduling() { + // automatic handoff + new Trigger(() -> arm.getState() == Arm.State.FEED + && arm.atTarget() + && !shooter.hasNote() + && intake.hasNote() + && intake.getState() != Intake.State.DEACQUIRING) + // .onTrue(new ShooterAcquireFromIntakeWithRetry().andThen(new BuzzController(driver))); + .onTrue(new ShooterAcquireFromIntake().andThen(new BuzzController(driver))); + + // feeder automatically pushes note further into shooter when its sticking too far out + new Trigger(() -> arm.getState() == Arm.State.AMP + && !shooter.hasNote() + && shooter.getFeederState() != Shooter.FeederState.DEACQUIRING) + .onTrue(new ShooterManualIntake().until(() -> arm.getState() != Arm.State.AMP)); + + // run the intake when the arm is moving up from a low angle (to prevent intake from gripping it) + new Trigger(() -> arm.getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED + && arm.atIntakeShouldShootAngle()) + .onTrue(new IntakeShoot() + .until( + () -> arm.getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED + || !arm.atIntakeShouldShootAngle() + ) + ); + + // run the intake when shooting in case the intake is holding onto the note also + new Trigger(() -> shooter.getFeederState() == Shooter.FeederState.SHOOTING + && arm.atIntakeShouldShootAngle()) + .onTrue(new IntakeShoot().until(() -> shooter.getFeederState() != Shooter.FeederState.SHOOTING)); + } + /**************/ /*** AUTONS ***/ /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java index 2fdd384..dc7bfc6 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java @@ -39,6 +39,10 @@ public LEDDefaultMode() { private LEDInstruction getInstruction() { + if (Arm.getInstance().getState() == Arm.State.CLIMBING) { + return LEDInstructions.CLIMBING; + } + if (Arm.getInstance().getState() == Arm.State.AMP) { return LEDInstructions.ARM_AT_AMP; } diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java index 95c297e..a35138c 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java @@ -12,44 +12,19 @@ public class ShooterAcquireFromIntake extends Command { private final Shooter shooter; private final Intake intake; - private final StopWatch stopWatch; - private boolean isFeeding; - public ShooterAcquireFromIntake() { shooter = Shooter.getInstance(); intake = Intake.getInstance(); - stopWatch = new StopWatch(); - isFeeding = true; - addRequirements(shooter, intake); } @Override public void initialize() { - stopWatch.reset(); intake.setState(Intake.State.FEEDING); shooter.setFeederState(Shooter.FeederState.INTAKING); } - @Override - public void execute() { - if (isFeeding) { - if (stopWatch.getTime() > Settings.Intake.HANDOFF_TIMEOUT) { - intake.setState(Intake.State.DEACQUIRING); - isFeeding = false; - stopWatch.reset(); - } - } - else { - if (stopWatch.getTime() > Settings.Intake.MINIMUM_DEACQUIRE_TIME_WHEN_STUCK && intake.hasNote()) { - intake.setState(Intake.State.FEEDING); - isFeeding = true; - stopWatch.reset(); - } - } - } - @Override public boolean isFinished() { return shooter.hasNote(); diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java new file mode 100644 index 0000000..3d6ceef --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java @@ -0,0 +1,64 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.stuylib.util.StopWatch; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ShooterAcquireFromIntakeWithRetry extends Command { + + private final Shooter shooter; + private final Intake intake; + + private final StopWatch stopWatch; + private boolean isFeeding; + + public ShooterAcquireFromIntakeWithRetry() { + shooter = Shooter.getInstance(); + intake = Intake.getInstance(); + + stopWatch = new StopWatch(); + isFeeding = true; + + addRequirements(shooter, intake); + } + + @Override + public void initialize() { + stopWatch.reset(); + intake.setState(Intake.State.FEEDING); + shooter.setFeederState(Shooter.FeederState.INTAKING); + } + + @Override + public void execute() { + if (isFeeding) { + if (stopWatch.getTime() > Settings.Intake.HANDOFF_TIMEOUT) { + intake.setState(Intake.State.DEACQUIRING); + isFeeding = false; + stopWatch.reset(); + } + } + else { + if (stopWatch.getTime() > Settings.Intake.MINIMUM_DEACQUIRE_TIME_WHEN_STUCK && intake.hasNote()) { + intake.setState(Intake.State.FEEDING); + isFeeding = true; + stopWatch.reset(); + } + } + } + + @Override + public boolean isFinished() { + return shooter.hasNote(); + } + + @Override + public void end(boolean interrupted) { + shooter.setFeederState(Shooter.FeederState.STOP); + intake.setState(Intake.State.STOP); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 8f69290..3bc76a2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -28,7 +28,7 @@ public interface Limelight { "tower-cam", new Pose3d( new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(+3.333797), Units.inchesToMeters(23.929362)), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(180)) + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-15), Units.degreesToRadians(180)) ), "11", 3000 @@ -38,7 +38,7 @@ public interface Limelight { "plate-cam", new Pose3d( new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591), Units.inchesToMeters(19.216471)), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(80), Units.degreesToRadians(0)) + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-10), Units.degreesToRadians(0)) ), "96", 3001 diff --git a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java index 3644885..9718f24 100644 --- a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java +++ b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java @@ -55,8 +55,8 @@ public interface LEDInstructions { LEDInstruction DEFAULT = LEDInstructions.OFF; - LEDInstruction FIELD_RELATIVE_INTAKING = new LEDRainbow(); - LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE); + LEDInstruction FIELD_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE); + LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDRainbow(); LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.GOLD); LEDInstruction SPEAKER_ALIGN = GREEN; @@ -71,6 +71,8 @@ public interface LEDInstructions { LEDInstruction ARM_AT_AMP = YELLOW; LEDInstruction AMP_SCORE = GREEN; + LEDInstruction CLIMBING = new LEDRainbow(); + LEDInstruction ATTENTION = new LED694(0.01, SLColor.BLUE); LEDInstruction CONTAINS_NOTE = RED; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index df11a84..58432d5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -66,11 +66,11 @@ public interface Arm { SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2.5); - SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 43); - 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 POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get() + 7); + SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 49); + SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", MIN_ANGLE.get()); + SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", -50); + SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 90); + SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get()); SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 0); SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 4); @@ -108,7 +108,7 @@ public interface Intake { double INTAKE_FEED_SPEED = 0.4; double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE.get() + 25; - double ARM_SPEED_THRESHOLD_TO_FEED = 2.5; // degrees per second + double ARM_SPEED_THRESHOLD_TO_FEED = 1.75; // degrees per second double INTAKE_SHOOT_SPEED = 0.9; double INTAKE_SHOOT_TIME = 0.75; @@ -127,7 +127,7 @@ public interface Shooter { double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 1.0; - boolean ALWAYS_KEEP_AT_SPEED = false; + boolean ALWAYS_KEEP_AT_SPEED = true; double TARGET_RPM_THRESHOLD = 200; double MAX_WAIT_TO_REACH_TARGET = ALWAYS_KEEP_AT_SPEED ? 1.5 : 2.0; diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java index 5d3c1e9..6563926 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java @@ -13,67 +13,57 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - {4000, 562}, - {4000, 538}, - {4000, 578}, - {4000, 498}, - {4000, 527}, - {4000, 499}, - {4000, 481}, - {4000, 611}, - {4000, 528}, - {4000, 365}, - {4000, 487}, - {4000, 548}, - {3800, 456}, - {3800, 457}, - {3800, 515}, - {3800, 563}, - {3800, 560}, - {3800, 495}, - {3600, 519}, - {3600, 505}, - {3600, 525}, - {3600, 513}, - {3600, 521}, - {3600, 513}, - {3600, 554}, - {3600, 495}, - {3400, 428}, - {3400, 415}, - {3400, 430}, - {3400, 428}, - {3400, 437}, - {3200, 439}, - {3200, 425}, - {3200, 455}, - {3200, 446}, + {4000, 551}, + {4000, 573}, + {4000, 546}, + {4000, 566}, + {4000, 579}, + {4000, 458}, + {4000, 533}, + {3800, 561}, + {3800, 499}, + {3800, 576}, + {3800, 517}, + {3800, 497}, + {3800, 522}, + {3600, 456}, + {3600, 516}, + {3600, 465}, + {3600, 422}, + {3600, 542}, + {3400, 435}, + {3400, 467}, + {3400, 496}, + {3400, 461}, + {3400, 470}, + {3400, 448}, + {3400, 516}, + {3200, 433}, + // {3200, 456}, {3200, 447}, - {3200, 447}, - {3000, 413}, - {3000, 428}, - {3000, 418}, - {3000, 413}, - {3000, 369}, + {3200, 406}, + {3200, 424}, + {3200, 454}, + // {3000, 414}, + {3000, 388}, + {3000, 389}, + {3000, 392}, + // {3000, 405}, {3000, 400}, - {3000, 342}, - {3000, 426}, - {2600, 285}, + // {3000, 430}, {2600, 316}, - {2600, 302}, - {2600, 304}, - {2600, 338}, - {2600, 318}, - {2200, 258}, - {2200, 307}, - {2200, 267}, - {2200, 267}, - {2200, 306}, - {2200, 272}, - {1500, 194}, - {1500, 198}, - {1500, 132}, - {1500, 133}, + // {2600, 354}, + // {2600, 339}, + // {2200, 288}, + {2200, 241}, + // {2200, 268}, + {2200, 218}, + {2200, 210}, + {1500, 145}, + {1500, 145}, + {1500, 136}, + // {1500, 184}, + // {1500, 178}, }; static { diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java index 6fdd7b7..7276aeb 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java @@ -13,88 +13,8 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - {4000, 551}, - {4000, 573}, - {4000,546 }, - {4000, 566}, - {4000, 579}, - {4000, 458}, - {4000, 533}, - {3800, 561}, - {3800, 499}, - {3800, 576}, - {3800, 517}, - {3800, 497}, - {3800, 522}, - {3600, 456}, - {3600, 516}, - {3600, 465}, - {3600, 422}, - {3600, 542}, - {3400, 435}, - {3400, 467}, - {3400, 496}, - {3400, 461}, - {3400, 470}, - {3400, 448}, - {3400, 516}, - {3200, 433}, - {3200, 456}, - {3200, 447}, - {3200, 406}, - {3200, 424}, - {3200, 454}, - {3000, 414}, - {3000, 388}, - {3000, 389}, - {3000, 392}, - {3000, 405}, - {3000, 400}, - {3000, 430}, - {2600, 316}, - {2600, 354}, - {2600, 339}, - {2200, 288}, - {2200, 241}, - {2200, 268}, - {2200, 218}, - {2200, 210}, - {1500, 145}, - {1500, 145}, - {1500, 136}, - {1500, 184}, - {1500, 178}, + }; - // private static final double[][] RPMAndDistance = { - // {1000, 55.5}, - // {1000, 42.5}, - // {1000, 57}, - // {1200, 73}, - // {2900, 295}, - // {2900, 265.5}, - // {2900, 279}, - // {2900, 289}, - // {3000, 306}, - // {3000, 317}, - // {3000, 311.5}, - // {3000, 324}, - // {3000, 303}, - // {3000, 300}, - // {3000, 303.5}, - // {3700, 437}, - // {3700, 447.5}, - // {3700, 419}, - // {3700, 416}, - // {3700, 400}, - // {3700, 381.5}, - // {3800, 410.5}, - // {3800, 441.25}, - // {3900, 457.5}, - // {3900, 427}, - // {4000, 476}, - // {4000, 435}, - // {4000, 439}, - // }; static { interpolatingDoubleTreeMap = new InterpolatingDoubleTreeMap();