Shintake subsystem

example.chor

@@ -5292,7 +5292,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [],
             "eventMarkers": [],
-            "isTrajectoryStale": false
+            "isTrajectoryStale": true
         },
         "Triple Helix": {
             "waypoints": [
@@ -6616,7 +6616,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [],
             "eventMarkers": [],
-            "isTrajectoryStale": false
+            "isTrajectoryStale": true
         },
         "Alliance Auto": {
             "waypoints": [
@@ -7405,7 +7405,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [],
             "eventMarkers": [],
-            "isTrajectoryStale": false
+            "isTrajectoryStale": true
         },
         "Allience Backup Path": {
             "waypoints": [
@@ -7632,7 +7632,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [],
             "eventMarkers": [],
-            "isTrajectoryStale": false
+            "isTrajectoryStale": true
         }
     },
     "splitTrajectoriesAtStopPoints": true,

src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpHelper.java

@@ -3,27 +3,23 @@
 import org.jmhsrobotics.frc2024.Constants;
 import org.jmhsrobotics.frc2024.subsystems.arm.ArmPIDSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.arm.commands.CommandArm;
-import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem;
-import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand;
-import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem;
-import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterAutoCommand;
+import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.shintake.commands.IntakeCommand;
+import org.jmhsrobotics.frc2024.subsystems.shintake.commands.ShooterAutoCommand;
 
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
 public class AmpHelper extends SequentialCommandGroup {
 	private ArmPIDSubsystem armPIDSubsystem;
-	private ShooterSubsystem shooterSubsystem;
-	private IntakeSubsystem intakeSubsystem;
+	private ShintakeSubsystem shintakeSubsystem;
 
-	public AmpHelper(ArmPIDSubsystem armPIDSubsystem, ShooterSubsystem shooterSubsystem,
-			IntakeSubsystem intakeSubsystem) {
-		this.intakeSubsystem = intakeSubsystem;
-		this.shooterSubsystem = shooterSubsystem;
+	public AmpHelper(ArmPIDSubsystem armPIDSubsystem, ShintakeSubsystem shintakeSubsystem) {
+		this.shintakeSubsystem = shintakeSubsystem;
 		this.armPIDSubsystem = armPIDSubsystem;
 
 		addCommands(new CommandArm(this.armPIDSubsystem, Constants.ArmSetpoint.AMP.value),
-				new ParallelRaceGroup(new ShooterAutoCommand(this.shooterSubsystem, 5000).withTimeout(5),
-						new IntakeCommand(0.8, this.intakeSubsystem, this.shooterSubsystem).withTimeout(5)));
+				new ParallelRaceGroup(new ShooterAutoCommand(this.shintakeSubsystem, 5000).withTimeout(5),
+						new IntakeCommand(0.8, true, this.shintakeSubsystem).withTimeout(5)));
 	}
 }

src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpShotCommand.java

@@ -0,0 +1,45 @@
+package org.jmhsrobotics.frc2024.ComboCommands;
+
+import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem.ControlType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AmpShotCommand extends Command {
+
+	private ShintakeSubsystem shintakeSubsystem;
+	/**
+	 * Runs the intake and the shooter to expell a note into the amp. Command Never
+	 * ends
+	 *
+	 * @param intakeSubsystem
+	 * @param shooterSubsystem
+	 */
+	public AmpShotCommand(ShintakeSubsystem shintakeSubsystem) {
+		this.shintakeSubsystem = shintakeSubsystem;
+
+		addRequirements(this.shintakeSubsystem);
+	}
+
+	@Override
+	public void initialize() {
+		this.shintakeSubsystem.setIntakeSpeed(0);
+	}
+
+	@Override
+	public void execute() {
+		this.shintakeSubsystem.setIntakeSpeed(1);
+		this.shintakeSubsystem.setShooterGoal(0.4 * 12, ControlType.VOLTAGE);
+	}
+
+	@Override
+	public boolean isFinished() {
+		// return this.intakeSubsystem.hasNote();
+		return false;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		this.shintakeSubsystem.setIntakeSpeed(0);
+	}
+}

src/main/java/org/jmhsrobotics/frc2024/ComboCommands/ComboIntakeArmCommand.java

@@ -3,9 +3,8 @@
 import org.jmhsrobotics.frc2024.Constants;
 import org.jmhsrobotics.frc2024.subsystems.arm.ArmPIDSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.arm.commands.CommandArm;
-import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem;
-import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand;
-import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.shintake.commands.IntakeCommand;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
@@ -15,12 +14,11 @@ public class ComboIntakeArmCommand extends ParallelCommandGroup {
 	 * Moves arm to the Pickup position and Intakes.
 	 *
 	 * @param arm
-	 * @param shooter
-	 * @param intake
+	 * @param shintake
 	 */
-	public ComboIntakeArmCommand(ArmPIDSubsystem arm, ShooterSubsystem shooter, IntakeSubsystem intake) {
+	public ComboIntakeArmCommand(ArmPIDSubsystem arm, ShintakeSubsystem shintakeSubsystem) {
 		addCommands(new CommandArm(arm, Constants.ArmSetpoint.PICKUP.value), // move arm to intake position
-				new IntakeCommand(1, intake, shooter)// start intkae
+				new IntakeCommand(1, false, shintakeSubsystem)// start intkae
 		);
 	}
 

src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java

@@ -4,11 +4,12 @@
 
 package org.jmhsrobotics.frc2024;
 
-import org.jmhsrobotics.frc2024.ComboCommands.AmpHelper;
+import org.jmhsrobotics.frc2024.ComboCommands.AmpShotCommand;
 import org.jmhsrobotics.frc2024.ComboCommands.ComboIntakeArmCommand;
-import org.jmhsrobotics.frc2024.autoCommands.AutoAmpShotCommand;
-import org.jmhsrobotics.frc2024.autoCommands.FireCommand;
-import org.jmhsrobotics.frc2024.autoCommands.TurnAndShootCommand;
+import org.jmhsrobotics.frc2024.autoCommands.NFireAmp;
+import org.jmhsrobotics.frc2024.autoCommands.NFloorIntake;
+import org.jmhsrobotics.frc2024.autoCommands.NSpinupAndShoot;
+import org.jmhsrobotics.frc2024.autoCommands.NSpinupNoStop;
 import org.jmhsrobotics.frc2024.controlBoard.CompControl;
 import org.jmhsrobotics.frc2024.controlBoard.ControlBoard;
 import org.jmhsrobotics.frc2024.controlBoard.SingleControl;
@@ -28,21 +29,12 @@
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.LockSpeaker;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.auto.DriveTimeCommand;
-import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem;
-import org.jmhsrobotics.frc2024.subsystems.intake.commands.AmpShotCommand;
-import org.jmhsrobotics.frc2024.subsystems.intake.commands.AutoIntakeCommand;
-import org.jmhsrobotics.frc2024.subsystems.intake.commands.DefaultIntakeCommand;
-import org.jmhsrobotics.frc2024.subsystems.intake.commands.ExtakeCommand;
-import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand;
-import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeFireCommand;
-import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem;
-import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterAutoCommand;
+import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.shintake.commands.DefaultShintakeCommand;
+import org.jmhsrobotics.frc2024.subsystems.shintake.commands.IntakeCommand;
+import org.jmhsrobotics.frc2024.subsystems.shintake.commands.ShooterAutoCommand;
 import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem;
 import org.jmhsrobotics.frc2024.utils.RumbleTimeCommand;
-import org.jmhsrobotics.frc2024.utils.newcmd.NFireAmp;
-import org.jmhsrobotics.frc2024.utils.newcmd.NFloorIntake;
-import org.jmhsrobotics.frc2024.utils.newcmd.NSpinupAndShoot;
-import org.jmhsrobotics.frc2024.utils.newcmd.NSpinupNoStop;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
@@ -72,9 +64,11 @@ public class RobotContainer implements Logged {
 
 	private final VisionSubsystem visionSubsystem = new VisionSubsystem(this.driveSubsystem);
 
-	private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
+	// private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
 	private final LEDSubsystem ledSubsystem = new LEDSubsystem();
-	private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
+
+	// private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
+	private final ShintakeSubsystem shintakeSubsystem = new ShintakeSubsystem();
 
 	private final ArmPIDSubsystem armSubsystem = new ArmPIDSubsystem();
 
@@ -100,14 +94,12 @@ public RobotContainer() {
 		this.driveSubsystem
 				.setDefaultCommand(new DriveCommand(this.driveSubsystem, this.visionSubsystem, this.control));
 
-		this.intakeSubsystem.setDefaultCommand(new DefaultIntakeCommand(intakeSubsystem, shooterSubsystem));
+		this.shintakeSubsystem.setDefaultCommand(new DefaultShintakeCommand(this.shintakeSubsystem));
 
 		this.ledSubsystem.setDefaultCommand(new RainbowLEDCommand(this.ledSubsystem));
 
 		configureSmartDashboard();
-		SmartDashboard.putBoolean("HasNote", false);
 		configureDriverFeedback();
-
 		configureBindings();
 		// Named commands must be added before building the chooser.
 		configurePathPlanner();
@@ -133,61 +125,47 @@ public RobotContainer() {
 
 		var preLoadOnePiece = Commands.sequence(
 				Commands.race(new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value),
-						new NSpinupNoStop(this.shooterSubsystem, 5000)),
-				new NSpinupAndShoot(this.shooterSubsystem, this.intakeSubsystem, 5000));
+						new NSpinupNoStop(this.shintakeSubsystem, 5000)),
+				new NSpinupAndShoot(this.shintakeSubsystem, 5000));
 		autoChooser.addOption("preLoadOnePiece", preLoadOnePiece);
 
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 		SmartDashboard.putData("Scheduler", CommandScheduler.getInstance());
 		// Commands to test
 		SmartDashboard.putData("Arm Preset Shoot",
 				new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value));
-		SmartDashboard.putData("Intake Floor", new NFloorIntake(armSubsystem, intakeSubsystem));
-		SmartDashboard.putData("Fire in Amp", new NFireAmp(this.shooterSubsystem, this.intakeSubsystem));
-		SmartDashboard.putData("Spinup and Shoot", new NSpinupAndShoot(shooterSubsystem, intakeSubsystem, 5000));
-		SmartDashboard.putData("Spinup no Stop", new NSpinupNoStop(shooterSubsystem, 5000));
+		SmartDashboard.putData("Intake Floor", new NFloorIntake(armSubsystem, shintakeSubsystem));
+		SmartDashboard.putData("Fire in Amp", new NFireAmp(shintakeSubsystem));
+		SmartDashboard.putData("Spinup and Shoot", new NSpinupAndShoot(shintakeSubsystem, 5000));
+		SmartDashboard.putData("Spinup no Stop", new NSpinupNoStop(shintakeSubsystem, 5000));
 		SmartDashboard.putData("Aim Arm Vision",
 				new ArmVision(armSubsystem, visionSubsystem, driveSubsystem).until(armSubsystem::atGoal)); // TODO:
 																											// Handle
 																											// End
-		// // condition
-		// SmartDashboard.putData("RedLED", new setRedLEDCommand(ledSubsystem,
-		// intakeSubsystem));
 		SmartDashboard.putData("ArmVision", new ArmVision(armSubsystem, visionSubsystem, driveSubsystem));
 		SmartDashboard.putData("Lock Speaker", new LockSpeaker(driveSubsystem, visionSubsystem));
 
 	}
 
 	private void configureDriverFeedback() {
-		new Trigger(intakeSubsystem::hasNote)
+		new Trigger(shintakeSubsystem::hasNote)
 				.onTrue(new ParallelCommandGroup(new RumbleTimeCommand(control, RumbleType.kLeftRumble, 1, 1),
-						new setBlueLEDCommand(ledSubsystem, this.intakeSubsystem)));
-
-		// new Trigger(this.defaultIntakeCommand::isScheduled)
-		// .onTrue(new FlashingLEDCommand(ledSubsystem, intakeSubsystem));
-		// new Trigger(intakeSubsystem::isIntaking).onTrue(new
-		// FlashingLEDCommand(ledSubsystem, intakeSubsystem));
+						new setBlueLEDCommand(ledSubsystem, this.shintakeSubsystem)));
 
-		// new Trigger(intakeSubsystem.getCurrentCommand()==new
-		// DefaultIntakeCommand(this.intakeSubsystem, this.shooterSubsystem)).onTrue(new
-		// FlashingLEDCommand(ledSubsystem, intakeSubsystem));
-		// new Trigger(this.defaultIntakeCommand::isScheduled)
-		// .onTrue(new FlashingLEDCommand(ledSubsystem, intakeSubsystem));
+		new Trigger(shintakeSubsystem::noteTooHigh).onTrue(new setRedLEDCommand(ledSubsystem, shintakeSubsystem));
 
-		new Trigger(intakeSubsystem::noteTooHigh).onTrue(new setRedLEDCommand(ledSubsystem, intakeSubsystem));
-
-		new Trigger(shooterSubsystem::atGoal)
+		new Trigger(shintakeSubsystem::shooterAtGoal)
 				.whileTrue(new RumbleTimeCommand(this.control, RumbleType.kRightRumble, 0.2, 1));
 
-		new Trigger(shooterSubsystem::atGoal).onTrue(new setYellowLEDCommand(ledSubsystem, shooterSubsystem));
+		new Trigger(shintakeSubsystem::shooterAtGoal).onTrue(new setYellowLEDCommand(ledSubsystem, shintakeSubsystem));
 	}
 
 	private void configurePathPlanner() {
 		// Add path planner auto chooser.
 		var preLoadOnePiece = Commands.sequence(
 				Commands.race(new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value),
-						new NSpinupNoStop(this.shooterSubsystem, 5000)),
-				new NSpinupAndShoot(this.shooterSubsystem, this.intakeSubsystem, 5000));
+						new NSpinupNoStop(this.shintakeSubsystem, 5000)),
+				new NSpinupAndShoot(this.shintakeSubsystem, 5000));
 
 		AutoBuilder.configureHolonomic(driveSubsystem::getPose, driveSubsystem::resetOdometry,
 				driveSubsystem::getChassisSpeeds, driveSubsystem::drive,
@@ -197,42 +175,26 @@ private void configurePathPlanner() {
 
 		NamedCommands.registerCommand("ArmAmp", new CommandArm(this.armSubsystem, Constants.ArmSetpoint.AMP.value));
 		NamedCommands.registerCommand("ArmShoot", new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value));
-		NamedCommands.registerCommand("Extake", new ExtakeCommand(this.intakeSubsystem, 1).withTimeout(5));
-		NamedCommands.registerCommand("TurnAndShoot", new TurnAndShootCommand(this.visionSubsystem, this.driveSubsystem,
-				this.armSubsystem, this.shooterSubsystem, this.intakeSubsystem));
-		NamedCommands.registerCommand("Intake",
-				new IntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem).withTimeout(0.5));
-		NamedCommands.registerCommand("AutoIntake",
-				new AutoIntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem));
+		NamedCommands.registerCommand("Intake", new IntakeCommand(1, false, this.shintakeSubsystem).withTimeout(0.5));
 
 		// Move Arm to Pickup position
 		NamedCommands.registerCommand("ArmPickup",
 				new CommandArm(this.armSubsystem, Constants.ArmSetpoint.PICKUP.value));
-		NamedCommands.registerCommand("Fire", new FireCommand(this.intakeSubsystem, this.shooterSubsystem));
 		NamedCommands.registerCommand("PrepareShot",
-				new PrepareShot(this.driveSubsystem, this.armSubsystem, this.shooterSubsystem, this.visionSubsystem)
+				new PrepareShot(this.driveSubsystem, this.armSubsystem, this.shintakeSubsystem, this.visionSubsystem)
 						.withTimeout(1));
-		NamedCommands.registerCommand("AmpScore",
-				new AmpHelper(this.armSubsystem, this.shooterSubsystem, this.intakeSubsystem));
 
-		// New Commands
+		// AutoCommands
 		NamedCommands.registerCommand("Arm Preset Shoot",
 				new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value));
-		NamedCommands.registerCommand("Intake Floor", new NFloorIntake(armSubsystem, intakeSubsystem).withTimeout(2));
-		NamedCommands.registerCommand("Fire in Amp", new NFireAmp(this.shooterSubsystem, this.intakeSubsystem));
-		NamedCommands.registerCommand("Spinup and Shoot", new NSpinupAndShoot(shooterSubsystem, intakeSubsystem, 5000));
-		NamedCommands.registerCommand("Spinup no Stop", new NSpinupNoStop(shooterSubsystem, 5000));
+		NamedCommands.registerCommand("Intake Floor", new NFloorIntake(armSubsystem, shintakeSubsystem).withTimeout(2));
+		NamedCommands.registerCommand("Fire in Amp", new NFireAmp(this.shintakeSubsystem));
+		NamedCommands.registerCommand("Spinup and Shoot", new NSpinupAndShoot(shintakeSubsystem, 5000));
+		NamedCommands.registerCommand("Spinup no Stop", new NSpinupNoStop(shintakeSubsystem, 5000));
 		NamedCommands.registerCommand("Aim Arm Vision", new ArmVision(armSubsystem, visionSubsystem, driveSubsystem)); // TODO:
 																														// Handle
 																														// End
-		// condition
 		NamedCommands.registerCommand("Lock Speaker", new LockSpeaker(driveSubsystem, visionSubsystem));
-		NamedCommands.registerCommand("ComboIntake",
-				new ComboIntakeArmCommand(this.armSubsystem, this.shooterSubsystem, this.intakeSubsystem)
-						.withTimeout(1));
-		// NamedCommands.registerCommand("AmpShoot", new AmpShotCommand(intakeSubsystem,
-		// shooterSubsystem).withTimeout(1));
-		NamedCommands.registerCommand("AmpShoot", new AutoAmpShotCommand(this.intakeSubsystem, this.shooterSubsystem));
 		NamedCommands.registerCommand("One Piece Preload Shoot", preLoadOnePiece);
 	}
 
@@ -253,16 +215,16 @@ private void configureBindings() {
 		this.control.presetMid().onTrue(new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value));
 		// this.control.presetLow().onTrue(new CommandArm(this.armSubsystem,
 		// Constants.ArmSetpoint.PICKUP.value));
-		this.control.presetLow().whileTrue(new ComboIntakeArmCommand(armSubsystem, shooterSubsystem, intakeSubsystem));
+		this.control.presetLow().whileTrue(new ComboIntakeArmCommand(armSubsystem, shintakeSubsystem));
 		this.control.presetLow().onFalse(new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value));
 
 		/* Intake Controls */
-		this.control.intakeInput().whileTrue(new IntakeFireCommand(1, this.intakeSubsystem));
-		this.control.extakeInput().whileTrue(new IntakeCommand(-1, this.intakeSubsystem, this.shooterSubsystem));
+		this.control.intakeInput().whileTrue(new IntakeCommand(1, true, this.shintakeSubsystem));
+		this.control.extakeInput().whileTrue(new IntakeCommand(-1, false, this.shintakeSubsystem));
 
 		/* Shooter Controls */
-		this.control.shooterInput().whileTrue(new ShooterAutoCommand(this.shooterSubsystem, 5000));
-		this.control.ampShooterInput().whileTrue(new AmpShotCommand(intakeSubsystem, shooterSubsystem));
+		this.control.shooterInput().whileTrue(new ShooterAutoCommand(this.shintakeSubsystem, 5000));
+		this.control.ampShooterInput().whileTrue(new AmpShotCommand(shintakeSubsystem));
 
 		// temp climber controls
 		// this.control.climberRetract().onTrue(new ClimbCommand(this.climberSubsystem,
@@ -303,7 +265,7 @@ public void configureSmartDashboard() {
 	public void configureTeam() {
 		this.control.AprilLockOn()
 				.whileTrue(Commands.repeatingSequence(new ArmVision(armSubsystem, visionSubsystem, driveSubsystem)));
-		this.control.AprilLockOn().whileTrue(new ShooterAutoCommand(shooterSubsystem, 5000));
+		this.control.AprilLockOn().whileTrue(new ShooterAutoCommand(shintakeSubsystem, 5000));
 		// if (getAllianceFlipState()) {
 		// this.control.AprilLockOn()
 		// .whileTrue(Commands.repeatingSequence(new PrepareShot(driveSubsystem,