Merge branch 'main' of https://github.com/SJJCoding/Crescendo2024

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

@@ -14,16 +14,19 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.team2412.robot.commands.intake.AllInCommand;
 import frc.team2412.robot.commands.intake.AllReverseCommand;
 import frc.team2412.robot.commands.intake.AllStopCommand;
 import frc.team2412.robot.commands.intake.FeederInCommand;
 import frc.team2412.robot.commands.intake.IntakeRejectCommand;
 import frc.team2412.robot.commands.launcher.FullTargetCommand;
+import frc.team2412.robot.commands.launcher.ManualAngleCommand;
 import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand;
-import frc.team2412.robot.commands.launcher.SetAngleCommand;
 import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand;
+import frc.team2412.robot.commands.launcher.SetPivotCommand;
 import frc.team2412.robot.subsystems.LauncherSubsystem;
+import frc.team2412.robot.util.TrapAlign;
 
 public class Controls {
 	public static class ControlConstants {
@@ -48,8 +51,9 @@ public static class ControlConstants {
 	// Launcher
 	private final Trigger launcherAmpPresetButton;
 	private final Trigger launcherSubwooferPresetButton;
+	private final Trigger launcherLowerPresetButton;
 	// private final Trigger launcherPodiumPresetButton;
-	// private final Trigger launcherTrapPresetButton;
+	private final Trigger launcherTrapPresetButton;
 	private final Trigger launcherLaunchButton;
 
 	private final Subsystems s;
@@ -64,9 +68,10 @@ public Controls(Subsystems s) {
 
 		launcherAmpPresetButton = codriveController.x();
 		launcherSubwooferPresetButton = codriveController.a();
+		launcherLowerPresetButton = codriveController.y();
 		// launcherPodiumPresetButton = codriveController.povLeft();
-		// launcherTrapPresetButton = codriveController.y();
-		launcherLaunchButton = codriveController.leftBumper();
+		launcherTrapPresetButton = codriveController.rightTrigger();
+		launcherLaunchButton = codriveController.rightBumper();
 		// intake controls (confirmed with driveteam)
 		driveIntakeInButton = driveController.a();
 		driveIntakeStopButton = driveController.b();
@@ -77,6 +82,10 @@ public Controls(Subsystems s) {
 		codriveIntakeReverseButton = codriveController.povLeft();
 		codriveIntakeRejectButton = codriveController.povDown();
 
+		if (Robot.isSysIdMode() && LAUNCHER_ENABLED) {
+			bindSysIdControls();
+			return;
+		}
 		if (DRIVEBASE_ENABLED) {
 			bindDrivebaseControls();
 		}
@@ -141,11 +150,12 @@ public void bindLimelightControls() {
 	private void bindIntakeControls() {
 		// CommandScheduler.getInstance()
 		// 		.setDefaultCommand(s.intakeSubsystem, new IntakeStopCommand(s.intakeSubsystem));
-		driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem));
-		// driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem));
+		driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this));
+		driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem));
+
 		driveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem));
 		driveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem));
-		codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem));
+		codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this));
 		codriveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem));
 		codriveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem));
 		codriveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem));
@@ -159,12 +169,14 @@ private void bindLauncherControls() {
 		CommandScheduler.getInstance()
 				.setDefaultCommand(
 						s.launcherSubsystem,
-						new SetAngleCommand(
+						new ManualAngleCommand(
 								s.launcherSubsystem,
 								() ->
 										MathUtil.applyDeadband(codriveController.getLeftY(), 0.1)
 												* LauncherSubsystem.ANGLE_MAX_SPEED));
 
+		launcherLowerPresetButton.onTrue(
+				new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE));
 		launcherSubwooferPresetButton.onTrue(
 				new SetAngleLaunchCommand(
 						s.launcherSubsystem,
@@ -180,8 +192,8 @@ private void bindLauncherControls() {
 						s.launcherSubsystem,
 						LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
 						LauncherSubsystem.AMP_AIM_ANGLE));
-		// launcherTrapPresetButton.onTrue(
-		//		TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));
+		launcherTrapPresetButton.onTrue(
+				TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));
 
 		codriveController.b().whileTrue(s.launcherSubsystem.run(s.launcherSubsystem::stopLauncher));
 
@@ -194,9 +206,32 @@ private void bindLauncherControls() {
 		driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch(4000)));
 	}
 
+	private void bindSysIdControls() {
+		// only one routine can be run in one robot log
+		// switch these between arm and flywheel in code when tuning
+		driveController
+				.leftBumper()
+				.whileTrue(s.launcherSubsystem.armSysIdQuasistatic(Direction.kForward));
+		driveController
+				.rightBumper()
+				.whileTrue(s.launcherSubsystem.armSysIdQuasistatic(Direction.kReverse));
+		driveController
+				.leftTrigger()
+				.whileTrue(s.launcherSubsystem.armSysIdDynamic(Direction.kForward));
+		driveController
+				.rightTrigger()
+				.whileTrue(s.launcherSubsystem.armSysIdDynamic(Direction.kReverse));
+		// switch these between angle and drive tests in code when tuning
+		driveController.x().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kForward));
+		driveController.y().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kReverse));
+		driveController.a().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kForward));
+		driveController.b().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kReverse));
+	}
+
 	public void vibrateDriveController(double vibration) {
 		if (!DriverStation.isAutonomous()) {
 			driveController.getHID().setRumble(RumbleType.kBothRumble, vibration);
+			codriveController.getHID().setRumble(RumbleType.kBothRumble, vibration);
 		}
 	}
 }

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

@@ -1,5 +1,6 @@
 package frc.team2412.robot;
 
+import com.ctre.phoenix6.SignalLogger;
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -40,6 +41,8 @@ public static Robot getInstance() {
 	}
 
 	private static final boolean debugMode = true;
+	// Really dangerous to keep this enabled as it disables all other controls, use with caution
+	private static final boolean sysIdMode = false;
 
 	private final RobotType robotType;
 	private final PowerDistribution PDP;
@@ -148,6 +151,7 @@ public void autonomousInit() {
 	@Override
 	public void teleopInit() {
 		Shuffleboard.startRecording();
+		SignalLogger.start();
 	}
 
 	@Override
@@ -158,6 +162,7 @@ public void autonomousExit() {
 	@Override
 	public void teleopExit() {
 		CommandScheduler.getInstance().cancelAll();
+		SignalLogger.stop();
 	}
 
 	@Override
@@ -192,4 +197,8 @@ public boolean isCompetition() {
 	public static boolean isDebugMode() {
 		return debugMode;
 	}
+
+	public static boolean isSysIdMode() {
+		return sysIdMode;
+	}
 }

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

@@ -2,8 +2,7 @@
 
 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.SetAngleLaunchCommand;
 import frc.team2412.robot.commands.launcher.StopLauncherCommand;
 import frc.team2412.robot.subsystems.LauncherSubsystem;
 
@@ -15,12 +14,11 @@ public LauncherDiagnosticCommand(LauncherSubsystem launcherSubsystem) {
 		this.launcherSubsystem = launcherSubsystem;
 		this.Angle = launcherSubsystem.getAngle();
 		addCommands(
-				new SetAngleCommand(launcherSubsystem, () -> 45),
+				new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 10),
 				new WaitCommand(2),
-				new SetAngleCommand(launcherSubsystem, () -> 90),
+				new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 20),
 				new WaitCommand(1),
-				new SetAngleCommand(launcherSubsystem, () -> Angle),
-				new SetLaunchSpeedCommand(launcherSubsystem, 100),
+				new SetAngleLaunchCommand(launcherSubsystem, 500, Angle),
 				new WaitCommand(5),
 				new StopLauncherCommand(launcherSubsystem));
 	}

src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java

@@ -1,13 +1,22 @@
 package frc.team2412.robot.commands.intake;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.team2412.robot.Controls;
 import frc.team2412.robot.subsystems.IntakeSubsystem;
 
 public class AllInCommand extends Command {
 	private final IntakeSubsystem intakeSubsystem;
+	private final Controls controls;
+	private boolean rumbledIntakeFront = false;
+	private boolean rumbledIntakeLeft = false;
+	private boolean rumbledIntakeRight = false;
+	private boolean rumbledIndex = false;
 
-	public AllInCommand(IntakeSubsystem intakeSubsystem) {
+	public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) {
 		this.intakeSubsystem = intakeSubsystem;
+		this.controls = controls;
 		addRequirements(intakeSubsystem);
 	}
 
@@ -20,16 +29,94 @@ public void initialize() {
 
 	@Override
 	public void execute() {
+		// intake rejecting
+		if (intakeSubsystem.intakeFrontSeesNote()) {
+			if (!intakeSubsystem.getRejectOverride()) {
+				intakeSubsystem.intakeBackReject();
+				intakeSubsystem.intakeLeftReject();
+				intakeSubsystem.intakeRightReject();
+			} else {
+				intakeSubsystem.intakeBackStop();
+				intakeSubsystem.intakeLeftStop();
+				intakeSubsystem.intakeRightStop();
+			}
+
+			if (controls != null && !rumbledIntakeFront) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule();
+				rumbledIntakeFront = true;
+			}
+		}
+
+		// back intake sensor does not exist (but it might?)
+
+		if (intakeSubsystem.intakeLeftSeesNote()) {
+			if (!intakeSubsystem.getRejectOverride()) {
+				intakeSubsystem.intakeFrontReject();
+				intakeSubsystem.intakeBackReject();
+				intakeSubsystem.intakeRightReject();
+			} else {
+				intakeSubsystem.intakeFrontStop();
+				intakeSubsystem.intakeBackStop();
+				intakeSubsystem.intakeRightStop();
+			}
+
+			if (controls != null && !rumbledIntakeLeft) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule();
+				rumbledIntakeLeft = true;
+			}
+		}
+
+		if (intakeSubsystem.intakeRightSeesNote()) {
+			if (!intakeSubsystem.getRejectOverride()) {
+				intakeSubsystem.intakeFrontReject();
+				intakeSubsystem.intakeBackReject();
+				intakeSubsystem.intakeLeftReject();
+			} else {
+				intakeSubsystem.intakeFrontStop();
+				intakeSubsystem.intakeBackStop();
+				intakeSubsystem.intakeLeftStop();
+			}
+
+			if (controls != null && !rumbledIntakeRight) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule();
+				rumbledIntakeRight = true;
+			}
+		}
+
+		// all intake motors rejecting after index
 		if (intakeSubsystem.indexSensorHasNote()) {
-			intakeSubsystem.intakeReject();
+			if (!intakeSubsystem.getRejectOverride()) {
+				intakeSubsystem.intakeReject();
+			} else {
+				intakeSubsystem.intakeStop();
+			}
+
+			if (controls != null && !rumbledIndex) {
+				Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule();
+				rumbledIndex = true;
+			}
 		}
 	}
 
 	@Override
 	public void end(boolean interrupted) {
-		// intakeSubsystem.intakeReject();
+		if (!intakeSubsystem.getRejectOverride()) {
+			intakeSubsystem.intakeReject();
+		} else {
+			intakeSubsystem.intakeStop();
+		}
+
 		intakeSubsystem.indexStop();
 		intakeSubsystem.feederStop();
+
+		if (controls != null) {
+			Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule();
+		}
+
+		rumbledIntakeFront = false;
+		rumbledIntakeLeft = false;
+		rumbledIntakeRight = false;
+		rumbledIndex = false;
 	}
 
 	@Override

src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java

@@ -0,0 +1,25 @@
+package frc.team2412.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.subsystems.IntakeSubsystem;
+
+public class AllInNoSensorCommand extends Command {
+	private final IntakeSubsystem intakeSubsystem;
+
+	public AllInNoSensorCommand(IntakeSubsystem intakeSubsystem) {
+		this.intakeSubsystem = intakeSubsystem;
+		addRequirements(intakeSubsystem);
+	}
+
+	@Override
+	public void initialize() {
+		intakeSubsystem.intakeIn();
+		intakeSubsystem.indexIn();
+		intakeSubsystem.feederIn();
+	}
+
+	@Override
+	public boolean isFinished() {
+		return true;
+	}
+}

src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java

@@ -0,0 +1,27 @@
+package frc.team2412.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.Controls;
+
+public class RumbleCommand extends Command {
+	private final Controls controls;
+
+	public RumbleCommand(Controls controls) {
+		this.controls = controls;
+	}
+
+	@Override
+	public void initialize() {
+		controls.vibrateDriveController(1.0);
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		controls.vibrateDriveController(0.0);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}

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

@@ -1,5 +1,7 @@
 package frc.team2412.robot.commands.launcher;
 
+import static frc.team2412.robot.Subsystems.SubsystemConstants.*;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -38,7 +40,9 @@ public FullTargetCommand(
 		this.launcherSubsystem = launcherSubsystem;
 		this.drivebaseSubsystem = drivebaseSubsystem;
 		this.controls = controls;
-		yawAlignmentCommand = drivebaseSubsystem.rotateToAngle(() -> yawTarget, false);
+		if (DRIVEBASE_ENABLED) {
+			yawAlignmentCommand = drivebaseSubsystem.rotateToAngle(() -> yawTarget, false);
+		}
 
 		addRequirements(launcherSubsystem);
 	}