diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b70a9b6..bb70c49 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -100,7 +100,7 @@ public class RobotContainer { () -> stateMachineInstance.tryState(RobotState.PREP_CORNER)); Command TRY_RETRACTING = Commands.deferredProxy( - () -> stateMachineInstance.tryState(RobotState.PREP_CORNER)); + () -> stateMachineInstance.tryState(RobotState.RETRACT_INTAKE)); Command MANUAL = new DeferredCommand( driverStateMachineInstance.tryState( diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index ce62b1f..a91d2cf 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -28,6 +28,7 @@ public void initialize() { RobotContainer.stateMachineInstance.setRobotState((StateMachine.RobotState.SHOOTING)); RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.TRANSFER_ROLLERS_SPEED); RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.SERIALIZER_SHOOTING_SPEED); + RobotContainer.rotorsInstance.setIntakeRollersPercentOutput(ConstRotors.INTAKE_ROLLERS_SPEED); intakeAgitationTimer.restart(); } @@ -35,10 +36,10 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (!RobotContainer.motionInstance.getIntakePivotAngle().isNear(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE, + if (!RobotContainer.motionInstance.getIntakePivotAngle().isNear(ConstMotion.AGITATE_PIVOT_ANGLE, ConstMotion.INTAKE_PIVOT_TOLERANCE) && intakeAgitationTimer.hasElapsed(ConstMotion.INTAKE_PIVOT_AGITATION_TIME)) { - RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE); + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.AGITATE_PIVOT_ANGLE); intakeAgitationTimer.restart(); } else if (intakeAgitationTimer.hasElapsed((ConstMotion.INTAKE_PIVOT_AGITATION_TIME))) { diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 118cfc7..8b63abb 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -19,16 +19,17 @@ /** Add your docs here. */ public class ConstMotion { - public static final Angle INTAKE_PIVOT_TOLERANCE = Degrees.of(1.0); + public static final Angle INTAKE_PIVOT_TOLERANCE = Degrees.of(5.0); public static final Time INTAKE_PIVOT_AGITATION_TIME = Units.Seconds.of(1.0 / 3.0); public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); 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 AGITATE_PIVOT_ANGLE = Degrees.of(63); 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_NONE_ANGLE = Degrees.of(3.2); public static final Angle HOOD_TRENCH_ANGLE = Units.Degrees.of(13); public static final Angle HOOD_CORNER_ANGLE = Units.Degrees.of(23.5); public static final Angle HOOD_TOWER_ANGLE = Units.Degrees.of(14); diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index edf68b8..2256e9d 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -33,6 +33,7 @@ public class Motion extends SubsystemBase { public Motion() { intakePivot.getConfigurator().apply(ConstMotion.INTAKE_PIVOT_CONFIGURATION); hoodPivot.getConfigurator().apply(ConstMotion.HOOD_PIVOT_CONFIGURATION); + hoodPivot.setPosition(ConstMotion.HOOD_NONE_ANGLE); } /**