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());