diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe26e84..2361c12 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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(); diff --git a/src/main/java/frc/robot/commands/states/presetLocations/BasePreps.java b/src/main/java/frc/robot/commands/states/presetLocations/BasePreps.java index b10aecb..81943a1 100644 --- a/src/main/java/frc/robot/commands/states/presetLocations/BasePreps.java +++ b/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); } diff --git a/src/main/java/frc/robot/commands/states/presetLocations/PreppingDSide.java b/src/main/java/frc/robot/commands/states/presetLocations/PreppingDSide.java index de3cb95..da810e8 100644 --- a/src/main/java/frc/robot/commands/states/presetLocations/PreppingDSide.java +++ b/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); diff --git a/src/main/java/frc/robot/commands/states/presetLocations/PreppingHub.java b/src/main/java/frc/robot/commands/states/presetLocations/PreppingHub.java index 4d5a636..1f49032 100644 --- a/src/main/java/frc/robot/commands/states/presetLocations/PreppingHub.java +++ b/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); diff --git a/src/main/java/frc/robot/commands/states/presetLocations/PreppingNeutralToAlliance.java b/src/main/java/frc/robot/commands/states/presetLocations/PreppingNeutralToAlliance.java index 849d9f3..149d5c8 100644 --- a/src/main/java/frc/robot/commands/states/presetLocations/PreppingNeutralToAlliance.java +++ b/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); diff --git a/src/main/java/frc/robot/commands/states/presetLocations/PreppingOSide.java b/src/main/java/frc/robot/commands/states/presetLocations/PreppingOSide.java index 17668d2..cec829e 100644 --- a/src/main/java/frc/robot/commands/states/presetLocations/PreppingOSide.java +++ b/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); diff --git a/src/main/java/frc/robot/commands/states/presetLocations/PreppingOpposingToAlliance.java b/src/main/java/frc/robot/commands/states/presetLocations/PreppingOpposingToAlliance.java index bd7013f..6c66167 100644 --- a/src/main/java/frc/robot/commands/states/presetLocations/PreppingOpposingToAlliance.java +++ b/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); diff --git a/src/main/java/frc/robot/commands/states/presetLocations/PreppingTower.java b/src/main/java/frc/robot/commands/states/presetLocations/PreppingTower.java index e2f45ba..300e363 100644 --- a/src/main/java/frc/robot/commands/states/presetLocations/PreppingTower.java +++ b/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); diff --git a/src/main/java/frc/robot/commands/states/presetLocations/PreppingTrench.java b/src/main/java/frc/robot/commands/states/presetLocations/PreppingTrench.java index b6d497c..d335b9e 100644 --- a/src/main/java/frc/robot/commands/states/presetLocations/PreppingTrench.java +++ b/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); diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretBasePrep.java b/src/main/java/frc/robot/commands/states/turretStates/TurretBasePrep.java new file mode 100644 index 0000000..21cfde8 --- /dev/null +++ b/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); + } +} diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretNone.java b/src/main/java/frc/robot/commands/states/turretStates/TurretNone.java new file mode 100644 index 0000000..d0572c7 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingDSide.java b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingDSide.java new file mode 100644 index 0000000..f02f13b --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingHub.java b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingHub.java new file mode 100644 index 0000000..b8bcd63 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingNeutralToAlliance.java b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingNeutralToAlliance.java new file mode 100644 index 0000000..5228ccd --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingOSide.java b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingOSide.java new file mode 100644 index 0000000..dca26df --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingOpposingToAlliance.java b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingOpposingToAlliance.java new file mode 100644 index 0000000..4553a76 --- /dev/null +++ b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingOpposingToAlliance.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 TurretPreppingOpposingToAlliance extends Command { + /** Creates a new TurretPreppingOpposingToAlliance. */ + public TurretPreppingOpposingToAlliance() { + // 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; + } +} diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingTower.java b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingTower.java new file mode 100644 index 0000000..db8333c --- /dev/null +++ b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingTower.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 TurretPreppingTower extends Command { + /** Creates a new TurretPreppingTower. */ + public TurretPreppingTower() { + // 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; + } +} diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingTrench.java b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingTrench.java new file mode 100644 index 0000000..2f19869 --- /dev/null +++ b/src/main/java/frc/robot/commands/states/turretStates/TurretPreppingTrench.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 TurretPreppingTrench extends Command { + /** Creates a new TurretPreppingTrench. */ + public TurretPreppingTrench() { + // 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; + } +} diff --git a/src/main/java/frc/robot/commands/states/turretStates/TurretShootingOnFly.java b/src/main/java/frc/robot/commands/states/turretStates/TurretShootingOnFly.java new file mode 100644 index 0000000..1e3b3f2 --- /dev/null +++ b/src/main/java/frc/robot/commands/states/turretStates/TurretShootingOnFly.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 TurretShootingOnFly extends Command { + /** Creates a new TurretShootingOnFly. */ + public TurretShootingOnFly() { + // 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/TurretStatemachine.java b/src/main/java/frc/robot/subsystems/TurretStatemachine.java new file mode 100644 index 0000000..f8ec9d9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TurretStatemachine.java @@ -0,0 +1,175 @@ +// 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.epilogue.Logged; +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.states.turretStates.TurretNone; +import frc.robot.commands.states.turretStates.TurretPreppingDSide; +import frc.robot.commands.states.turretStates.TurretPreppingHub; +import frc.robot.commands.states.turretStates.TurretPreppingNeutralToAlliance; +import frc.robot.commands.states.turretStates.TurretPreppingOSide; +import frc.robot.commands.states.turretStates.TurretPreppingOpposingToAlliance; +import frc.robot.commands.states.turretStates.TurretPreppingTower; +import frc.robot.commands.states.turretStates.TurretPreppingTrench; +import frc.robot.commands.states.turretStates.TurretShootingOnFly; + +@Logged +public class TurretStatemachine extends SubsystemBase { + /** Creates a new TurretStatemachine. */ + public static TurretState currentTurretState; + + public TurretStatemachine() { + currentTurretState = TurretState.NONE; + } + + public void setTurretState(TurretState turretState) { + currentTurretState = turretState; + } + + public TurretState getTurretState() { + return currentTurretState; + } + + public Command tryTurretState(TurretState desiredTurretState) { + switch (desiredTurretState) { + case NONE: + switch (currentTurretState) { + case NONE: + case SHOOTING_ON_FLY: + case PREPPING_TRENCH: + case PREPPING_OSIDE: + case PREPPING_DSIDE: + case PREPPING_TOWER: + case PREPPING_HUB: + case PREPPING_NEUTRAL_TO_ALLIANCE: + case PREPPING_OPPOSING_TO_ALLIANCE: + return new TurretNone(); + } + break; + case SHOOTING_ON_FLY: + switch (currentTurretState) { + case NONE: + case SHOOTING_ON_FLY: + return new TurretShootingOnFly(); + } + break; + case PREPPING_TRENCH: + switch (currentTurretState) { + case NONE: + case PREPPING_TRENCH: + case PREPPING_OSIDE: + case PREPPING_DSIDE: + case PREPPING_TOWER: + case PREPPING_HUB: + case PREPPING_NEUTRAL_TO_ALLIANCE: + case PREPPING_OPPOSING_TO_ALLIANCE: + return new TurretPreppingTrench(); + } + break; + case PREPPING_OSIDE: + switch (currentTurretState) { + case NONE: + case PREPPING_TRENCH: + case PREPPING_OSIDE: + case PREPPING_DSIDE: + case PREPPING_TOWER: + case PREPPING_HUB: + case PREPPING_NEUTRAL_TO_ALLIANCE: + case PREPPING_OPPOSING_TO_ALLIANCE: + return new TurretPreppingOSide(); + } + break; + case PREPPING_DSIDE: + switch (currentTurretState) { + case NONE: + case PREPPING_TRENCH: + case PREPPING_OSIDE: + case PREPPING_DSIDE: + case PREPPING_TOWER: + case PREPPING_HUB: + case PREPPING_NEUTRAL_TO_ALLIANCE: + case PREPPING_OPPOSING_TO_ALLIANCE: + return new TurretPreppingDSide(); + } + break; + case PREPPING_TOWER: + switch (currentTurretState) { + case NONE: + case PREPPING_TRENCH: + case PREPPING_OSIDE: + case PREPPING_DSIDE: + case PREPPING_TOWER: + case PREPPING_HUB: + case PREPPING_NEUTRAL_TO_ALLIANCE: + case PREPPING_OPPOSING_TO_ALLIANCE: + return new TurretPreppingTower(); + } + break; + case PREPPING_HUB: + switch (currentTurretState) { + case NONE: + case PREPPING_TRENCH: + case PREPPING_OSIDE: + case PREPPING_DSIDE: + case PREPPING_TOWER: + case PREPPING_HUB: + case PREPPING_NEUTRAL_TO_ALLIANCE: + case PREPPING_OPPOSING_TO_ALLIANCE: + return new TurretPreppingHub(); + } + break; + case PREPPING_NEUTRAL_TO_ALLIANCE: + switch (currentTurretState) { + case NONE: + case PREPPING_TRENCH: + case PREPPING_OSIDE: + case PREPPING_DSIDE: + case PREPPING_TOWER: + case PREPPING_HUB: + case PREPPING_NEUTRAL_TO_ALLIANCE: + case PREPPING_OPPOSING_TO_ALLIANCE: + return new TurretPreppingNeutralToAlliance(); + } + break; + case PREPPING_OPPOSING_TO_ALLIANCE: + switch (currentTurretState) { + case NONE: + case PREPPING_TRENCH: + case PREPPING_OSIDE: + case PREPPING_DSIDE: + case PREPPING_TOWER: + case PREPPING_HUB: + case PREPPING_NEUTRAL_TO_ALLIANCE: + case PREPPING_OPPOSING_TO_ALLIANCE: + return new TurretPreppingOpposingToAlliance(); + } + break; + } + return Commands + .print("ITS SO OVER D: Invalid State Provided, Blame Eli. Attempted to go to: " + desiredTurretState.toString() + + " while at " + currentTurretState.toString()); + + } + + public enum TurretState { + NONE, + SHOOTING_ON_FLY, + PREPPING_TRENCH, + PREPPING_OSIDE, + PREPPING_DSIDE, + PREPPING_TOWER, + PREPPING_HUB, + PREPPING_NEUTRAL_TO_ALLIANCE, + PREPPING_OPPOSING_TO_ALLIANCE, + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}