Turret StateMachine

src/main/java/frc/robot/RobotContainer.java

@@ -14,8 +14,8 @@
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.epilogue.NotLogged;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -28,11 +28,10 @@
 import frc.robot.DeviceIDs.controllerIDs;
 import frc.robot.commands.AddVisionMeasurement;
 import frc.robot.commands.ResetPose;
-import frc.robot.commands.states.ShootingOnPreset;
 import frc.robot.constants.ChoreoTraj;
-import frc.robot.constants.ConstRumble;
 import frc.robot.constants.ConstAuto;
 import frc.robot.constants.ConstField;
+import frc.robot.constants.ConstRumble;
 import frc.robot.constants.ConstSystem;
 import frc.robot.constants.ConstSystem.constControllers;
 import frc.robot.subsystems.DriverStateMachine;
@@ -44,6 +43,7 @@
 import frc.robot.subsystems.StateMachine;
 import frc.robot.subsystems.StateMachine.RobotState;
 import frc.robot.subsystems.Telemetry;
+import frc.robot.subsystems.TurretStatemachine;
 import frc.robot.subsystems.Vision;
 
 @Logged
@@ -67,6 +67,8 @@ public class RobotContainer {
   private final DriverStateMachine loggedDriverStateMachineInstance = driverStateMachineInstance;
   public static final StateMachine stateMachineInstance = new StateMachine();
   private final StateMachine loggedStateMachineInstance = stateMachineInstance;
+  public static final TurretStatemachine turretStateMachineInstance = new TurretStatemachine();
+  public final TurretStatemachine loggedTurretStatemachine = turretStateMachineInstance;
   public static final RobotPoses robotPose = new RobotPoses();
   private final RobotPoses loggedRobotPose = robotPose;
   public static final Vision visionInstance = new Vision();

src/main/java/frc/robot/commands/states/presetLocations/BasePreps.java

@@ -6,7 +6,6 @@
 
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
-
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotContainer;
 import frc.robot.constants.ConstFreeSpin;
@@ -15,15 +14,13 @@
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class BasePreps extends Command {
-  Angle commandTurretAngle;
   Angle commandHoodAngle;
   AngularVelocity commandFlywheelVelocity;
   RobotState commandState;
 
   /** Creates a new BasePreps. */
-  public BasePreps(Angle inputTurretAngle, Angle inputHoodAngle, AngularVelocity inputFlywheelVelocity,
+  public BasePreps(Angle inputHoodAngle, AngularVelocity inputFlywheelVelocity,
       RobotState inputState) {
-    commandTurretAngle = inputTurretAngle;
     commandHoodAngle = inputHoodAngle;
     commandFlywheelVelocity = inputFlywheelVelocity;
     commandState = inputState;
@@ -35,7 +32,6 @@ public BasePreps(Angle inputTurretAngle, Angle inputHoodAngle, AngularVelocity i
   @Override
   public void initialize() {
     RobotContainer.stateMachineInstance.setRobotState(commandState);
-    RobotContainer.positionalInstance.setTurretAngle(commandTurretAngle);
     RobotContainer.positionalInstance.setHoodPivotAngle(commandHoodAngle);
     RobotContainer.freeSpinInstance.setFlywheelVelocity(commandFlywheelVelocity);
   }
@@ -53,10 +49,8 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return RobotContainer.positionalInstance.getTurretAngle().isNear(commandTurretAngle,
-        ConstPositional.TURRET_TOLERANCE)
-        && RobotContainer.positionalInstance.getHoodPivotAngle().isNear(commandHoodAngle,
-            ConstPositional.HOOD_TOLERANCE)
+    return RobotContainer.positionalInstance.getHoodPivotAngle().isNear(commandHoodAngle,
+        ConstPositional.HOOD_TOLERANCE)
         && RobotContainer.freeSpinInstance.getFlywheelVelocity().isNear(commandFlywheelVelocity,
             ConstFreeSpin.FLYWHEEL_TOLERANCE);
   }

src/main/java/frc/robot/commands/states/presetLocations/PreppingDSide.java

@@ -12,7 +12,7 @@
 public class PreppingDSide extends BasePreps {
   /** Creates a new PreppingDSide. */
   public PreppingDSide() {
-    super(ConstPositional.PREP_D_SIDE_TURRET,
+    super(
         ConstPositional.PREP_D_SIDE_HOOD,
         ConstFreeSpin.PREP_D_SIDE_FLYWHEEL,
         StateMachine.RobotState.PREPPING_DSIDE);

src/main/java/frc/robot/commands/states/presetLocations/PreppingHub.java

@@ -12,7 +12,7 @@
 public class PreppingHub extends BasePreps {
   /** Creates a new PreppingHub. */
   public PreppingHub() {
-    super(ConstPositional.PREP_HUB_TURRET,
+    super(
         ConstPositional.PREP_HUB_HOOD,
         ConstFreeSpin.PREP_HUB_FLYWHEEL,
         StateMachine.RobotState.PREPPING_HUB);

src/main/java/frc/robot/commands/states/presetLocations/PreppingNeutralToAlliance.java

@@ -13,7 +13,7 @@ public class PreppingNeutralToAlliance extends BasePreps {
   /** Creates a new PreppingNeutralToAlliance. */
   public PreppingNeutralToAlliance() {
 
-    super(ConstPositional.PREP_NEUTRAL_TO_ALLIANCE_TURRET,
+    super(
         ConstPositional.PREP_NEUTRAL_TO_ALLIANCE_HOOD,
         ConstFreeSpin.PREP_NEUTRAL_TO_ALLIANCE_FLYWHEEL,
         StateMachine.RobotState.PREPPING_NEUTRAL_TO_ALLIANCE);

src/main/java/frc/robot/commands/states/presetLocations/PreppingOSide.java

@@ -12,7 +12,7 @@
 public class PreppingOSide extends BasePreps {
   /** Creates a new PreppingOSide. */
   public PreppingOSide() {
-    super(ConstPositional.PREP_O_SIDE_TURRET,
+    super(
         ConstPositional.PREP_O_SIDE_HOOD,
         ConstFreeSpin.PREP_O_SIDE_FLYWHEEL,
         StateMachine.RobotState.PREPPING_OSIDE);

src/main/java/frc/robot/commands/states/presetLocations/PreppingOpposingToAlliance.java

@@ -13,7 +13,7 @@ public class PreppingOpposingToAlliance extends BasePreps {
   /** Creates a new PreppingOpposingToAlliance. */
   public PreppingOpposingToAlliance() {
 
-    super(ConstPositional.PREP_OPPOSING_TO_ALLIANCE_TURRET,
+    super(
         ConstPositional.PREP_OPPOSING_TO_ALLIANCE_HOOD,
         ConstFreeSpin.PREP_OPPOSING_TO_ALLIANCE_FLYWHEEL,
         StateMachine.RobotState.PREPPING_OPPOSING_TO_ALLIANCE);

src/main/java/frc/robot/commands/states/presetLocations/PreppingTower.java

@@ -12,7 +12,7 @@
 public class PreppingTower extends BasePreps {
   /** Creates a new PreppingTower. */
   public PreppingTower() {
-    super(ConstPositional.PREP_TOWER_TURRET,
+    super(
         ConstPositional.PREP_TOWER_HOOD,
         ConstFreeSpin.PREP_TOWER_FLYWHEEL,
         StateMachine.RobotState.PREPPING_TOWER);

src/main/java/frc/robot/commands/states/presetLocations/PreppingTrench.java

@@ -12,7 +12,7 @@
 public class PreppingTrench extends BasePreps {
   /** Creates a new PreppingTrench. */
   public PreppingTrench() {
-    super(ConstPositional.PREP_TRENCH_TURRET,
+    super(
         ConstPositional.PREP_TRENCH_HOOD,
         ConstFreeSpin.PREP_TRENCH_FLYWHEEL,
         StateMachine.RobotState.PREPPING_TRENCH);

src/main/java/frc/robot/commands/states/turretStates/TurretBasePrep.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.turretStates;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.constants.ConstPositional;
+import frc.robot.subsystems.TurretStatemachine.TurretState;;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class TurretBasePrep extends Command {
+  Angle commandTurretAngle;
+
+  /** Creates a new TurretBasePrep. */
+  public TurretBasePrep(Angle inputTurretAngle, TurretState inputState) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    commandTurretAngle = inputTurretAngle;
+    addRequirements(RobotContainer.turretStateMachineInstance);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.positionalInstance.setTurretAngle(commandTurretAngle);
+  }
+
+  // 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 RobotContainer.positionalInstance.getTurretAngle().isNear(commandTurretAngle,
+        ConstPositional.TURRET_TOLERANCE);
+  }
+}

src/main/java/frc/robot/commands/states/turretStates/TurretNone.java

@@ -0,0 +1,38 @@
+// 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.turretStates;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class TurretNone extends Command {
+  /** Creates a new TurretNone. */
+  public TurretNone() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.turretStateMachineInstance);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // 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;
+  }
+}

src/main/java/frc/robot/commands/states/turretStates/TurretPreppingDSide.java

@@ -0,0 +1,33 @@
+// 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.turretStates;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class TurretPreppingDSide extends Command {
+  /** Creates a new TurretPreppingDSide. */
+  public TurretPreppingDSide() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // 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;
+  }
+}

src/main/java/frc/robot/commands/states/turretStates/TurretPreppingHub.java

@@ -0,0 +1,33 @@
+// 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.turretStates;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class TurretPreppingHub extends Command {
+  /** Creates a new TurretPreppingHub. */
+  public TurretPreppingHub() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // 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;
+  }
+}

src/main/java/frc/robot/commands/states/turretStates/TurretPreppingNeutralToAlliance.java

@@ -0,0 +1,33 @@
+// 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.turretStates;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class TurretPreppingNeutralToAlliance extends Command {
+  /** Creates a new TurretPreppingNeutralToAlliance. */
+  public TurretPreppingNeutralToAlliance() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // 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;
+  }
+}

src/main/java/frc/robot/commands/states/turretStates/TurretPreppingOSide.java

@@ -0,0 +1,33 @@
+// 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.turretStates;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class TurretPreppingOSide extends Command {
+  /** Creates a new TurretPreppingOSide. */
+  public TurretPreppingOSide() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // 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;
+  }
+}