Merge branch 'main' of github.com:Stuypulse/2024-kitbot and commands …

…changes for more configurable rpm

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 ***/

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

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

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

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

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

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 {

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