diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index 8c06822..d4c0916 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -8,32 +8,27 @@ import frc.robot.RobotContainer; import frc.robot.constants.ConstMotion; import frc.robot.constants.ConstRotors; +import frc.robot.subsystems.StateMachine; public class Shooting extends Command { /** Creates a new Shooting. */ public Shooting() { // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.stateMachineInstance); } // Called when the command is initially scheduled. @Override public void initialize() { - RobotContainer.motionInstance.setHoodPivotAngle(ConstMotion.HOOD_PIVOT_ANGLE); + RobotContainer.stateMachineInstance.setRobotState((StateMachine.RobotState.SHOOTING)); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - - RobotContainer.rotorsInstance.setFlyWheelSpeeds(ConstRotors.FLYWHEEL_SHOOTING_SPEED); - if (RobotContainer.rotorsInstance.isFlyWheelAtSpeed(ConstRotors.FLYWHEEL_SHOOTING_SPEED)) { - RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.INTAKE_TRANSFER_SPEED); - RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.SERIALIZER_SHOOTING_SPEED); - - } else { - RobotContainer.rotorsInstance.setFlywheelPercentOutput(0.0); - } + RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.TRANSFER_ROLLERS_SPEED); + RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.SERIALIZER_SHOOTING_SPEED); } @@ -43,6 +38,7 @@ public void end(boolean interrupted) { RobotContainer.rotorsInstance.setFlywheelPercentOutput(0.0); RobotContainer.rotorsInstance.setTransferRollersPercentOutput(0.0); RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(0.0); + RobotContainer.motionInstance.setHoodPivotAngle(ConstMotion.HOOD_PIVOT_ANGLE_RETRACT); } @@ -51,4 +47,4 @@ public void end(boolean interrupted) { public boolean isFinished() { return false; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 3ef98db..d367b3f 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -23,6 +23,7 @@ public class ConstMotion { public static final Angle DEPLOY_INTAKE_PIVOT_ANGLE = Degrees.of(125); public static final Angle RETRACT_INTAKE_PIVOT_ANGLE = Degrees.of(0); public static final Angle HOOD_PIVOT_ANGLE = Degrees.of(14); + public static final Angle HOOD_PIVOT_ANGLE_RETRACT = Degrees.of(0); public static final double STOP = 0; public static final Angle HOOD_NONE_ANGLE = Degrees.of(0); public static final Angle HOOD_TRENCH_ANGLE = Units.Degrees.of(13); diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index efe9a82..3364df3 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -33,9 +33,10 @@ public class ConstRotors { public static final double INTAKE_ROLLERS_SPEED = 0.5; public static final AngularVelocity FLYWHEEL_SHOOTING_SPEED = Units.RPM.of(1000); // public static final AngularVelocity INTAKE_ROLLER_SPEED = Units.RPM.of(2000); - public static final AngularVelocity INTAKE_TRANSFER_SPEED = Units.RPM.of(3000); public static final double SERIALIZER_SHOOTING_SPEED = 0.5; public static final double STOP = 0; + public static final AngularVelocity TRANSFER_ROLLERS_SPEED = Units.RPM.of(3400); + public static final AngularVelocity FLYWHEEL_TOLERANCE = Units.RPM.of(100); public static final double REVERSE_SHOOTING_SPEED = -1; public static final double REVERSE_SERIALIZER_ROLLERS_SPEED = -0.5; public static final double REVERSE_INTAKE_ROLLERS_SPEED = -0.5; diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index 51b57be..fce3fba 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -127,4 +127,5 @@ public boolean isFlyWheelAtSpeed(AngularVelocity tolerance) { public void periodic() { // This method will be called once per scheduler run } + }