diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/simgui.json b/simgui.json index 5f9d275..3679e10 100644 --- a/simgui.json +++ b/simgui.json @@ -1,9 +1,28 @@ { + "HALProvider": { + "DIO": { + "window": { + "visible": true + } + }, + "RoboRIO": { + "window": { + "visible": true + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo" } }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "open": true + } + } + }, "NetworkTables Info": { "visible": true } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 58a214e..9a38ac4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,7 +24,9 @@ public static class constLED { public static final int[] LED_INTAKE_GROUND = { 187, 1, 249 }; public static final int[] LED_HASGP = { 249, 1, 109 }; public static final int[] LED_FULL_HOPPER = { 255, 255, 255 }; - public static final int[] LED_EJECTGP = { 3, 242, 230 }; + public static final int[] LED_EJECT_INTAKE = { 3, 242, 230 }; + public static final int[] LED_EJECT_SHOOTER = { 242, 3, 230 }; + public static final int[] LED_DRIVE = { 109, 11, 140 }; } @@ -49,6 +51,5 @@ public static class constShooter { public static class constStager { public static final double STAGER_MOTOR_VELOCITY = 0.3; - public static final double TOP_STAGER_MOTOR_VELOCITY = 0.3; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ca01572..7f70f30 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,18 +4,16 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.Drive; -import frc.robot.commands.EjectShooter; -import frc.robot.commands.IntakeGround; -import frc.robot.commands.PrepShooter; -import frc.robot.commands.Shoot; -import frc.robot.commands.intakeHopper; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Hopper; import frc.robot.subsystems.Intake; import frc.robot.subsystems.LED; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Stager; +import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; public class RobotContainer { private final SN_XboxController m_driverController = new SN_XboxController(RobotMap.mapControllers.DRIVER_USB); @@ -25,14 +23,16 @@ public class RobotContainer { private final Hopper subHopper = new Hopper(); private final Shooter subShooter = new Shooter(); private final Stager subStager = new Stager(); + private final StateMachine subStateMachine = new StateMachine(subHopper, subIntake, subShooter, subStager, subLED); + private final Drive com_Drive = new Drive(subDrivetrain, m_driverController.axis_RightX, m_driverController.axis_LeftY, m_driverController.btn_LeftBumper); - private final IntakeGround com_IntakeGround = new IntakeGround(subIntake, subStager, subLED); - private final PrepShooter com_PrepShooter = new PrepShooter(subShooter, subLED); - private final Shoot com_Shoot = new Shoot(subStager, subShooter, subLED); - private final intakeHopper com_IntakeHopper = new intakeHopper(subHopper, subStager, subLED); - private final EjectShooter com_EjectShooter = new EjectShooter(subStager, subShooter, subLED); + private final Trigger hasGamePieceTrigger = new Trigger(subStager::getHasGP); + private final Trigger isGPDetectedTrigger = new Trigger(subHopper::getGamePieceHopper); + + + // Drive public RobotContainer() { subDrivetrain.setDefaultCommand(com_Drive); m_driverController.setLeftDeadband(Constants.constDrivetrain.CONTROLLER_DEADZONE); @@ -41,15 +41,60 @@ public RobotContainer() { } private void configureBindings() { - m_driverController.btn_LeftTrigger.whileTrue(com_IntakeGround); - m_driverController.btn_A.onTrue(com_PrepShooter); - m_driverController.btn_B.whileTrue(com_IntakeHopper); - m_driverController.btn_LeftBumper.whileTrue(com_EjectShooter); - m_driverController.btn_RightTrigger.onTrue(com_Shoot); + // Intake + m_driverController.btn_LeftTrigger + .whileTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.INTAKE_GROUND))) + .onFalse(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.NONE)) + .onlyIf(subStateMachine::desiredIsCurrent)); + + // PrepShooter + m_driverController.btn_A + .whileTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.PREP_SHOOTER))); + + // IntakeHopper + isGPDetectedTrigger + .whileTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.INTAKE_HOPPER))); + + // EjectShooter + m_driverController.btn_RightBumper + .whileTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.EJECT_SHOOTER))) + .onFalse(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.NONE)).onlyIf(subStateMachine::desiredIsCurrent)); + + // EjectIntake + m_driverController.btn_LeftBumper + .whileTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.EJECT_INTAKE))) + .onFalse(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.NONE)).onlyIf(subStateMachine::desiredIsCurrent)); + + // Shoot + m_driverController.btn_RightTrigger + .onTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.SHOOT))) + .onFalse(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.NONE)).onlyIf(subStateMachine::desiredIsCurrent) ); + + // hasGP + + hasGamePieceTrigger + .whileTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.HAS_GP))); + + // StopShooter + m_driverController.btn_X + .onTrue(Commands.deferredProxy( + () -> subStateMachine.tryState(RobotState.STOP_SHOOTER))); } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return Commands.print("No autonomous command configured :3"); } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/EjectGP.java b/src/main/java/frc/robot/commands/EjectIntake.java similarity index 69% rename from src/main/java/frc/robot/commands/EjectGP.java rename to src/main/java/frc/robot/commands/EjectIntake.java index e356223..6992d61 100644 --- a/src/main/java/frc/robot/commands/EjectGP.java +++ b/src/main/java/frc/robot/commands/EjectIntake.java @@ -6,27 +6,33 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -import frc.robot.Constants.constLED; - import frc.robot.subsystems.Intake; import frc.robot.subsystems.LED; +import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; -public class EjectGP extends Command { +public class EjectIntake extends Command { /** Creates a new EjectGP. */ - LED globalLED; + Intake globalIntake; + StateMachine globalStateMachine; + LED globalLED; - public EjectGP(Intake subIntake, LED ejectLED) { + public EjectIntake(StateMachine passedStateMachine, Intake subIntake, LED subLED) { // Use addRequirements() here to declare subsystem dependencies. this.globalIntake = subIntake; - globalLED = ejectLED; + globalStateMachine = passedStateMachine; + globalLED = subLED; + addRequirements(globalStateMachine); + } // Called when the command is initially scheduled. @Override public void initialize() { + globalLED.setLEDs(Constants.constLED.LED_EJECT_INTAKE); + globalStateMachine.setState(RobotState.EJECT_INTAKE); globalIntake.setIntakeVelocity(Constants.constIntake.INTAKE_EJECT_VELOCITY); - globalLED.setLEDs(constLED.LED_EJECTGP); } // Called every time the scheduler runs while the command is scheduled. @@ -37,7 +43,6 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - globalIntake.setIntakeNuetralOutput(); } diff --git a/src/main/java/frc/robot/commands/EjectShooter.java b/src/main/java/frc/robot/commands/EjectShooter.java index 5df817f..aa2c872 100644 --- a/src/main/java/frc/robot/commands/EjectShooter.java +++ b/src/main/java/frc/robot/commands/EjectShooter.java @@ -9,23 +9,31 @@ import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Stager; import frc.robot.Constants; +import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; public class EjectShooter extends Command { /** Creates a new EjectShooter. */ Stager globalStager; Shooter globalShooter; + StateMachine globalStateMachine; LED globalLED; - public EjectShooter(Stager passedStager, Shooter passedShooter, LED ejectLED) { + public EjectShooter(StateMachine passedStateMachine, Stager passedStager, Shooter passedShooter, LED ejectLED) { // Use addRequirements() here to declare subsystem dependencies. globalStager = passedStager; globalShooter = passedShooter; globalLED = ejectLED; + globalStateMachine = passedStateMachine; + addRequirements(globalStateMachine); + } // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setState(RobotState.EJECT_SHOOTER); + globalLED.setLEDs(Constants.constLED.LED_EJECT_SHOOTER); globalShooter.setPropelMotorVelocity(Constants.constShooter.PROPEL_MOTOR_VELOCITY_EJECT); globalShooter.setSpiralMotorVelocity(Constants.constShooter.SPIRAL_MOTOR_VELOCITY_EJECT); } @@ -36,7 +44,6 @@ public void execute() { if ((globalShooter.getPropelMotorVelocity() >= Constants.constShooter.PROPEL_MOTOR_VELOCITY_EJECT_REQUIREMENT) && globalShooter.getSpiralMotorVelocity() >= Constants.constShooter.SPIRAL_MOTOR_VELOCITY_EJECT_REQUIREMENT) { globalStager.setStagerMotorVelocity(Constants.constStager.STAGER_MOTOR_VELOCITY); - globalStager.setTopStagerMotorVelocity(Constants.constStager.TOP_STAGER_MOTOR_VELOCITY); } else { globalStager.setStagerMotorVelocityNuetralOutput(); } diff --git a/src/main/java/frc/robot/commands/FullHopper.java b/src/main/java/frc/robot/commands/FullHopper.java deleted file mode 100644 index f8fe667..0000000 --- a/src/main/java/frc/robot/commands/FullHopper.java +++ /dev/null @@ -1,45 +0,0 @@ -// 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 edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.constLED; -import frc.robot.subsystems.Hopper; -import frc.robot.subsystems.LED; - -public class FullHopper extends Command { - /** Creates a new FullHopper. */ - Hopper subHopper; - LED subLED; - - public FullHopper(Hopper subHopper, LED fullLED) { - // Use addRequirements() here to declare subsystem dependencies. - this.subHopper = subHopper; - subLED = fullLED; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - subLED.setLEDs(constLED.LED_FULL_HOPPER); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - } - - // 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 true; - } -} diff --git a/src/main/java/frc/robot/commands/HasGP.java b/src/main/java/frc/robot/commands/HasGP.java index a4c9e2d..67a7302 100644 --- a/src/main/java/frc/robot/commands/HasGP.java +++ b/src/main/java/frc/robot/commands/HasGP.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Stager; +import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; import frc.robot.Constants.constLED; import frc.robot.subsystems.LED; @@ -15,19 +17,25 @@ public class HasGP extends Command { Stager globalStager; LED globalLED; Shooter globalShooter; + StateMachine globalStateMachine; - public HasGP(Stager passedStager, Shooter passedShooter, LED stagerLED) { + public HasGP(Stager passedStager, Shooter passedShooter, StateMachine passedStateMachine, LED stagerLED) { // Use addRequirements() here to declare subsystem dependencies. globalStager = passedStager; globalShooter = passedShooter; globalLED = stagerLED; + globalStateMachine = passedStateMachine; + addRequirements(globalStateMachine); } // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setState(RobotState.HAS_GP); globalLED.setLEDs(constLED.LED_HASGP); globalShooter.setShooterNuetralOutput(); + globalStager.getHasGP(); + } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/IntakeGround.java b/src/main/java/frc/robot/commands/IntakeGround.java index ec29280..fe663b2 100644 --- a/src/main/java/frc/robot/commands/IntakeGround.java +++ b/src/main/java/frc/robot/commands/IntakeGround.java @@ -6,6 +6,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Stager; +import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; import frc.robot.Constants; import frc.robot.Constants.constLED; import frc.robot.subsystems.Intake; @@ -15,10 +17,13 @@ public class IntakeGround extends Command { /** Creates a new IntakeGround. */ Intake globalIntake; Stager globalStager; + StateMachine globalStateMachine; LED globalLED; - public IntakeGround(Intake passedIntake, Stager passedStager, LED groundIntakeLED) { + public IntakeGround(StateMachine passedStateMachine, Intake passedIntake, Stager passedStager, LED groundIntakeLED) { // Use addRequirements() here to declare subsystem dependencies. + globalStateMachine = passedStateMachine; + addRequirements(globalStateMachine); globalIntake = passedIntake; globalStager = passedStager; globalLED = groundIntakeLED; @@ -27,6 +32,7 @@ public IntakeGround(Intake passedIntake, Stager passedStager, LED groundIntakeLE // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setState(RobotState.INTAKE_GROUND); globalIntake.setIntakeNuetralOutput(); globalLED.setLEDs(constLED.LED_INTAKE_GROUND); } diff --git a/src/main/java/frc/robot/commands/PrepShooter.java b/src/main/java/frc/robot/commands/PrepShooter.java index c111e0b..4b6f584 100644 --- a/src/main/java/frc/robot/commands/PrepShooter.java +++ b/src/main/java/frc/robot/commands/PrepShooter.java @@ -9,21 +9,28 @@ import frc.robot.Constants.constLED; import frc.robot.subsystems.LED; import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; public class PrepShooter extends Command { /** Creates a new PrepShooter. */ Shooter globalShooter; + StateMachine globalStateMachine; LED globalLED; - public PrepShooter(Shooter passedShooter, LED prepLED) { + public PrepShooter(StateMachine passedStateMachine, Shooter passedShooter, LED prepLED) { // Use addRequirements() here to declare subsystem dependencies. globalShooter = passedShooter; globalLED = prepLED; + globalStateMachine = passedStateMachine; + addRequirements(globalStateMachine); + } // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setState(RobotState.PREP_SHOOTER); globalShooter.setPropelMotorVelocity(Constants.constShooter.PROPEL_MOTOR_VELOCITY); globalShooter.setSpiralMotorVelocity(Constants.constShooter.SPIRAL_MOTOR_VELOCITY); globalLED.setLEDs(constLED.LED_PREP_SHOOTING); @@ -37,6 +44,8 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + globalShooter.setPropelMotorVelocity(Constants.constShooter.PROPEL_MOTOR_VELOCITY); + globalShooter.setSpiralMotorVelocity(Constants.constShooter.SPIRAL_MOTOR_VELOCITY); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/Shoot.java b/src/main/java/frc/robot/commands/Shoot.java index 8abeb9b..91892d9 100644 --- a/src/main/java/frc/robot/commands/Shoot.java +++ b/src/main/java/frc/robot/commands/Shoot.java @@ -10,15 +10,20 @@ import frc.robot.subsystems.LED; import frc.robot.Constants; import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.StateMachine.RobotState; +import frc.robot.subsystems.StateMachine; public class Shoot extends Command { /** Creates a new Shoot. */ + StateMachine globalStateMachine; Stager globalStager; Shooter globalShooter; LED globalLED; - public Shoot(Stager passedStager, Shooter passedShooter, LED shootLED) { + public Shoot(StateMachine passedStateMachine, Stager passedStager, Shooter passedShooter, LED shootLED) { // Use addRequirements() here to declare subsystem dependencies. + globalStateMachine = passedStateMachine; + addRequirements(globalStateMachine); globalStager = passedStager; globalShooter = passedShooter; globalLED = shootLED; @@ -27,12 +32,12 @@ public Shoot(Stager passedStager, Shooter passedShooter, LED shootLED) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setState(RobotState.SHOOT); globalShooter.setPropelMotorVelocity(Constants.constShooter.PROPEL_MOTOR_VELOCITY); globalShooter.setSpiralMotorVelocity(Constants.constShooter.SPIRAL_MOTOR_VELOCITY); if ((globalShooter.getPropelMotorVelocity() >= Constants.constShooter.PROPEL_MOTOR_VELOCITY) && (globalShooter.getSpiralMotorVelocity() >= Constants.constShooter.SPIRAL_MOTOR_VELOCITY)) { globalStager.setStagerMotorVelocity(Constants.constStager.STAGER_MOTOR_VELOCITY); - globalStager.setTopStagerMotorVelocity(Constants.constStager.TOP_STAGER_MOTOR_VELOCITY); globalLED.setLEDs(constLED.LED_SHOOTING); } else { globalStager.setStagerMotorVelocityNuetralOutput(); diff --git a/src/main/java/frc/robot/commands/intakeHopper.java b/src/main/java/frc/robot/commands/intakeHopper.java index 01ee104..c341445 100644 --- a/src/main/java/frc/robot/commands/intakeHopper.java +++ b/src/main/java/frc/robot/commands/intakeHopper.java @@ -10,19 +10,25 @@ import frc.robot.subsystems.Hopper; import frc.robot.subsystems.LED; import frc.robot.subsystems.Stager; +import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; public class intakeHopper extends Command { Hopper subHopper; Stager subStager; + StateMachine globalStateMachine; LED subLED; /** Creates a new intakeHopper. */ - public intakeHopper(Hopper subHopper, Stager subStager, LED intakeHopperLED) { + public intakeHopper(StateMachine subStateMachine, Hopper subHopper, Stager subStager, LED intakeHopperLED) { // Use addRequirements() here to declare subsystem dependencies. this.subHopper = subHopper; this.subStager = subStager; subLED = intakeHopperLED; + globalStateMachine = subStateMachine; + addRequirements(globalStateMachine); + } // Called when the command is initially scheduled. @@ -36,6 +42,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + globalStateMachine.setState(RobotState.INTAKE_HOPPER); if (!subStager.getHasGP()) { subHopper.setOrientationMotorSpeed(Constants.constHopper.HOPPER_ORIENTATION_SPEED); } else { diff --git a/src/main/java/frc/robot/commands/none.java b/src/main/java/frc/robot/commands/none.java index 835169a..9368663 100644 --- a/src/main/java/frc/robot/commands/none.java +++ b/src/main/java/frc/robot/commands/none.java @@ -11,17 +11,23 @@ import frc.robot.subsystems.LED; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Stager; +import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; public class none extends Command { Hopper subHopper; Intake subIntake; Shooter subShooter; Stager subStager; + StateMachine globalStateMachine; LED subLED; /** Creates a new none. */ - public none(Hopper passedHopper, Intake passedIntake, Shooter passedShooter, Stager passedStager, LED noneLED) { + public none(StateMachine passedStateMachine, Hopper passedHopper, Intake passedIntake, Shooter passedShooter, + Stager passedStager, LED noneLED) { // Use addRequirements() here to declare subsystem dependencies. + globalStateMachine = passedStateMachine; + addRequirements(globalStateMachine); subHopper = passedHopper; subIntake = passedIntake; subShooter = passedShooter; @@ -32,6 +38,7 @@ public none(Hopper passedHopper, Intake passedIntake, Shooter passedShooter, Sta // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setState(RobotState.NONE); subHopper.setOrientationMotorSpeed(0); subIntake.setIntakeVelocity(0); subStager.setStagerMotorVelocity(0); diff --git a/src/main/java/frc/robot/commands/states/StopShooter.java b/src/main/java/frc/robot/commands/states/StopShooter.java new file mode 100644 index 0000000..0a7ec0a --- /dev/null +++ b/src/main/java/frc/robot/commands/states/StopShooter.java @@ -0,0 +1,46 @@ +// 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.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; + +public class StopShooter extends Command { + /** Creates a new StopShooter. */ + Shooter globalShooter; + StateMachine globalStateMachine; + public StopShooter(StateMachine passedStateMachine, Shooter passedShooter) { + // Use addRequirements() here to declare subsystem dependencies. + globalStateMachine = passedStateMachine; + addRequirements(globalStateMachine); + globalShooter = passedShooter; + + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + globalStateMachine.setState(RobotState.STOP_SHOOTER); + globalShooter.setShooterNuetralOutput(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // 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/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index dff0759..9564fdd 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -7,6 +7,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotMap; @@ -36,6 +37,7 @@ public boolean getGamePieceHopper() { @Override public void periodic() { + SmartDashboard.putBoolean("GP Hopper Sensor", getGamePieceHopper()); } } diff --git a/src/main/java/frc/robot/subsystems/Stager.java b/src/main/java/frc/robot/subsystems/Stager.java index f921688..3292e2c 100644 --- a/src/main/java/frc/robot/subsystems/Stager.java +++ b/src/main/java/frc/robot/subsystems/Stager.java @@ -16,12 +16,10 @@ public class Stager extends SubsystemBase { /** Creates a new Stager. */ TalonSRX stagerMotor; - TalonSRX topStagerMotor; DigitalInput hasGP; public Stager() { stagerMotor = new TalonSRX(RobotMap.mapStager.STAGER_MOTOR_CAN); - topStagerMotor = new TalonSRX(RobotMap.mapStager.TOP_STAGER_MOTOR_CAN); hasGP = new DigitalInput(RobotMap.mapStager.HAS_GP_DIO); } @@ -30,12 +28,10 @@ public void setStagerMotorVelocity(double velocity) { } public void setTopStagerMotorVelocity(double velocity) { - topStagerMotor.set(ControlMode.PercentOutput, velocity); } public void setStagerMotorVelocityNuetralOutput() { stagerMotor.set(ControlMode.PercentOutput, 0); - topStagerMotor.set(ControlMode.PercentOutput, 0); } public boolean getHasGP() { @@ -45,6 +41,6 @@ public boolean getHasGP() { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putBoolean("hasGP", !getHasGP()); + SmartDashboard.putBoolean("hasGP", getHasGP()); } } diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java new file mode 100644 index 0000000..2f4890c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -0,0 +1,141 @@ +// 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.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.commands.*; +import frc.robot.commands.states.StopShooter; + +public class StateMachine extends SubsystemBase { + /** Creates a new StateMachine. */ + public static RobotState currentState; + public static RobotState globalDesiredState; + Hopper globalHopper; + Intake globalIntake; + Shooter globalShooter; + Stager globalStager; + LED globalLED; + StateMachine globalStateMachine = this; + + public StateMachine(Hopper passedHopper, Intake passedIntake, Shooter passedShooter, Stager passedStager, + LED passedLED) { + globalHopper = passedHopper; + globalIntake = passedIntake; + globalShooter = passedShooter; + globalStager = passedStager; + globalLED = passedLED; + currentState = RobotState.NONE; + globalDesiredState = currentState; + } + + public enum RobotState { + EJECT_INTAKE, + EJECT_SHOOTER, + HAS_GP, + INTAKE_GROUND, + INTAKE_HOPPER, + NONE, + PREP_SHOOTER, + SHOOT, + STOP_SHOOTER, + } + + public void setState(RobotState state) { + currentState = state; + } + + public RobotState getState() { + return currentState; + } + + public boolean desiredIsCurrent() { + return globalDesiredState == currentState; + } + + public Command tryState(RobotState desiredState) { + globalDesiredState = desiredState; + switch (desiredState) { // check what our desired state is + case EJECT_INTAKE: // if desired state is eject intake + switch (currentState) { // check what our current state is + case INTAKE_GROUND: // what are we allowed to come from + return new EjectIntake(globalStateMachine, globalIntake, globalLED); + } + break; + case EJECT_SHOOTER: + switch (currentState) { + case HAS_GP: + case PREP_SHOOTER: + return new EjectShooter(globalStateMachine, globalStager, globalShooter, globalLED); + } + break; + case HAS_GP: + switch (currentState) { + case INTAKE_HOPPER: + case STOP_SHOOTER: + case NONE: + + return new HasGP(globalStager, globalShooter, globalStateMachine, globalLED); + } + break; + case INTAKE_GROUND: // if desired state is intake ground + switch (currentState) { // check what our current state is + case NONE: // what are we allowed to come from + return new IntakeGround(this, globalIntake, globalStager, globalLED); + } + break; + case INTAKE_HOPPER: + switch (currentState) { + case NONE: // is the current state NONE + case INTAKE_GROUND: // or is the current state INTAKE_GROUND + return new intakeHopper(globalStateMachine, globalHopper, globalStager, globalLED); + } + break; + case PREP_SHOOTER: + switch (currentState) { + case HAS_GP: + return new PrepShooter(globalStateMachine, globalShooter, globalLED); + } + break; + case SHOOT: + switch (currentState) { + case PREP_SHOOTER: + return new Shoot(globalStateMachine, globalStager, globalShooter, globalLED); + } + break; + case STOP_SHOOTER: + switch (currentState) { + case PREP_SHOOTER: + return new StopShooter(globalStateMachine, globalShooter); + + } + break; + case NONE: + switch (currentState) { + case EJECT_INTAKE: + case EJECT_SHOOTER: + case SHOOT: + case INTAKE_GROUND: + case INTAKE_HOPPER: + return new none(globalStateMachine, globalHopper, globalIntake, globalShooter, globalStager, globalLED); + } + break; + + // default: + // return new None(globalHopper, globalIntake, globalShooter, globalStager); + } + return Commands.print("Invalid state provided: Blame Eli, he provided it >:("); + + } + + @Override + public void periodic() { + + // This method will be called once per scheduler run + SmartDashboard.putString("Current State", currentState.toString()); + } +}