Add LaunchPrepare

Author: anivanchen

.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"
+            }
+          ]
+        }
+      ]
+    }
   }
 }

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() {}

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;
     } 
 }