diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 970a1760..cd565224 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -14,10 +14,14 @@ import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.commands.TrampShoot; +import org.team1540.robot2024.commands.climb.ClimbSequence; +import org.team1540.robot2024.commands.climb.DeclimbSequence; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.commands.indexer.StageTrampCommand; +import org.team1540.robot2024.commands.shooter.ManualShooterCommand; +import org.team1540.robot2024.commands.shooter.PrepareShooterCommand; import org.team1540.robot2024.commands.shooter.ShootSequence; import org.team1540.robot2024.commands.shooter.TuneShooterCommand; import org.team1540.robot2024.subsystems.drive.*; @@ -25,6 +29,7 @@ import org.team1540.robot2024.subsystems.elevator.ElevatorIO; import org.team1540.robot2024.subsystems.elevator.ElevatorIOSim; import org.team1540.robot2024.subsystems.elevator.ElevatorIOTalonFX; +import org.team1540.robot2024.subsystems.fakesubsystems.Hooks; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.indexer.IndexerIO; import org.team1540.robot2024.subsystems.indexer.IndexerIOSim; @@ -56,6 +61,7 @@ public class RobotContainer { public final Tramp tramp; public final Shooter shooter; public final Elevator elevator; + public final Hooks hook; public final Indexer indexer; public final AprilTagVision aprilTagVision; @@ -87,6 +93,7 @@ public RobotContainer() { tramp = new Tramp(new TrampIOSparkMax()); shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX()); elevator = new Elevator(new ElevatorIOTalonFX()); + hook = new Hooks(); indexer = new Indexer( new IndexerIOSparkMax() @@ -111,6 +118,7 @@ public RobotContainer() { tramp = new Tramp(new TrampIOSim()); shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim()); elevator = new Elevator(new ElevatorIOSim()); + hook = new Hooks(); aprilTagVision = new AprilTagVision( new AprilTagVisionIOSim(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE, drivetrain::getPose), @@ -134,6 +142,7 @@ public RobotContainer() { new ModuleIO() {}); shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {}); elevator = new Elevator(new ElevatorIO() {}); + hook = new Hooks(); aprilTagVision = new AprilTagVision( new AprilTagVisionIO() {}, @@ -174,27 +183,32 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + // driver left (x,y) and right (x,y) -> drive drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); - elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); - copilot.rightBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.TOP)); - copilot.leftBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)); + // driver y -> zero field orientation, x -> x stop driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); - driver.b().onTrue( + driver.y().onTrue( Commands.runOnce( () -> drivetrain.setPose(new Pose2d(drivetrain.getPose().getTranslation(), new Rotation2d())), drivetrain ).ignoringDisable(true) ); - driver.rightBumper().onTrue(new IntakeCommand(indexer)); - driver.leftBumper().onTrue(new TrampShoot(tramp)); - //TODO: not sure how to do angling/don't think there's a command for this yet? - - copilot.a().onTrue(new ShootSequence(shooter, indexer)) + // driver left bumper -> shooter align and shoot + driver.leftBumper().onTrue(new ShootSequence(shooter, indexer)) .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter)); - copilot.rightBumper().onTrue(new StageTrampCommand(tramp, indexer)); + //TODO: driver needs angling command (Alvin wrote it) - // copilot: shooter staging, climbing, shooter override, elevator override, signaling - // use tramp stage + // copilot left and right trigger -> elevator up and down + elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); + // copilot left y -> shooter angle + shooter.setDefaultCommand(new ManualShooterCommand(shooter, copilot)); + // copilot left and right bumper -> climb up climb down + copilot.leftBumper().onTrue(new ClimbSequence(elevator, hook)); + copilot.rightBumper().onTrue(new DeclimbSequence(elevator, hook)); + // copilot a + b -> stage trap + stage shooter + copilot.a().onTrue(new StageTrampCommand(tramp, indexer)); + copilot.b().onTrue(new PrepareShooterCommand(shooter)); + //TODO: copilot needs signaling? (not sure how this works) } /** diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ManualShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/ManualShooterCommand.java new file mode 100644 index 00000000..514ac97c --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/shooter/ManualShooterCommand.java @@ -0,0 +1,30 @@ +package org.team1540.robot2024.commands.shooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team1540.robot2024.subsystems.shooter.Shooter; + +public class ManualShooterCommand extends Command{ + private final Shooter shooter; + private final CommandXboxController controller; + + public ManualShooterCommand(Shooter shooter, CommandXboxController controller) { + this.shooter = shooter; + addRequirements(shooter); + this.controller = controller; + } + @Override + public void execute() { + Rotation2d angleRotation2d = shooter.getPivotPosition(); + shooter.setPivotPosition(angleRotation2d.plus(Rotation2d.fromRotations(0.01*(controller.getLeftY())))); + // controller values range from -1 to 1 + // pivot position will be within 90 degrees + } + + @Override + public boolean isFinished() { + return shooter.isPivotAtSetpoint(); + } + } + diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java index 8d757fb8..56787464 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import org.team1540.robot2024.subsystems.shooter.Shooter; -class PrepareShooterCommand extends Command { +public class PrepareShooterCommand extends Command { private final Shooter shooter; public PrepareShooterCommand(Shooter shooter) { diff --git a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java index b9853233..151fe100 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java +++ b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java @@ -4,9 +4,13 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -//TODO: Get rid of this class and make an actual one its just a temporary thingie for David :D +//TODO: Get rid of this class and make an actual one its just a temporary thing for David :D public class Hooks extends SubsystemBase { + public Hooks(){ + //something + } + //TODO: Whoever does this it should retract or deploy the hooks depending on if they are already deployed or retracted public void deployHooks() { @@ -24,7 +28,7 @@ public Command deployHooksCommand() { } /** - * Factory method for undeploying hooks + * Factory method for un-deploying hooks */ public Command undeployHooksCommand() { return new InstantCommand(this::undeployHooks, this);