Soumik locked in and fixed auto

Author: ExamWeHaltLitMaTowel

src/main/java/frc/robot/Constants.java

@@ -167,6 +167,7 @@ public static class ClimbHardware {
     public static final Spark.ID ENCODER_ID = new Spark.ID("climbHardware/Encoder", 51);
     public static final TalonFX.ID CLIMB_MOTOR_ID =
         new TalonFX.ID("climbHardware/Motor", PhoenixCANBus.RIO, 59);
+    public static final int CLIMB_PWM = 2;
   }
 
   public static class EndEffectorHardware {

src/main/java/frc/robot/HeadHoncho.java

@@ -497,52 +497,19 @@ public void end(boolean interrupted) {
       }
     },
     SCORE_REVERSE {
-      Timer panicTimer = new Timer();
 
       @Override
       public void initialize() {
         END_EFFECTOR_SUBSYSTEM.requestScoreReverse();
-        this.panicTimer.reset();
-        this.panicTimer.start();
-
       }
 
       @Override
       public SystemState nextState() {
         if (s_cancelButton.getAsBoolean()) return STOW;
 
         if (END_EFFECTOR_SUBSYSTEM.isEmpty()) {
-          panicTimer.reset();
-          panicTimer.stop();
           return INTAKE;
         };
-        if (LIFT_SUBSYSTEM.getState() == LiftStates.L4 && END_EFFECTOR_SUBSYSTEM.getState() == EndEffectorStates.SCORE_L4 && panicTimer.get() >= 0.5) {
-          return PANIC_SCORE_REVERSE;
-        }
-
-        return this;
-      }
-
-      @Override
-      public void end(boolean interrupted) {
-        LIFT_SUBSYSTEM.setState(TargetLiftStates.STOW);
-        END_EFFECTOR_SUBSYSTEM.requestStop();
-        DRIVE_SUBSYSTEM.cancelAutoAlign();
-        DRIVE_SUBSYSTEM.setDriveSpeed(Constants.Drive.FAST_SPEED_SCALAR);
-      }
-    },
-    PANIC_SCORE_REVERSE {
-      @Override
-      public void initialize() {
-        END_EFFECTOR_SUBSYSTEM.requestScoreReverse();
-        LIFT_SUBSYSTEM.setState(TargetLiftStates.PANIC);
-
-      }
-
-      @Override
-      public SystemState nextState() {
-        if (s_cancelButton.getAsBoolean()) return STOW;
-        if (END_EFFECTOR_SUBSYSTEM.isEmpty()) return INTAKE;
 
         return this;
       }
@@ -719,17 +686,31 @@ public Command autonomousScoreCommand() { {
                 END_EFFECTOR_SUBSYSTEM.setState(EndEffectorStates.SCORE_L4);
                 DRIVE_SUBSYSTEM.cancelAutoAlign();
               },
-              () -> {
-                LIFT_SUBSYSTEM.setState(TargetLiftStates.STOW);
-                INTAKE_SUBSYSTEM.startIntake();
-                END_EFFECTOR_SUBSYSTEM.requestIntake();
-              },
+              () -> {},
 		  this
     )
 		.until(() -> {
                 return END_EFFECTOR_SUBSYSTEM.isEmpty();
               })
-          .withTimeout(0.8);
+    .withTimeout(0.8)
+    .andThen(
+      Commands.startEnd(
+                () -> {
+          Logger.recordOutput("Auto/Command", Constants.NamedCommands.AUTO_SCORE_COMMAND_NAME);
+                  LIFT_SUBSYSTEM.setState(TargetLiftStates.PANIC);
+                  END_EFFECTOR_SUBSYSTEM.setState(EndEffectorStates.SCORE_L4);
+                },
+                () -> {
+                  LIFT_SUBSYSTEM.setState(TargetLiftStates.STOW);
+                  INTAKE_SUBSYSTEM.startIntake();
+                  END_EFFECTOR_SUBSYSTEM.requestIntake();
+                },
+        this
+      )
+      .until(() -> {
+        return END_EFFECTOR_SUBSYSTEM.isEmpty();
+      })
+      .withTimeout(0.5));
     }
   }
 

src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java

@@ -13,6 +13,7 @@
 import org.lasarobotics.hardware.revrobotics.Spark.SparkInputs;
 import org.littletonrobotics.junction.Logger;
 
+import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.LoopTimer;
@@ -27,9 +28,8 @@ public class ClimbSubsystem extends StateMachine implements AutoCloseable {
 
   public static record Hardware (
     Spark climbEncoder,
-    TalonFX climbMotor
-    
-    
+    TalonFX climbMotor,
+    Servo climbServo
   ) {}
 
   public enum ClimbStates implements SystemState {
@@ -53,6 +53,7 @@ public ClimbStates nextState() {
     MOUNT {
       @Override
       public void initialize() {
+        s_climbInstance.setServo(0.0);
         s_climbInstance.mount();
         if((s_climbInstance.inMountPosition())){
           s_climbInstance.setIsMounted(true);
@@ -77,6 +78,7 @@ public ClimbStates nextState() {
     CLIMB {
       @Override
       public void initialize() {
+        s_climbInstance.setServo(0.5);
         s_climbInstance.climb();
         s_climbInstance.setIsMounted(false);
 
@@ -101,6 +103,7 @@ public ClimbStates nextState() {
   private static ClimbSubsystem s_climbInstance;
   private final Spark m_climbEncoder;
   private final TalonFX m_climbMotor;
+  private final Servo m_climbServo;
   private ClimbStates nextState;
   private boolean m_mounted;
 
@@ -112,12 +115,15 @@ private ClimbSubsystem(Hardware ClimbHardware) {
 
     this.m_climbEncoder = ClimbHardware.climbEncoder;
     this.m_climbMotor = ClimbHardware.climbMotor;
+    this.m_climbServo = ClimbHardware.climbServo;
 
     this.m_mounted = false;
 
     TalonFXConfiguration motorConfig = new TalonFXConfiguration();
 
     this.m_climbMotor.getConfigurator().apply(motorConfig);
+
+    m_climbServo.set(1.0);
   }
 
   /**
@@ -164,7 +170,8 @@ public Command lowerClimberCommand() {
   public static Hardware initializeHardware() {
     Hardware climbHardware = new Hardware(
       new Spark(Constants.ClimbHardware.ENCODER_ID, MotorKind.NEO),
-      new TalonFX(Constants.ClimbHardware.CLIMB_MOTOR_ID.deviceID, Constants.ClimbHardware.CLIMB_MOTOR_ID.bus.name)
+      new TalonFX(Constants.ClimbHardware.CLIMB_MOTOR_ID.deviceID, Constants.ClimbHardware.CLIMB_MOTOR_ID.bus.name),
+      new Servo(Constants.ClimbHardware.CLIMB_PWM)
     );
 
 
@@ -214,6 +221,14 @@ public void stow() {
   public boolean inStowPosition() {
     return s_climbInstance.getInputs().absoluteEncoderPosition <= STOW_ANGLE;
   }
+  
+  /**
+   * Sets climb servo pwm value
+   * @param PWM
+   */
+  public void setServo(double PWM) {
+    m_climbServo.set(PWM);
+  }
 
   /**
    * Returns whether or not climber is in mount position