diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 85c1a8a..13f91de 100644 --- 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 index 2725dc4..9217a5f 100644 --- 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 index eea4f2b..2cad7e4 100644 --- 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 index 44dcc4a..5c98e62 100644 --- 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()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java b/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java index 5de5db0..570cbb9 100644 --- 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());