Better dynamic state pt2 - remove seperate wheel intake settings

WindingMotor · Feb 5, 2025 · ec7d0c0 · ec7d0c0
1 parent f9160cf
commit ec7d0c0
Show file tree

Hide file tree

Showing 5 changed files with 8 additions and 49 deletions.
diff --git 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
@@ -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
diff --git 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
@@ -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() {