Add LaunchPrepare

StuyPulse · Jan 31, 2024 · 9e5bb0b · 9e5bb0b
1 parent 6c83117
commit 9e5bb0b
Show file tree

Hide file tree

Showing 3 changed files with 68 additions and 2 deletions.
diff --git a/.Glass/glass.json b/.Glass/glass.json
@@ -1,10 +1,30 @@
 {
  "NetworkTables": {
+ "retained": {
+ "SmartDashboard": {
+ "Autonomous": {
+ "open": true
+ },
+ "open": true
+ }
+ },
  "transitory": {
+ "Shuffleboard": {
+ "open": true
+ },
  "SmartDashboard": {
+ "Driver": {
+ "open": true
+ },
  "Drivetrain": {
  "open": true
  },
+ "Launcher": {
+ "Amp": {
+ "open": true
+ },
+ "open": true
+ },
  "open": true
  }
  },
@@ -19,5 +39,49 @@
  "NetworkTables Settings": {
  "mode": "Client (NT4)",
  "serverTeam": "694"
+ },
+ "Plots": {
+ "Plot <0>": {
+ "plots": [
+ {
+ "backgroundColor": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.8500000238418579
+ ],
+ "height": 332,
+ "series": [
+ {
+ "color": [
+ 0.2980392277240753,
+ 0.44705885648727417,
+ 0.6901960968971252,
+ 1.0
+ ],
+ "id": "NT:/SmartDashboard/Launcher/Feeder Output Speed"
+ },
+ {
+ "color": [
+ 0.8666667342185974,
+ 0.5176470875740051,
+ 0.32156863808631897,
+ 1.0
+ ],
+ "id": "NT:/SmartDashboard/Launcher/Launcher Output Speed"
+ },
+ {
+ "color": [
+ 0.3333333432674408,
+ 0.658823549747467,
+ 0.4078431725502014,
+ 1.0
+ ],
+ "id": "NT:/SmartDashboard/Launcher/Launcher Velocity"
+ }
+ ]
+ }
+ ]
+ }
  }
 }
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -7,9 +7,11 @@
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.commands.drivetrain.DrivetrainDrive;
+import com.stuypulse.robot.commands.launcher.LaunchPrepare;
 import com.stuypulse.robot.commands.launcher.LauncherHoldSpeed;
 import com.stuypulse.robot.commands.launcher.LauncherLaunchSpeaker;
 import com.stuypulse.robot.commands.launcher.LauncherIntakeNote;
+import com.stuypulse.robot.commands.launcher.LauncherLaunch;
 import com.stuypulse.robot.commands.launcher.LauncherStop;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
@@ -68,7 +70,7 @@ private void configureDriverBindings() {
  .whileTrue(new LauncherIntakeNote());
 
  driver.getBottomButton()
- .whileTrue(new LauncherLaunchSpeaker());
+ .whileTrue(new LaunchPrepare(Settings.Launcher.LAUNCHER_AMP_SPEED, Settings.Launcher.AMP_THRESHOLD_RPM).andThen(new LauncherLaunch(Settings.Launcher.FEEDER_AMP_SPEED, Settings.Launcher.LAUNCHER_AMP_SPEED)));
  }
 
  private void configureOperatorBindings() {}

diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java b/src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java
@@ -30,6 +30,6 @@ public void initialize() {
 
  @Override
  public boolean isFinished() {
- return launcher.getLauncherVelocity() >= thresholdRPM.doubleValue();
+ return Math.abs(launcher.getLauncherVelocity() - thresholdRPM.doubleValue()) < 100;
  } 
 }