From f46241ce8b271e85eca33f979a23ddd6cad46d56 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 10 Mar 2026 20:17:44 -0400 Subject: [PATCH] Added commands, lotta commands, im sleepy --- TODOs.md | 10 ++-- .../robot/commands/ManualHopperCommand.java | 50 +++++++++++++++++ .../robot/commands/ManualShootCommand.java | 40 +++++++++++++ .../robot/commands/SetDeflectorCommand.java | 32 +++++++++++ .../frc/robot/subsystems/HopperSubsystem.java | 56 ++----------------- .../frc/robot/subsystems/IntakeSubsystem.java | 51 +++++++++-------- .../frc/robot/subsystems/TurretSubsystem.java | 5 +- 7 files changed, 159 insertions(+), 85 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ManualHopperCommand.java create mode 100644 src/main/java/frc/robot/commands/ManualShootCommand.java create mode 100644 src/main/java/frc/robot/commands/SetDeflectorCommand.java diff --git a/TODOs.md b/TODOs.md index f1cc0fc..98fe386 100644 --- a/TODOs.md +++ b/TODOs.md @@ -3,24 +3,24 @@ These are the jobs to be completed first, to ensure we have minimal functionality for RIT - Add subsystems to robot - - Define unique IDs and channels for all devices in Constants.java IP Dom + - Define unique IDs and channels for all devices in Constants.java (IP Dom) - Add subsystems to the RobotContainer X - Updates to subsystems X - Update launcher wheel to a Neo/SparkMax X - Define basic commands for manual fire mode - - Y/Top button: Deploy the intake (and run the rollers) + - Y/Top button: Deploy the intake (and run the rollers) (TO MAP) - Could be a single command - Could be 2 commands in sequence - - B/Right button: Reverse the intake + - B/Right button: Reverse the intake (TO MAP) - Unclear if this should touch the deploy motor. ATM suggest leaving the deploy motor alone - - A/Bottom button: Retract the intake (and stop the rollers) + - A/Bottom button: Retract the intake (and stop the rollers) (TO MAP) - D-pad: Set deflector to pre-programmed angle - - Suggestion: Implement this once, and make the angle an member variable of the command + - Suggestion: Implement this once, and make the angle an member variable of the command (TO MAP) - Both Triggers: Spin up flywheel (if not already spun up) and then start the kicker. - Re: Sensing flywheel speed diff --git a/src/main/java/frc/robot/commands/ManualHopperCommand.java b/src/main/java/frc/robot/commands/ManualHopperCommand.java new file mode 100644 index 0000000..fe7a2c5 --- /dev/null +++ b/src/main/java/frc/robot/commands/ManualHopperCommand.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import frc.robot.subsystems.HopperSubsystem; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class ManualHopperCommand extends Command { + private final HopperSubsystem m_subsystem; + private final XboxController m_xboxController; + + public ManualHopperCommand(HopperSubsystem subsystem, XboxController xboxController) { + m_subsystem = subsystem; + m_xboxController = xboxController; + addRequirements(subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_subsystem.spin(.5); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.5) + { + m_subsystem.feed(); + } + else + { + m_subsystem.stop(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ManualShootCommand.java b/src/main/java/frc/robot/commands/ManualShootCommand.java new file mode 100644 index 0000000..3827b32 --- /dev/null +++ b/src/main/java/frc/robot/commands/ManualShootCommand.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import frc.robot.subsystems.TurretSubsystem; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class ManualShootCommand extends Command { + private final TurretSubsystem m_subsystem; + private final XboxController m_controller; + + public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) { + m_subsystem = subsystem; + m_controller = controller; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(m_controller.getLeftTriggerAxis()>.5 || m_controller.getRightTriggerAxis()>.5) + { + m_subsystem.setLauncher(1); + } + else + { + m_subsystem.setLauncher(0); + } + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/SetDeflectorCommand.java b/src/main/java/frc/robot/commands/SetDeflectorCommand.java new file mode 100644 index 0000000..a670ed0 --- /dev/null +++ b/src/main/java/frc/robot/commands/SetDeflectorCommand.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import frc.robot.subsystems.TurretSubsystem; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. **/ +public class SetDeflectorCommand extends Command { + private final TurretSubsystem m_subsystem; + private Angle deployAngle; + + public SetDeflectorCommand(TurretSubsystem subsystem, Angle deployAngle) { + m_subsystem = subsystem; + this.deployAngle = deployAngle; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_subsystem.setTargetElevation(deployAngle); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 8dfe649..3da74ad 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -6,12 +6,10 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; - import com.ctre.phoenix6.hardware.TalonFX; - import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import frc.robot.Constants; -import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.ManualHopperCommand; public class HopperSubsystem extends TestableSubsystem { @@ -23,55 +21,13 @@ public class HopperSubsystem extends TestableSubsystem { kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); } - public void feed() { - kickerMotor.set(0.5); - } + public void feed() {kickerMotor.set(0.5);} - public void reverse() { - kickerMotor.set(-0.5); - } + public void reverse() {kickerMotor.set(-0.5);} - public void stop() { - kickerMotor.set(0); - } + public void stop() {kickerMotor.set(0);} - public void spin(double speed) { - spindexerMotor.set(speed); - } - - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } + public void spin(double speed) {spindexerMotor.set(speed);} @PostTest(name = "A friendly name", enabled = true) public void myFailingPostTest(){ @@ -82,4 +38,4 @@ public class HopperSubsystem extends TestableSubsystem { public void myPassingPostTest(){ assertThat("Hello World").endWith("World"); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index cbffe0c..7123b1b 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -7,15 +7,21 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix6.hardware.TalonFX; import frc.robot.Constants; import static org.fairportrobotics.frc.posty.assertions.Assertions.*; +import java.time.chrono.ThaiBuddhistChronology; + import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; public class IntakeSubsystem extends TestableSubsystem { /** Creates a new ExampleSubsystem. */ @@ -27,6 +33,8 @@ public class IntakeSubsystem extends TestableSubsystem { { intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID); deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID); + deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); } public void neutral() @@ -49,38 +57,29 @@ public class IntakeSubsystem extends TestableSubsystem { intakeMotor.set(speed); } - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); + public boolean isDeployed() + { + return deployMotor.isFwdLimitSwitchClosed()==1; } - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; + public boolean isRetracted() + { + return deployMotor.isRevLimitSwitchClosed()==1; } - @Override - public void periodic() { - // This method will be called once per scheduler run + public Command deployCommand() + { + return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(50), () -> setSpeed(0), this)); } - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation + public Command reverseCommand() + { + return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(-50), () -> setSpeed(0), this)); + } + + public Command retractCommand() + { + return this.runOnce(() -> setSpeed(0)).andThen(new FunctionalCommand(this::retract, this::retract, interrupted -> neutral(), this::isRetracted, this)); } @PostTest(name = "A friendly name", enabled = true) diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 3db5e42..e4e4057 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -54,10 +54,7 @@ public class TurretSubsystem extends TestableSubsystem { targElev = edu.wpi.first.units.Units.Degrees.of(45); } - public void setLauncher(double speed) - { - launcherMotor.set(speed); - } + public void setLauncher(double speed) {launcherMotor.set(speed);} public void setTargetElevation(Angle elev) {