Remove top stager motor

simgui-ds.json

@@ -88,5 +88,11 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    }
   ]
 }

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
   }

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;
   }
 }

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");
   }
+
 }

src/main/java/frc/robot/commands/EjectGP.java → 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();
   }
 

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();
     }

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.

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);
   }