From 9dd321e9996f345055de3bc231813bd326815803 Mon Sep 17 00:00:00 2001 From: Aidan <> Date: Sat, 10 Feb 2024 13:08:34 -0800 Subject: [PATCH] create new intake subsystem --- src/main/java/frc/robot/Robot.java | 3 +- .../java/frc/robot/config/CompConfig.java | 9 +- .../java/frc/robot/config/PracticeConfig.java | 10 +- .../java/frc/robot/config/RobotConfig.java | 6 +- .../java/frc/robot/intake/IntakeState.java | 19 +- .../frc/robot/intake/IntakeSubsystem.java | 240 ++++-------------- .../frc/robot/robot_manager/RobotManager.java | 64 ++--- 7 files changed, 86 insertions(+), 265 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fcf0ccaf..eb652c0c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -64,8 +64,7 @@ public class Robot extends LoggedRobot { RobotConfig.get().climber().followerMotorID(), RobotConfig.get().canivoreName())); private final IntakeSubsystem intake = new IntakeSubsystem( - new TalonFX(RobotConfig.get().intake().topMotorID(), RobotConfig.get().canivoreName()), - new TalonFX(RobotConfig.get().intake().bottomMotorID(), RobotConfig.get().canivoreName()), + new TalonFX(RobotConfig.get().intake().motorID(), RobotConfig.get().canivoreName()), new DigitalInput(RobotConfig.get().intake().sensorID())); private final SwerveSubsystem swerve = new SwerveSubsystem(driverController); private final ImuSubsystem imu = new ImuSubsystem(swerve); diff --git a/src/main/java/frc/robot/config/CompConfig.java b/src/main/java/frc/robot/config/CompConfig.java index ec93755b..00a742d9 100644 --- a/src/main/java/frc/robot/config/CompConfig.java +++ b/src/main/java/frc/robot/config/CompConfig.java @@ -128,7 +128,6 @@ class CompConfig { }), new IntakeConfig( 16, - 22, 0, // Top Motor new TalonFXConfiguration() @@ -136,13 +135,7 @@ class CompConfig { .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20)) .withMotorOutput( new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)), - // Bottom Motor - new TalonFXConfiguration() - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1)) - .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20)) - .withMotorOutput( - new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))), + .withInverted(InvertedValue.CounterClockwise_Positive))), new SwerveConfig( new CurrentLimitsConfigs().withStatorCurrentLimit(60), new CurrentLimitsConfigs().withStatorCurrentLimit(60), diff --git a/src/main/java/frc/robot/config/PracticeConfig.java b/src/main/java/frc/robot/config/PracticeConfig.java index f4f05990..9c4d8d5c 100644 --- a/src/main/java/frc/robot/config/PracticeConfig.java +++ b/src/main/java/frc/robot/config/PracticeConfig.java @@ -117,21 +117,13 @@ class PracticeConfig { }), new IntakeConfig( 16, - 22, 0, // Top Motor new TalonFXConfiguration() .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1)) .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20)) .withMotorOutput( - new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)), - // Bottom Motor - new TalonFXConfiguration() - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1)) - .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20)) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive))), + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))), new SwerveConfig( new CurrentLimitsConfigs().withStatorCurrentLimit(80), new CurrentLimitsConfigs().withStatorCurrentLimit(80), diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java index 93b56fa1..eca3de3e 100644 --- a/src/main/java/frc/robot/config/RobotConfig.java +++ b/src/main/java/frc/robot/config/RobotConfig.java @@ -33,11 +33,9 @@ public record ClimberConfig( int mainMotorID, int followerMotorID, TalonFXConfiguration motorConfig) {} public record IntakeConfig( - int topMotorID, - int bottomMotorID, + int motorID, int sensorID, - TalonFXConfiguration topMotorConfig, - TalonFXConfiguration bottomMotorConfig) {} + TalonFXConfiguration motorConfig) {} public record ShoulderConfig( int rightMotorID, diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java index da65a0c0..16885bfb 100644 --- a/src/main/java/frc/robot/intake/IntakeState.java +++ b/src/main/java/frc/robot/intake/IntakeState.java @@ -5,18 +5,9 @@ package frc.robot.intake; public enum IntakeState { - IDLE_NO_GP, - IDLE_WITH_GP, - - // Intaking, waitinf or sensor to trigger for the first time - INTAKING_GP_WAITING_FOR_SENSOR_ON, - // Intaking, wait for the note to pass all the way through, and stop triggering the sensor - INTAKING_GP_WAITING_FOR_SENSOR_OFF, - // Intaking in reverse, waiting for note to hit sensor again, then we transition to IDLE_WITH_GP - INTAKING_GP_FINAL_WAITING_FOR_SENSOR_ON, - - OUTTAKING, - SHOOTING, - TRAP_OUTTAKE, - CLIMBING; + IDLE, + INTAKING, + PASS_TO_QUEUER, + PASS_TO_CONVEYOR, + OUTTAKING; } diff --git a/src/main/java/frc/robot/intake/IntakeSubsystem.java b/src/main/java/frc/robot/intake/IntakeSubsystem.java index bdd69cb8..cdc3643b 100644 --- a/src/main/java/frc/robot/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/intake/IntakeSubsystem.java @@ -1,228 +1,76 @@ -// 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.intake; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; + import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.config.RobotConfig; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; -import org.littletonrobotics.junction.Logger; public class IntakeSubsystem extends LifecycleSubsystem { - private final TalonFX topMotor; - private final TalonFX bottomMotor; + private final TalonFX motor; private final DigitalInput sensor; - private IntakeState state = IntakeState.IDLE_NO_GP; - private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(false); - - private boolean idleFlag = false; - private boolean idleWithGPFlag = false; - private boolean idleNoGPFlag = false; - private boolean intakingFlag = false; - private boolean outtakingFlag = false; - private boolean shootingFlag = false; - private boolean trapOuttakeFlag = false; - private boolean climbingFlag = false; - - public IntakeSubsystem(TalonFX topMotor, TalonFX bottomMotor, DigitalInput sensor) { + private VoltageOut voltageRequest = new VoltageOut(0.0); + private IntakeState goalState = IntakeState.IDLE; + private boolean hasNote = false; + + public IntakeSubsystem(TalonFX motor, DigitalInput sensor) { super(SubsystemPriority.INTAKE); - this.topMotor = topMotor; + motor.getConfigurator().apply(RobotConfig.get().intake().motorConfig()); + + this.motor = motor; this.sensor = sensor; - this.bottomMotor = bottomMotor; - topMotor.getConfigurator().apply(RobotConfig.get().intake().topMotorConfig()); - bottomMotor.getConfigurator().apply(RobotConfig.get().intake().bottomMotorConfig()); } @Override public void enabledPeriodic() { - Logger.recordOutput("Intake/State", state); - - Logger.recordOutput("Intake/IdleFlag", idleFlag); - Logger.recordOutput("Intake/IdleWithGPFlag", idleWithGPFlag); - Logger.recordOutput("Intake/IdleNoGPFlag", idleNoGPFlag); - Logger.recordOutput("Intake/IntakingFlag", intakingFlag); - Logger.recordOutput("Intake/OuttakingFlag", outtakingFlag); - Logger.recordOutput("Intake/ShootingFlag", shootingFlag); - Logger.recordOutput("Intake/TrapOuttakeFlag", trapOuttakeFlag); - Logger.recordOutput("Intake/ClimbingFlag", climbingFlag); - - Logger.recordOutput("Intake/SensorHasNote", sensorHasNote()); - - Logger.recordOutput("Intake/TopMotor/Current", topMotor.getStatorCurrent().getValueAsDouble()); - Logger.recordOutput("Intake/TopMotor/Velocity", topMotor.getVelocity().getValueAsDouble()); - Logger.recordOutput( - "Intake/TopMotor/OutputVoltage", topMotor.getMotorVoltage().getValueAsDouble()); - Logger.recordOutput("Intake/TopMotor/Temperature", topMotor.getDeviceTemp().getValue()); - - Logger.recordOutput( - "Intake/BottomMotor/Current", bottomMotor.getStatorCurrent().getValueAsDouble()); - Logger.recordOutput( - "Intake/BottomMotor/Velocity", bottomMotor.getVelocity().getValueAsDouble()); - Logger.recordOutput( - "Intake/BottomMotor/OutputVoltage", bottomMotor.getMotorVoltage().getValueAsDouble()); - Logger.recordOutput("Intake/BottomMotor/Temperature", bottomMotor.getDeviceTemp().getValue()); - - // State transitions from requests - if (idleFlag) { - switch (state) { - case IDLE_NO_GP: - case INTAKING_GP_WAITING_FOR_SENSOR_ON: - case OUTTAKING: - case SHOOTING: - case TRAP_OUTTAKE: - state = IntakeState.IDLE_NO_GP; - break; - - default: - state = IntakeState.IDLE_WITH_GP; - break; - } - } - if (intakingFlag) { - // Don't interrupt the intake sequence if we are already in it - if (state != IntakeState.INTAKING_GP_WAITING_FOR_SENSOR_OFF - && state != IntakeState.INTAKING_GP_FINAL_WAITING_FOR_SENSOR_ON) { - - state = IntakeState.INTAKING_GP_WAITING_FOR_SENSOR_ON; - } - } - if (outtakingFlag) { - state = IntakeState.OUTTAKING; - } - if (trapOuttakeFlag) { - state = IntakeState.TRAP_OUTTAKE; - } - if (climbingFlag) { - state = IntakeState.CLIMBING; - } - if (shootingFlag) { - state = IntakeState.SHOOTING; - } - if (idleWithGPFlag) { - state = IntakeState.IDLE_WITH_GP; - } - if (idleNoGPFlag) { - state = IntakeState.IDLE_NO_GP; - } - - // Automatic state transitions - switch (state) { - case IDLE_WITH_GP: - case CLIMBING: - case IDLE_NO_GP: - // Do nothing - break; - case INTAKING_GP_WAITING_FOR_SENSOR_ON: - if (sensorHasNote()) { - state = IntakeState.INTAKING_GP_WAITING_FOR_SENSOR_OFF; - } - break; - case INTAKING_GP_WAITING_FOR_SENSOR_OFF: - if (!sensorHasNote()) { - state = IntakeState.INTAKING_GP_FINAL_WAITING_FOR_SENSOR_ON; - } - break; - case INTAKING_GP_FINAL_WAITING_FOR_SENSOR_ON: - if (sensorHasNote()) { - state = IntakeState.IDLE_WITH_GP; - } - break; - case OUTTAKING: - case SHOOTING: - case TRAP_OUTTAKE: - if (!sensorHasNote()) { - state = IntakeState.IDLE_NO_GP; - } - break; + if (goalState == IntakeState.IDLE) { + motor.disable(); + } else if ( goalState == IntakeState.OUTTAKING) { + voltageRequest.withOutput(-4.0); + } else if (goalState == IntakeState.PASS_TO_CONVEYOR) { + voltageRequest.withOutput(2.0); + } else if (goalState == IntakeState.PASS_TO_QUEUER) { + voltageRequest.withOutput(2.0); + } else if (goalState == IntakeState.INTAKING) { + voltageRequest.withOutput(4.0); + } else { + motor.disable(); } - // State actions - - // negative voltage intake - // positive voltage outtake - switch (state) { - case IDLE_WITH_GP: - case IDLE_NO_GP: - topMotor.disable(); - bottomMotor.disable(); - break; - case INTAKING_GP_WAITING_FOR_SENSOR_ON: - topMotor.setControl(voltageRequest.withOutput(-3.5)); - bottomMotor.setControl(voltageRequest.withOutput(-3.5)); - break; - case INTAKING_GP_WAITING_FOR_SENSOR_OFF: - topMotor.setControl(voltageRequest.withOutput(-1.0)); - bottomMotor.setControl(voltageRequest.withOutput(-1.0)); - break; - case INTAKING_GP_FINAL_WAITING_FOR_SENSOR_ON: - topMotor.setControl(voltageRequest.withOutput(1.0)); - bottomMotor.setControl(voltageRequest.withOutput(1.0)); - break; - case OUTTAKING: - topMotor.setControl(voltageRequest.withOutput(6.0)); - bottomMotor.setControl(voltageRequest.withOutput(6.0)); - break; - case SHOOTING: - topMotor.setControl(voltageRequest.withOutput(-10.0)); - bottomMotor.setControl(voltageRequest.withOutput(-10.0)); - break; - case TRAP_OUTTAKE: - topMotor.setControl(voltageRequest.withOutput(6.0)); - bottomMotor.setControl(voltageRequest.withOutput(-6.0)); - break; - case CLIMBING: - topMotor.disable(); - bottomMotor.setControl(voltageRequest.withOutput(-10.0)); - break; - } - - // Reset all flags - idleFlag = false; - idleNoGPFlag = false; - idleWithGPFlag = false; - intakingFlag = false; - outtakingFlag = false; - shootingFlag = false; - trapOuttakeFlag = false; - climbingFlag = false; - } - - public void idleRequest() { - idleFlag = true; + setHasNote(sensorHasNote()); } - public void intakingRequest() { - intakingFlag = true; + public void setState(IntakeState state) { + goalState = state; } - public void outtakingRequest() { - outtakingFlag = true; - } - - public void shootingRequest() { - shootingFlag = true; - } - - public void trapOuttakeRequest() { - trapOuttakeFlag = true; - } + public boolean atGoal(IntakeState state) { + if (goalState != state) { + return false; + } - public void climbingRequest() { - climbingFlag = true; + if (goalState == IntakeState.IDLE) { + return true; + } + if (hasNote) { + return true; + } + if (Math.abs(motor.getMotorVoltage().getValue()) > 0.0 && !hasNote) { + return true; + } + return false; } - public void idleWithGPRequest() { - idleWithGPFlag = true; + public boolean getHasNote() { + return hasNote; } - public void idleNoGPRequest() { - idleNoGPFlag = true; + public void setHasNote(boolean bool) { + hasNote = bool; } private boolean sensorHasNote() { @@ -230,6 +78,6 @@ private boolean sensorHasNote() { } public IntakeState getState() { - return state; + return goalState; } } diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 5d1523b9..5b491c2a 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -156,7 +156,7 @@ public void robotPeriodic() { state = RobotState.HOMING; } if (groundIntakeFlag) { - intake.idleNoGPRequest(); + intake.setState(IntakeState.IDLE); if (HOMED_STATES.contains(state)) { state = RobotState.GROUND_INTAKING; } @@ -228,7 +228,7 @@ public void robotPeriodic() { } } if (sourceIntakeFlag) { - intake.idleNoGPRequest(); + intake.setState(IntakeState.IDLE); if (HOMED_STATES.contains(state)) { state = RobotState.SOURCE_INTAKING; } @@ -300,22 +300,22 @@ public void robotPeriodic() { } break; case GROUND_INTAKING: - if (intake.getState() == IntakeState.INTAKING_GP_WAITING_FOR_SENSOR_OFF) { + if (intake.atGoal(IntakeState.INTAKING)) { state = RobotState.GROUND_INTAKING_SETTLING; } break; case GROUND_INTAKING_SETTLING: - if (intake.getState() == IntakeState.IDLE_WITH_GP) { + if (intake.atGoal(IntakeState.IDLE)) { state = RobotState.IDLE_DOWN_WITH_GP; } break; case SOURCE_INTAKING: - if (intake.getState() == IntakeState.INTAKING_GP_WAITING_FOR_SENSOR_OFF) { + if (intake.atGoal(IntakeState.INTAKING)) { state = RobotState.SOURCE_INTAKING_SETTLING; } break; case SOURCE_INTAKING_SETTLING: - if (intake.getState() == IntakeState.IDLE_WITH_GP) { + if (intake.atGoal(IntakeState.IDLE)) { state = RobotState.IDLE_UP_WITH_GP; } break; @@ -357,12 +357,12 @@ public void robotPeriodic() { case AMP_SHOOT: case SUBWOOFER_SHOOT: case SPEAKER_SHOOT: - if (intake.getState() == IntakeState.IDLE_NO_GP) { + if (intake.atGoal(IntakeState.IDLE)) { state = RobotState.IDLE_DOWN_NO_GP; } break; case TRAP_SHOOT: - if (intake.getState() == IntakeState.IDLE_NO_GP) { + if (intake.atGoal(IntakeState.IDLE)) { state = RobotState.CLIMBER_HANGING; } break; @@ -385,72 +385,72 @@ public void robotPeriodic() { switch (state) { case UNHOMED: shoulder.startPreMatchHoming(); - intake.idleRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); break; case HOMING: shoulder.startMidMatchHoming(); - intake.idleRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); break; case IDLE_UP_NO_GP: shoulder.setAngle(ShoulderPositions.STOWED_UP); - intake.idleNoGPRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case IDLE_UP_WITH_GP: shoulder.setAngle(ShoulderPositions.STOWED_UP); - intake.idleWithGPRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case IDLE_DOWN_NO_GP: shoulder.setAngle(ShoulderPositions.STOWED_DOWN); - intake.idleNoGPRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case IDLE_DOWN_WITH_GP: shoulder.setAngle(ShoulderPositions.STOWED_DOWN); - intake.idleWithGPRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case GROUND_INTAKING: shoulder.setAngle(ShoulderPositions.GROUND_INTAKING); - intake.intakingRequest(); + intake.setState(IntakeState.INTAKING); shooter.setMode(ShooterMode.INTAKE); climber.setGoal(ClimberMode.IDLE); break; case GROUND_INTAKING_SETTLING: shoulder.setAngle(ShoulderPositions.STOWED_DOWN); - intake.intakingRequest(); + intake.setState(IntakeState.INTAKING); shooter.setMode(ShooterMode.INTAKE); climber.setGoal(ClimberMode.IDLE); break; case SOURCE_INTAKING: shoulder.setAngle(ShoulderPositions.SOURCE_INTAKING); - intake.intakingRequest(); + intake.setState(IntakeState.INTAKING); shooter.setMode(ShooterMode.INTAKE); climber.setGoal(ClimberMode.IDLE); break; case SOURCE_INTAKING_SETTLING: shoulder.setAngle(ShoulderPositions.STOWED_UP); - intake.intakingRequest(); + intake.setState(IntakeState.INTAKING); shooter.setMode(ShooterMode.INTAKE); climber.setGoal(ClimberMode.IDLE); break; case OUTTAKING: shoulder.setAngle(ShoulderPositions.OUTTAKING); - intake.outtakingRequest(); + intake.setState(IntakeState.OUTTAKING); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case WAITING_FLOOR_SHOT: case PREPARE_FLOOR_SHOT: shoulder.setAngle(shoulderAngleForFloorSpot); - intake.idleRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.FLOOR_SHOT); climber.setGoal(ClimberMode.IDLE); snaps.setAngle(robotAngleToFloorSpot); @@ -459,7 +459,7 @@ public void robotPeriodic() { break; case FLOOR_SHOOT: shoulder.setAngle(shoulderAngleForFloorSpot); - intake.shootingRequest(); + intake.setState(IntakeState.PASS_TO_QUEUER); shooter.setMode(ShooterMode.FLOOR_SHOT); climber.setGoal(ClimberMode.IDLE); snaps.setAngle(robotAngleToFloorSpot); @@ -469,20 +469,20 @@ public void robotPeriodic() { case WAITING_SUBWOOFER_SHOT: case PREPARE_SUBWOOFER_SHOT: shoulder.setAngle(ShoulderPositions.SUBWOOFER_SHOT); - intake.idleRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.SUBWOOFER_SHOT); climber.setGoal(ClimberMode.IDLE); break; case SUBWOOFER_SHOOT: shoulder.setAngle(ShoulderPositions.SUBWOOFER_SHOT); - intake.shootingRequest(); + intake.setState(IntakeState.PASS_TO_QUEUER); shooter.setMode(ShooterMode.SUBWOOFER_SHOT); climber.setGoal(ClimberMode.IDLE); break; case WAITING_SPEAKER_SHOT: case PREPARE_SPEAKER_SHOT: shoulder.setAngle(shoulderAngleForSpeaker); - intake.idleRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.SPEAKER_SHOT); climber.setGoal(ClimberMode.IDLE); snaps.setAngle(robotAngleToSpeaker); @@ -491,7 +491,7 @@ public void robotPeriodic() { break; case SPEAKER_SHOOT: shoulder.setAngle(shoulderAngleForSpeaker); - intake.shootingRequest(); + intake.setState(IntakeState.PASS_TO_QUEUER); shooter.setMode(ShooterMode.SPEAKER_SHOT); climber.setGoal(ClimberMode.IDLE); snaps.setAngle(robotAngleToSpeaker); @@ -501,44 +501,44 @@ public void robotPeriodic() { case WAITING_AMP_SHOT: case PREPARE_AMP_SHOT: shoulder.setAngle(ShoulderPositions.AMP_SHOT); - intake.idleRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.AMP_SHOT); climber.setGoal(ClimberMode.IDLE); break; case AMP_SHOOT: shoulder.setAngle(ShoulderPositions.AMP_SHOT); - intake.shootingRequest(); + intake.setState(IntakeState.PASS_TO_CONVEYOR); shooter.setMode(ShooterMode.AMP_SHOT); climber.setGoal(ClimberMode.IDLE); break; case WAITING_CLIMBER_RAISED: shoulder.setAngle(ShoulderPositions.WAITING_CLIMBER_RAISED); - intake.idleRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.RAISED); break; case PREPARE_CLIMBER_RAISED: case CLIMBER_RAISED: shoulder.setAngle(ShoulderPositions.TRAP_SHOT); - intake.idleRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.RAISED); break; case PREPARE_CLIMBER_HANGING: shoulder.setAngle(ShoulderPositions.TRAP_SHOT); - intake.climbingRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.HANGING); break; case CLIMBER_HANGING: shoulder.setAngle(ShoulderPositions.TRAP_SHOT); - intake.climbingRequest(); + intake.setState(IntakeState.IDLE); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.HANGING); break; case TRAP_SHOOT: shoulder.setAngle(ShoulderPositions.TRAP_SHOT); - intake.trapOuttakeRequest(); + intake.setState(IntakeState.PASS_TO_CONVEYOR); shooter.setMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.HANGING); break;