diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 13f91de..057b4f9 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -6,24 +6,21 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.drivetrain.DrivetrainDriveForever; -import com.stuypulse.robot.commands.launcher.LaunchPrepare; -import com.stuypulse.robot.commands.launcher.LauncherLaunch; +import com.stuypulse.robot.commands.drivetrain.DrivetrainDrive; +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.LauncherStop; -import com.stuypulse.robot.commands.odometry.OdometryRealign; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.drivetrain.AbstractDrivetrain; import com.stuypulse.robot.subsystems.launcher.Launcher; -import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; -import com.stuypulse.robot.subsystems.vision.AbstractVision; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.WaitCommand; public class RobotContainer { @@ -53,7 +50,8 @@ public RobotContainer() { /****************/ private void configureDefaultCommands() { - //drivetrain.setDefaultCommand(new DrivetrainDrive(driver)); + drivetrain.setDefaultCommand(new DrivetrainDrive(driver)); + launcher.setDefaultCommand(new LauncherHoldSpeed(Settings.Launcher.LAUNCHER_SPEAKER_SPEED)); } /***************/ @@ -67,16 +65,13 @@ private void configureButtonBindings() { private void configureDriverBindings() { driver.getRightBumper() - .whileTrue(new LauncherIntakeNote()) - .onFalse(new LauncherStop()); + .whileTrue(new LauncherIntakeNote()); driver.getBottomButton() - .whileTrue(new LaunchPrepare().andThen(new LauncherLaunch())) - .onFalse(new LauncherStop()); + .whileTrue(new LauncherLaunchSpeaker()); } - private void configureOperatorBindings() { - } + private void configureOperatorBindings() {} /**************/ /*** AUTONS ***/ 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 9217a5f..fad06d2 100644 --- a/src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java @@ -4,27 +4,32 @@ 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 Command { - Launcher launcher; - public LaunchPrepare() { + private final Launcher launcher; + private final Number thresholdRPM; + private final Number launchSpeed; + + public LaunchPrepare(Number launchSpeed, Number thresholdRPM) { + this.thresholdRPM = thresholdRPM; + this.launchSpeed = launchSpeed; + launcher = Launcher.getInstance(); addRequirements(launcher); } @Override public void initialize() { - launcher.setLaunchSpeed(Settings.Launcher.LAUNCH_FEEDER_VOLTAGE); + launcher.setLaunchSpeed(launchSpeed); } @Override public boolean isFinished() { - return launcher.getFeederVoltage() == Settings.Launcher.LAUNCH_FEEDER_VOLTAGE; + return launcher.getLauncherVelocity() >= thresholdRPM.doubleValue(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LauncherHoldSpeed.java b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherHoldSpeed.java new file mode 100644 index 0000000..771c093 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherHoldSpeed.java @@ -0,0 +1,25 @@ +package com.stuypulse.robot.commands.launcher; + +import com.stuypulse.robot.subsystems.launcher.Launcher; + +import edu.wpi.first.wpilibj2.command.Command; + +public class LauncherHoldSpeed extends Command { + + private final Launcher launcher; + private final Number launcherSpeed; + + public LauncherHoldSpeed(Number launcherSpeed) { + launcher = Launcher.getInstance(); + this.launcherSpeed = launcherSpeed; + + addRequirements(launcher); + } + + @Override + public void execute() { + launcher.setFeederSpeed(0); + launcher.setLaunchSpeed(launcherSpeed); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LauncherIntakeNote.java b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherIntakeNote.java index 1ea0e35..aa71da5 100644 --- a/src/main/java/com/stuypulse/robot/commands/launcher/LauncherIntakeNote.java +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherIntakeNote.java @@ -2,9 +2,9 @@ import com.stuypulse.robot.subsystems.launcher.Launcher; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class LauncherIntakeNote extends InstantCommand { +public class LauncherIntakeNote extends Command { Launcher launcher; public LauncherIntakeNote() { @@ -13,7 +13,7 @@ public LauncherIntakeNote() { } @Override - public void initialize() { + public void execute() { launcher.intake(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunch.java b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunch.java index bb1b12d..953e839 100644 --- a/src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunch.java +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunch.java @@ -2,18 +2,24 @@ import com.stuypulse.robot.subsystems.launcher.Launcher; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class LauncherLaunch extends InstantCommand { - Launcher launcher; +public class LauncherLaunch extends Command { + + private final Launcher launcher; + private final Number feederSpeed; + private final Number launcherSpeed; + + public LauncherLaunch(Number feederSpeed, Number launcherSpeed) { + this.launcherSpeed = launcherSpeed; + this.feederSpeed = feederSpeed; - public LauncherLaunch() { launcher = Launcher.getInstance(); addRequirements(launcher); } @Override public void initialize() { - launcher.launch(); + launcher.launch(feederSpeed, launcherSpeed); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunchSpeaker.java b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunchSpeaker.java new file mode 100644 index 0000000..c5186b0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherLaunchSpeaker.java @@ -0,0 +1,11 @@ +package com.stuypulse.robot.commands.launcher; + +import com.stuypulse.robot.constants.Settings.Launcher; + +public class LauncherLaunchSpeaker extends LauncherLaunch { + + public LauncherLaunchSpeaker() { + super(Launcher.FEEDER_SPEAKER_SPEED, Launcher.LAUNCHER_SPEAKER_SPEED); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index ce362ff..d6db63f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -24,7 +24,7 @@ public interface Drivetrain { double MASS_Kg = Units.lbsToKilograms(32.5462411); double GEARING = 8.45; double J_KG_METER_SQUARED = 7.19537; - + SmartNumber MAX_SPEED_PERCENT = new SmartNumber("Driver/Max Speed Percent", 0.75); SmartNumber MAX_TURN_PERCENT = new SmartNumber("Driver/Max Turn Percent", 0.45); @@ -41,14 +41,18 @@ 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_VOLTAGE= 1; - double LAUNCH_FEEDER_VOLTAGE = 1; - double INTAKE_LAUNCHER_VOLTAGE = -1; - double INTAKE_FEEDER_VOLTAGE = -.2; - - double kLauncherDelay = 1; + double LAUNCHER_SPEAKER_SPEED = 1; + double FEEDER_SPEAKER_SPEED = 1; + + double SPEAKER_THRESHOLD_RPM = 5700; + + double LAUNCHER_INTAKE_SPEED = -1; + double FEEDER_INTAKE_SPEED = -0.2; + + SmartNumber AMP_THRESHOLD_RPM = new SmartNumber("Launcher/Amp/Threshold RPM", 4000); + + SmartNumber LAUNCHER_AMP_SPEED = new SmartNumber("Launcher/Amp/Launcher Speed", 0.7); + SmartNumber FEEDER_AMP_SPEED = new SmartNumber("Launcher/Amp/Feeder Speed", 0.7); } public interface Alignment { 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 570cbb9..ba2562a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Launcher extends SubsystemBase{ +public class Launcher extends SubsystemBase { private static final Launcher instance; @@ -34,12 +34,12 @@ public Launcher() { } //********** SETTERS ********** - public void setLaunchSpeed(double speed) { - launcher.set(speed); + public void setLaunchSpeed(Number speed) { + launcher.set(speed.doubleValue()); } - public void setFeederSpeed(double speed) { - feeder.set(speed); + public void setFeederSpeed(Number speed) { + feeder.set(speed.doubleValue()); } public void stop() { @@ -48,39 +48,39 @@ public void stop() { } public void intake() { - launcher.set(Settings.Launcher.INTAKE_LAUNCHER_VOLTAGE); - feeder.set(Settings.Launcher.INTAKE_FEEDER_VOLTAGE); + launcher.set(Settings.Launcher.LAUNCHER_INTAKE_SPEED); + feeder.set(Settings.Launcher.FEEDER_INTAKE_SPEED); } - public void launch() { - launcher.set(Settings.Launcher.LAUNCH_LAUNCHER_VOLTAGE); - feeder.set(Settings.Launcher.LAUNCH_FEEDER_VOLTAGE); + public void launch(Number feederSpeed, Number launcherSpeed) { + launcher.set(launcherSpeed.doubleValue()); + feeder.set(feederSpeed.doubleValue()); } - //********** GETTERS ********** - public double getLauncherSpeed() { + //********** GETTERS **********// + public double getLauncherVelocity() { return launcher.getEncoder().getVelocity(); } - public double getFeederSpeed() { + public double getFeederVelocity() { return feeder.getEncoder().getVelocity(); } - public double getLauncherVoltage() { + public double getLauncherSpeed() { return launcher.getAppliedOutput(); } - public double getFeederVoltage() { + public double getFeederSpeed() { return feeder.getAppliedOutput(); } @Override public void periodic() { - SmartDashboard.putNumber("Launcher/Launcher Speed", getLauncherSpeed()); - SmartDashboard.putNumber("Launcher/Feeder Speed", getFeederSpeed()); + SmartDashboard.putNumber("Launcher/Launcher Velocity", getLauncherVelocity()); + SmartDashboard.putNumber("Launcher/Feeder Velocity", getFeederVelocity()); - SmartDashboard.putNumber("Launcher/Launcher Voltage", getLauncherVoltage()); - SmartDashboard.putNumber("Launcher/Feeder Voltage", getFeederVoltage()); + SmartDashboard.putNumber("Launcher/Launcher Output Speed", getLauncherSpeed()); + SmartDashboard.putNumber("Launcher/Feeder Output Speed", getFeederSpeed()); } } \ No newline at end of file