Merge pull request #33 from iamawesomecat/main

Launcher updates

src/main/java/frc/team2412/robot/Controls.java

@@ -3,11 +3,15 @@
 import static frc.team2412.robot.Controls.ControlConstants.CODRIVER_CONTROLLER_PORT;
 import static frc.team2412.robot.Controls.ControlConstants.CONTROLLER_PORT;
 import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED;
+import static frc.team2412.robot.Subsystems.SubsystemConstants.LAUNCHER_ENABLED;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand;
+import frc.team2412.robot.subsystems.LauncherSubsystem;
 
 public class Controls {
 	public static class ControlConstants {
@@ -20,14 +24,29 @@ public static class ControlConstants {
 
 	private final Subsystems s;
 
+	// Launcher
+
+	private final Trigger launcherAmpPresetButton;
+	private final Trigger launcherSubwooferPresetButton;
+	private final Trigger launcherPodiumPresetButton;
+	private final Trigger launcherTrapPresetButton;
+
 	public Controls(Subsystems s) {
 		driveController = new CommandXboxController(CONTROLLER_PORT);
 		codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT);
 		this.s = s;
+		launcherAmpPresetButton = codriveController.povDown();
+		launcherSubwooferPresetButton = codriveController.povRight();
+		launcherPodiumPresetButton = codriveController.povLeft();
+		launcherTrapPresetButton = codriveController.povUp();
 
 		if (DRIVEBASE_ENABLED) {
 			bindDrivebaseControls();
 		}
+
+		if (LAUNCHER_ENABLED) {
+			bindLauncherControls();
+		}
 	}
 
 	private void bindDrivebaseControls() {
@@ -41,4 +60,17 @@ private void bindDrivebaseControls() {
 		driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro));
 		driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels));
 	}
+
+	private void bindLauncherControls() {
+		launcherPodiumPresetButton.onTrue(
+				new SetAngleLaunchCommand(
+						s.launcherSubsystem,
+						LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
+						LauncherSubsystem.PODIUM_AIM_ANGLE));
+		launcherSubwooferPresetButton.onTrue(
+				new SetAngleLaunchCommand(
+						s.launcherSubsystem,
+						LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
+						LauncherSubsystem.SUBWOOFER_AIM_ANGLE));
+	}
 }

src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java

@@ -0,0 +1,27 @@
+package frc.team2412.robot.commands.diagnostic;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.team2412.robot.commands.launcher.SetAngleCommand;
+import frc.team2412.robot.commands.launcher.SetLaunchSpeedCommand;
+import frc.team2412.robot.commands.launcher.StopLauncherCommand;
+import frc.team2412.robot.subsystems.LauncherSubsystem;
+
+public class LauncherDiagnosticCommand extends SequentialCommandGroup {
+	private final LauncherSubsystem launcherSubsystem;
+	private final double Angle;
+
+	public LauncherDiagnosticCommand(LauncherSubsystem launcherSubsystem) {
+		this.launcherSubsystem = launcherSubsystem;
+		this.Angle = launcherSubsystem.getAngle();
+		addCommands(
+				new SetAngleCommand(launcherSubsystem, 45),
+				new WaitCommand(2),
+				new SetAngleCommand(launcherSubsystem, 90),
+				new WaitCommand(1),
+				new SetAngleCommand(launcherSubsystem, Angle),
+				new SetLaunchSpeedCommand(launcherSubsystem, 100),
+				new WaitCommand(5),
+				new StopLauncherCommand(launcherSubsystem));
+	}
+}

src/main/java/frc/team2412/robot/commands/launcher/StopLauncherCommand.java

@@ -0,0 +1,23 @@
+package frc.team2412.robot.commands.launcher;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.subsystems.LauncherSubsystem;
+
+public class StopLauncherCommand extends Command {
+	private final LauncherSubsystem launcherSubsystem;
+
+	public StopLauncherCommand(LauncherSubsystem launcherSubsystem) {
+		this.launcherSubsystem = launcherSubsystem;
+		addRequirements(launcherSubsystem);
+	}
+
+	@Override
+	public void initialize() {
+		launcherSubsystem.stopLauncher();
+	}
+
+	@Override
+	public boolean isFinished() {
+		return true;
+	}
+}

src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java

@@ -14,15 +14,23 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.team2412.robot.Hardware;
+import frc.team2412.robot.util.SparkPIDWidget;
 import java.util.Map;
 
 public class LauncherSubsystem extends SubsystemBase {
 	// CONSTANTS
-	// MOTOR SPEED VALUES
-	public static final double SPEAKER_SHOOT_SPEED = 0.5;
-	public static final double AMP_SHOOT_SPEED = 0.2;
-	public static final double ANGLE_CHANGE_SPEED = 0.15;
+	// ANGLE VALUES
+	public static final int SUBWOOFER_AIM_ANGLE = 54;
+	public static final int PODIUM_AIM_ANGLE = 39;
+	// MOTOR VALUES
+	// max Free Speed: 6784 RPM
+	private static final int MAX_FREE_SPEED_RPM = 6784;
 	public static final double ANGLE_TOLERANCE = 0.5;
+	// RPM
+	public static final int SPEAKER_SHOOT_SPEED_RPM = 3392; // 50%
+	// 3392 RPM = 50% Speed
+	// 1356 RPM = 20% Speed
+	// 1017 RPM = 15% Speed
 
 	// HARDWARE
 	private final CANSparkFlex launcherTopMotor;
@@ -32,13 +40,15 @@ public class LauncherSubsystem extends SubsystemBase {
 	private final RelativeEncoder launcherBottomEncoder;
 	private final SparkAbsoluteEncoder launcherAngleEncoder;
 	private final SparkPIDController launcherAnglePIDController;
+	private final SparkPIDController launcherTopPIDController;
+	private final SparkPIDController launcherBottomPIDController;
 
 	private final GenericEntry setLauncherSpeedEntry =
 			Shuffleboard.getTab("Launcher")
-					.addPersistent("Launcher Speed setpoint", SPEAKER_SHOOT_SPEED)
-					.withSize(2, 1)
+					.addPersistent("Launcher Speed setpoint", SPEAKER_SHOOT_SPEED_RPM)
+					.withSize(3, 1)
 					.withWidget(BuiltInWidgets.kNumberSlider)
-					.withProperties(Map.of("Min", -1, "Max", 1))
+					.withProperties(Map.of("Min", -MAX_FREE_SPEED_RPM, "Max", MAX_FREE_SPEED_RPM))
 					.getEntry();
 
 	private final GenericEntry launcherAngleEntry =
@@ -67,8 +77,19 @@ public LauncherSubsystem() {
 		launcherAngleEncoder = launcherAngleMotor.getAbsoluteEncoder(Type.kDutyCycle);
 
 		// PID controllers
+		// Create launcherTopPIDController and launcherTopMotor]
+		launcherTopPIDController = launcherTopMotor.getPIDController();
+		launcherTopPIDController.setFeedbackDevice(launcherTopEncoder);
+		launcherBottomPIDController = launcherBottomMotor.getPIDController();
+		launcherBottomPIDController.setFeedbackDevice(launcherBottomEncoder);
 		launcherAnglePIDController = launcherAngleMotor.getPIDController();
 		launcherAnglePIDController.setFeedbackDevice(launcherAngleEncoder);
+		Shuffleboard.getTab("Launcher")
+				.add(new SparkPIDWidget(launcherAnglePIDController, "launcherAnglePIDController"));
+		Shuffleboard.getTab("Launcher")
+				.add(new SparkPIDWidget(launcherTopPIDController, "launcherTopPIDController"));
+		Shuffleboard.getTab("Launcher")
+				.add(new SparkPIDWidget(launcherBottomPIDController, "launcherBottomPIDController"));
 	}
 
 	public void configMotors() {
@@ -90,23 +111,38 @@ public void configMotors() {
 		launcherTopMotor.burnFlash();
 		launcherBottomMotor.burnFlash();
 		launcherAngleMotor.burnFlash();
-	}
 
+		// PID
+		launcherAnglePIDController.setP(0.1);
+		launcherAnglePIDController.setI(0);
+		launcherAnglePIDController.setD(0);
+		launcherAnglePIDController.setFF(0);
+
+		launcherTopPIDController.setP(0.1);
+		launcherTopPIDController.setI(0);
+		launcherTopPIDController.setD(0);
+		launcherTopPIDController.setFF(0);
+
+		launcherBottomPIDController.setP(0.1);
+		launcherBottomPIDController.setI(0);
+		launcherBottomPIDController.setD(0);
+		launcherBottomPIDController.setFF(0);
+	}
 	// stop launcher motors method
 	public void stopLauncher() {
 		launcherTopMotor.stopMotor();
 		launcherBottomMotor.stopMotor();
 	}
-
+	// uses the value from the entry
 	public void launch() {
-		double speed = setLauncherSpeedEntry.getDouble(SPEAKER_SHOOT_SPEED);
-		launcherTopMotor.set(speed);
-		launcherBottomMotor.set(speed);
+		double speed = setLauncherSpeedEntry.getDouble(SPEAKER_SHOOT_SPEED_RPM);
+		launcherTopPIDController.setReference(speed, ControlType.kVelocity);
+		launcherBottomPIDController.setReference(speed, ControlType.kVelocity);
 	}
-
+	// used for presets
 	public void launch(double speed) {
-		launcherTopMotor.set(speed);
-		launcherBottomMotor.set(speed);
+		launcherTopPIDController.setReference(speed, ControlType.kVelocity);
+		launcherBottomPIDController.setReference(speed, ControlType.kVelocity);
 	}
 	// returns the degrees of the angle of the launcher
 	public double getAngle() {
@@ -120,8 +156,7 @@ public void setAngle(double angle) {
 
 	@Override
 	public void periodic() {
-		// .get will be replaced with .getVelocity once PID is established for flywheels :C
 		launcherAngleEntry.setDouble(getAngle());
-		launcherSpeedEntry.setDouble(launcherTopMotor.get());
+		launcherSpeedEntry.setDouble(launcherTopEncoder.getVelocity());
 	}
 }

src/main/java/frc/team2412/robot/util/SparkPIDWidget.java

@@ -11,7 +11,6 @@ public class SparkPIDWidget implements NTSendable {
 
 	public SparkPIDWidget(SparkPIDController controller, String name) {
 		this.controller = controller;
-
 		SendableRegistry.add(this, name);
 	}