From ec7d0c0cb2fc379e3ba2a8b385547a704a22e54b Mon Sep 17 00:00:00 2001 From: WindingMotor Date: Wed, 5 Feb 2025 14:41:42 -0500 Subject: [PATCH] Better dynamic state pt2 - remove seperate wheel intake settings --- src/main/java/frc/robot/RobotContainer.java | 9 ++--- .../robot/commands/coral/CMD_IntakeCoral.java | 4 +- .../commands/generic/CMD_IntakeWheel.java | 38 ------------------- .../java/frc/robot/intake/SUB_Intake.java | 1 - .../superstructure/SUB_Superstructure.java | 5 ++- 5 files changed, 8 insertions(+), 49 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/generic/CMD_IntakeWheel.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a973b90..d20a782 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,7 +18,6 @@ import frc.robot.commands.coral.CMD_IntakeCoral; import frc.robot.commands.drive.CMD_Drive; import frc.robot.commands.generic.CMD_Elevator; -import frc.robot.commands.generic.CMD_IntakeWheel; import frc.robot.commands.generic.CMD_Superstructure; import frc.robot.constants.FieldConstants; import frc.robot.constants.InputConstants; @@ -101,10 +100,10 @@ private void configurePathPlannerCommands() { private void configureButtonBindings() { // Extake - operatorController.x().onTrue(new CMD_IntakeWheel(intake, -0.6)); + operatorController.x().onTrue(new CMD_Superstructure(superstructure, superstructure.getCurrentStateWithNewWheelSpeed(-0.6))); // Intake alage - operatorController.y().onTrue(new CMD_IntakeWheel(intake, 0.95)); + operatorController.x().onTrue(new CMD_Superstructure(superstructure, superstructure.getCurrentStateWithNewWheelSpeed(0.95))); // Coral Controls operatorController @@ -128,12 +127,12 @@ private void configureButtonBindings() { .onTrue( new SequentialCommandGroup( new CMD_Superstructure(superstructure, SuperstructureState.IDLE), - new CMD_IntakeWheel(intake, SuperstructureState.IDLE.getSpeed()))); + new CMD_Superstructure(superstructure, superstructure.getCurrentStateWithNewWheelSpeed(SuperstructureState.CORAL_STATION.getSpeed())))); // B - Idle // Climbing operatorController .rightBumper() - .onTrue(new CMD_Elevator(elevator, led, SuperstructureState.)); + .onTrue(new CMD_Elevator(elevator, led, SuperstructureState.CLIMB)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/coral/CMD_IntakeCoral.java b/src/main/java/frc/robot/commands/coral/CMD_IntakeCoral.java index 8630799..cbc11b4 100644 --- a/src/main/java/frc/robot/commands/coral/CMD_IntakeCoral.java +++ b/src/main/java/frc/robot/commands/coral/CMD_IntakeCoral.java @@ -8,7 +8,6 @@ package frc.robot.commands.coral; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.commands.generic.CMD_IntakeWheel; import frc.robot.commands.generic.CMD_Superstructure; import frc.robot.superstructure.SUB_Superstructure; import frc.robot.superstructure.SuperstructureState; @@ -18,8 +17,7 @@ public CMD_IntakeCoral(SUB_Superstructure superstructure) { addCommands( // Move to Coral Station state - new CMD_IntakeWheel( - superstructure.intake, SuperstructureState.CORAL_STATION.getSpeed()), + new CMD_Superstructure(superstructure, superstructure.getCurrentStateWithNewWheelSpeed(SuperstructureState.CORAL_STATION.getSpeed())), new CMD_Superstructure(superstructure, SuperstructureState.CORAL_STATION) // TODO: Auto idle intake after coral is sensed by the sensor diff --git a/src/main/java/frc/robot/commands/generic/CMD_IntakeWheel.java b/src/main/java/frc/robot/commands/generic/CMD_IntakeWheel.java deleted file mode 100644 index 84330de..0000000 --- a/src/main/java/frc/robot/commands/generic/CMD_IntakeWheel.java +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs -// https://www.team2106.org - -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.commands.generic; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.intake.SUB_Intake; - -public class CMD_IntakeWheel extends Command { - private final SUB_Intake intake; - private final double newIntakeWheelSpeed; - - public CMD_IntakeWheel(SUB_Intake intake, double newIntakeWheelSpeed) { - this.intake = intake; - this.newIntakeWheelSpeed = newIntakeWheelSpeed; - addRequirements(intake); - } - - @Override - public void initialize() { - intake.updateLocalWheelSpeed(newIntakeWheelSpeed); - } - - @Override - public void execute() {} - - @Override - public boolean isFinished() { - return true; - } - - @Override - public void end(boolean interrupted) {} -} diff --git a/src/main/java/frc/robot/intake/SUB_Intake.java b/src/main/java/frc/robot/intake/SUB_Intake.java index cbfa254..b3c2ad4 100644 --- a/src/main/java/frc/robot/intake/SUB_Intake.java +++ b/src/main/java/frc/robot/intake/SUB_Intake.java @@ -17,7 +17,6 @@ public SUB_Intake(IO_IntakeBase io) { @Override public void periodic() { io.setArmAngle(localState.getDeg()); - io.setWheelSpeed(localState.getSpeed()) io.updateInputs(inputs); Logger.processInputs("Intake", inputs); } diff --git a/src/main/java/frc/robot/superstructure/SUB_Superstructure.java b/src/main/java/frc/robot/superstructure/SUB_Superstructure.java index 2721741..7d12bb9 100644 --- a/src/main/java/frc/robot/superstructure/SUB_Superstructure.java +++ b/src/main/java/frc/robot/superstructure/SUB_Superstructure.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.elevator.SUB_Elevator; import frc.robot.intake.SUB_Intake; +import frc.robot.superstructure.SuperstructureState.State; import frc.robot.util.SUB_Led; import org.littletonrobotics.junction.Logger; @@ -31,8 +32,8 @@ public void updateSuperstructureState(SuperstructureState.State newSuperstructur Logger.recordOutput("Superstructure State", currentSuperstructureState.toString()); } - public void updateIntakeWheelSpeed(double newIntakeWheelSpeed) { - intake.updateLocalWheelSpeed(newIntakeWheelSpeed); + public State getCurrentStateWithNewWheelSpeed(double newSpeed){ + return SuperstructureState.createState(currentSuperstructureState.getName(), currentSuperstructureState.getHeightM(), currentSuperstructureState.getDeg(), newSpeed); } public SuperstructureState.State getCurrentSuperstructureState() {