get ready for friday testing of LaunchPrepare

StuyPulse · Jan 18, 2024 · 6773b23 · 6773b23
1 parent fa7b6d0
commit 6773b23
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 11 deletions.
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -71,7 +71,7 @@ private void configureDriverBindings() {
             .onFalse(new LauncherStop());
 
         driver.getBottomButton()
-            .whileTrue(new WaitCommand(0.5).andThen(new LauncherLaunch()))
+            .whileTrue(new LaunchPrepare().andThen(new LauncherLaunch()))
             .onFalse(new LauncherStop());
     }
 

diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java b/src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java
@@ -3,13 +3,14 @@
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.launcher.Launcher;
 
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 /**
  * WARNING: DONT USE THIS COMMAND
  * PrepareLaunch sets the launcher to the launch speed, spins just the outside wheel of the launcher to allow it to get up to speed before launching
  */
-public class LaunchPrepare extends InstantCommand {
+public class LaunchPrepare extends Command {
     Launcher launcher;
 
     public LaunchPrepare() {
@@ -19,6 +20,11 @@ public LaunchPrepare() {
 
     @Override
     public void initialize() {
-        launcher.setLaunchSpeed(Settings.Launcher.LAUNCH_LAUNCHER_SPEED);
+        launcher.setLaunchSpeed(Settings.Launcher.LAUNCH_FEEDER_VOLTAGE);
+    } 
+
+    @Override
+    public boolean isFinished() {
+        return launcher.getFeederVoltage() == Settings.Launcher.LAUNCH_FEEDER_VOLTAGE;
     } 
 }
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -37,10 +37,10 @@ public interface Feedforward {
     public interface Launcher {
         // Speeds for wheels when intaking and launching. Intake speeds are negative to run the wheels
         // in reverse
-        double LAUNCH_LAUNCHER_SPEED = 1;
-        double LAUNCH_FEEDER_SPEED = 1;
-        double INTAKE_LAUNCHER_SPEED = -1;
-        double INTAKE_FEEDER_SPEED = -.2;
+        double LAUNCH_LAUNCHER_VOLTAGE= 1;
+        double LAUNCH_FEEDER_VOLTAGE = 1;
+        double INTAKE_LAUNCHER_VOLTAGE = -1;
+        double INTAKE_FEEDER_VOLTAGE = -.2;
 
         double kLauncherDelay = 1;
     }

diff --git a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java
@@ -102,7 +102,16 @@ public void stop() {
     public void periodicChild() {
         SmartDashboard.putNumber("Drivetrain/Left Speed", getLeftSpeed());
         SmartDashboard.putNumber("Drivetrain/Right Speed", getRightSpeed());
+
+        SmartDashboard.putNumber("Drivetrain/Left Distance", getLeftDistance());
+        SmartDashboard.putNumber("Drivetrain/Right Distance", getRightDistance());
+
         SmartDashboard.putNumber("Drivetrain/Left Voltage", getLeftVoltage());
         SmartDashboard.putNumber("Drivetrain/Right Voltage", getRightVoltage());
+
+        SmartDashboard.putNumber("Drivetrain/Velocity", getVelocity());
+        SmartDashboard.putNumber("Drivetrain/Distance", getDistance());
+
+        SmartDashboard.putNumber("Drivetrain/Angle", getAngle().toDegrees());        
     }
 }
diff --git a/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java b/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java
@@ -48,13 +48,13 @@ public void stop() {
     }
 
     public void intake() {
-        launcher.set(Settings.Launcher.INTAKE_LAUNCHER_SPEED);
-        feeder.set(Settings.Launcher.INTAKE_FEEDER_SPEED);
+        launcher.set(Settings.Launcher.INTAKE_LAUNCHER_VOLTAGE);
+        feeder.set(Settings.Launcher.INTAKE_FEEDER_VOLTAGE);
     }
 
     public void launch() {
-        launcher.set(Settings.Launcher.LAUNCH_LAUNCHER_SPEED);
-        feeder.set(Settings.Launcher.LAUNCH_FEEDER_SPEED);
+        launcher.set(Settings.Launcher.LAUNCH_LAUNCHER_VOLTAGE);
+        feeder.set(Settings.Launcher.LAUNCH_FEEDER_VOLTAGE);
     }
 
     //********** GETTERS **********
@@ -78,6 +78,7 @@ public double getFeederVoltage() {
     public void periodic() {
         SmartDashboard.putNumber("Launcher/Launcher Speed", getLauncherSpeed());
         SmartDashboard.putNumber("Launcher/Feeder Speed", getFeederSpeed());
+
         SmartDashboard.putNumber("Launcher/Launcher Voltage", getLauncherVoltage());
         SmartDashboard.putNumber("Launcher/Feeder Voltage", getFeederVoltage());