Repository: NicholsSchool/2025-Janice
Pull request #62: New DeAlgifier
Author: login: indoorfish1
Created: 2025-03-23T04:16:45Z | Merged: 2025-03-25T22:56:26Z

### Attempt at new dealgifier code.

By Nate's instructions there will be a forward and backward limit switch because I don't want to trust the compliant wheel rubbing against the metal for position data. So we're going to run the motor in or out at some velocity until it hits the limit switch. 

Another thing of importance is holding the algae once its in our robot. I saw a lot of teams try to keep the intake setpoint once an algae was in their control, but that ended up stalling the motors and causing the algae to drop. Instead, what I wanted to try is a StaticBrake, stronger than the normal idle mode brake, but requring some new control schemes with CTRE's control base. If this does not work (which I sort of suspect), we can go back to intaking the algae farther into our robot. 
I would love for a limit switch to be placed in the back of the grabber to indicate if or not we have an algae, but I don't see that going on soon, so for autos I will add support for .withTimeout() on the intake.

Oh also CAN ID's need to be updated are:
- Laterator: ID **39**
- Grabber: ID **38**

and we need the two new limit switch channels.

Note: StaticBrake shorts motor leads together so any input voltage is translated the other direction, similar to Lenz law. NeutralModeBrake simply applys zero volts to the motor and just revolves around friction. NeutralModeCoast applys zero amps to the motor and thus regards zero torque.


Diff:
diff --git a/2025-Janice/src/main/java/frc/robot/Constants.java b/2025-Janice/src/main/java/frc/robot/Constants.java
index 76f05a1..d4af76f 100644
--- a/2025-Janice/src/main/java/frc/robot/Constants.java
+++ b/2025-Janice/src/main/java/frc/robot/Constants.java
@@ -84,8 +84,8 @@ public static final class CAN {
     //TODO find real value
     public static final int kOuttakeMotor = 51;
 
-    public static final int kDeAlgifierKicker = 39;
-    public static final int kDeAlgifierArm = 38;
+    public static final int kDeAlgifierLaterator = 37;
+    public static final int kDeAlgifierGrabber = 38;
     //TODO someone check this please.
   }
 
@@ -198,17 +198,26 @@ public static final class OuttakeConstants {
   }
 
 public static final class DeAlgifierConstants {
-  public static final double kDeAlgifierCurrentLimit = 30;
-  public static final double kArmGearRatio = 28;
-  public static final double kKickerGearRatio = 1; //TODO find gear ratio
+  public static final int kFrontLimitSwitchChannel = 2; //TODO: find these
+  public static final int kBackLimitSwitchChannel = 3;
 
-  public static final double kArmAlgaeSetpointRad = 2.0;
-  public static final double kKickerSetpointRPM = 6000;
+  public static final double kDeAlgifierGrabberCurrentLimit = 60; //amps
+  public static final double kDeAlgifierLateratorCurrentLimit = 30; // low to make sure does not overrdrive into hard stop
 
-  public static final double ARM_P = 3.0;
-  public static final double ARM_D = 0.0;
-  public static final double KICKER_P = 0.0012;
-  public static final double KICKER_D = 0.0;
+  public static final double kLateratorGearRatio = 5;
+  public static final double kGrabberGearRatio = 3.86;
+
+  public static final double kGrabberIntakeSetpointRPM = 0.0;
+  public static final double kGrabberEjectSetpointRPM = 0.0;
+  public static final double kGrabberP = 1.0;
+  public static final double kGrabberD = 0.0;
+
+  public static final double kLateratorInPositionRad = 5.0; //only used in SIM
+  public static final double kLateratorOutPositionRad = -5.0; //TODO: UPDATE THESE
+
+  public static final double kLateratorVelocityGoalRadPerSec = 5;
+  public static final double kLateratorPVelocity = 2.0;
+  public static final double kLateratorDVelocity = 0.0;
 }
 
 
diff --git a/2025-Janice/src/main/java/frc/robot/RobotContainer.java b/2025-Janice/src/main/java/frc/robot/RobotContainer.java
index 6a5ad56..6a1499c 100644
--- a/2025-Janice/src/main/java/frc/robot/RobotContainer.java
+++ b/2025-Janice/src/main/java/frc/robot/RobotContainer.java
@@ -3,17 +3,11 @@
 import static edu.wpi.first.units.Units.Seconds;
 
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.path.PathPlannerPath;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -26,32 +20,24 @@
 import edu.wpi.first.wpilibj2.command.RepeatCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.Constants.RobotType;
 import frc.robot.commands.AutoCommands;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.DriveToHumanPlayer;
-import frc.robot.commands.DriveToPose;
 import frc.robot.commands.DriveToReef;
 import frc.robot.commands.DriveToReef.ReefDirection;
 import frc.robot.subsystems.DeAlgifier.DeAlgifier;
-import frc.robot.subsystems.DeAlgifier.DeAlgifierIO;
 import frc.robot.subsystems.DeAlgifier.DeAlgifierIOReal;
 import frc.robot.subsystems.DeAlgifier.DeAlgifierIOSim;
-import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.Intake.IntakeIOReal;
-import frc.robot.subsystems.Intake.IntakeIOSim;
 import frc.robot.subsystems.Outtake.Outtake;
 import frc.robot.subsystems.Outtake.OuttakeIO;
 import frc.robot.subsystems.Outtake.OuttakeIOSim;
 import frc.robot.subsystems.Outtake.OuttakeIOReal;
 import frc.robot.subsystems.climber.Climber;
 import frc.robot.subsystems.climber.ClimberIO;
-import frc.robot.subsystems.climber.ClimberIOReal;
 import frc.robot.subsystems.climber.ClimberIOSim;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIONAVX;
-import frc.robot.subsystems.drive.GyroIORedux;
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOMaxSwerve;
 import frc.robot.subsystems.drive.ModuleIOSim;
@@ -68,8 +54,6 @@
 import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
 import static frc.robot.subsystems.vision.VisionConstants.*;
 
-import java.lang.annotation.Repeatable;
-
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
@@ -218,6 +202,7 @@ public RobotContainer() {
         elevator = new Elevator(new ElevatorIO() {});
         outtake = new Outtake(new OuttakeIO() {});
         climber = new Climber(new ClimberIO() {});
+        deAlgifier = new DeAlgifier(new DeAlgifierIOSim() {});
         // (Use same number of dummy implementations as the real robot)
         vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
         break;
@@ -410,8 +395,18 @@ private void configureButtonBindings() {
     operatorController.leftTrigger(0.8).whileTrue(new RepeatCommand( new InstantCommand( () -> outtake.outtake(), outtake )));
     //operatorController.leftTrigger(0.8).whileFalse(new InstantCommand(() -> outtake.processCoral(), outtake ));
 
-    operatorController.povUp().and(operatorController.start().and(operatorController.rightStick())).whileTrue(new InstantCommand(() -> climber.setClimbState(true)));
-    
+    // //axis 4 is Right X
+    // operatorController.axisGreaterThan(4, 0.8).onTrue( new RepeatCommand(new InstantCommand( () -> deAlgifier.lateratorOut() )));
+    // operatorController.axisLessThan(4, -0.8).onTrue( new RepeatCommand(new InstantCommand( () -> deAlgifier.lateratorIn() )));
+    deAlgifier.setDefaultCommand(new InstantCommand(() -> deAlgifier.lateratorManual(operatorController.getRightY()), deAlgifier));
+
+    operatorController.rightTrigger(0.8).whileTrue(new RepeatCommand(new InstantCommand( () -> deAlgifier.intake() )));
+    operatorController.rightBumper().whileTrue(new RepeatCommand(new InstantCommand( () -> deAlgifier.outtake() )));
+    operatorController.rightTrigger(0.8)
+      .and(operatorController.rightBumper())
+      .whileFalse(new InstantCommand( () -> deAlgifier.holdAlgae()));
+
+
   }
 
   // /**
diff --git a/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifier.java b/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifier.java
index 6b52954..c307c12 100644
--- a/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifier.java
+++ b/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifier.java
@@ -2,7 +2,6 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
 import frc.robot.Constants.DeAlgifierConstants;
 
 import org.littletonrobotics.junction.Logger;
@@ -11,15 +10,40 @@ public class DeAlgifier extends SubsystemBase{
     private DeAlgifierIO io;
     private final DeAlgifierIOInputsAutoLogged inputs = new DeAlgifierIOInputsAutoLogged();
 
-    private final PIDController armPidController;
-    private final PIDController kickerPidController;
+    private final PIDController lateratorPidController;
+    private final PIDController grabberPidController;
+
+    private LateratorMode lateratorMode;
+    private GrabberMode grabberMode;
+
+    private double lateralManual = 0.0;
+
+    boolean manual = true;
+
+    enum LateratorMode {
+        IN,
+        OUT,
+        IDLE,
+    }
+
+    enum GrabberMode {
+        INTAKE,
+        OUTTAKE,
+        HOLD,
+        IDLE
+    }
+
     
     public DeAlgifier(DeAlgifierIO io){
         this.io = io;
-        this.armPidController = new PIDController(DeAlgifierConstants.ARM_P, 0, DeAlgifierConstants.ARM_D);
-        this.kickerPidController = new PIDController(DeAlgifierConstants.KICKER_P, 0, DeAlgifierConstants.KICKER_D);
-        armPidController.reset();
-        kickerPidController.reset();
+        lateratorPidController = new PIDController(DeAlgifierConstants.kLateratorPVelocity, 0, DeAlgifierConstants.kLateratorDVelocity);
+        grabberPidController = new PIDController(DeAlgifierConstants.kGrabberP, 0, DeAlgifierConstants.kGrabberD);
+
+        lateratorPidController.reset();
+        grabberPidController.reset();
+
+        lateratorMode = LateratorMode.IDLE;
+        grabberMode = GrabberMode.IDLE;
     }
     
     public void periodic(){
@@ -27,29 +51,78 @@ public void periodic(){
         Logger.processInputs("DeAlgifier", inputs);
         
         if (DriverStation.isDisabled()) {
-             io.setArmVoltage(0.0);
-             io.setKickerVoltage(0.0);
+             io.setLateratorVoltage(0.0);
+             io.setGrabberBrake(true);
+
+             lateratorMode = LateratorMode.IDLE;
+             grabberMode = GrabberMode.IDLE; //for safety upon reenable
+
+             return;
         } 
-        else {
-            //io.setArmVoltage(armPidController.calculate(inputs.armPositionRad));
-            // io.setKickerVoltage( kickerPidController.getSetpoint() == 0.0 ? 0.0 : -5.0 ); //using the pid controller as a state machine
-            // System.out.println(kickerPidController.getSetpoint());
+        if(manual){
+            io.setLateratorVoltage(lateralManual);
+        }else{
+        
+            switch( lateratorMode ) {
+            case IN -> {
+                if( !inputs.backLimitSwitch)
+                    io.setLateratorVoltage(lateratorPidController.calculate(
+                        inputs.lateratorVelocityRadPerSec, -DeAlgifierConstants.kLateratorVelocityGoalRadPerSec));
+                else io.setLateratorVoltage(0.0);
+            }
+            case OUT -> {
+                if( !inputs.frontLimitSwitch )
+                    io.setLateratorVoltage(lateratorPidController.calculate(
+                        inputs.lateratorVelocityRadPerSec, DeAlgifierConstants.kLateratorVelocityGoalRadPerSec));
+                else io.setLateratorVoltage(0.0);
+            }
+            case IDLE -> io.setLateratorVoltage(0.0);
+            default -> io.setLateratorVoltage(0.0);
+        }
+
+        switch( grabberMode ) {
+            case INTAKE -> {
+                io.setGrabberBrake(false);
+                io.setGrabberVoltage(grabberPidController.calculate(inputs.grabberVelocityRPM, DeAlgifierConstants.kGrabberIntakeSetpointRPM));
+            }
+            case OUTTAKE -> {
+                io.setGrabberBrake(false);
+                io.setGrabberVoltage(grabberPidController.calculate(inputs.grabberVelocityRPM, DeAlgifierConstants.kGrabberEjectSetpointRPM));
+            }
+            case HOLD, IDLE -> io.setGrabberBrake(true);
+            default -> io.setGrabberBrake(true);
+        }
         }
     }
+    
 
-    public void deAlgify(double input) {
-        io.setArmVoltage(input);
-        kickerPidController.setSetpoint(Math.abs(input) > Constants.JOYSTICK_DEADBAND ? DeAlgifierConstants.kKickerSetpointRPM : 0.0);
-        // System.out.println("Dealgifying");
+    /**
+     * Called once to request laterator out
+     */
+    public void lateratorOut() {
+        lateratorMode = LateratorMode.OUT;
     }
 
-    public void resetToZero() {
-        armPidController.setSetpoint(0.0);
-        kickerPidController.setSetpoint(0.0);
+    public void lateratorManual(double input){
+        lateralManual = input*4.0;
     }
-    
-    public void stop() {
-        armPidController.setSetpoint(inputs.armPositionRad);
-        kickerPidController.setSetpoint(0.0);
+
+    /**
+     * Called once to request laterator in
+     */
+    public void lateratorIn() {
+       lateratorMode = LateratorMode.IN;
+    }
+
+    public void intake() {
+        grabberMode = GrabberMode.INTAKE;
+    }
+
+    public void outtake() {
+        grabberMode = GrabberMode.OUTTAKE;
+    }
+
+    public void holdAlgae() {
+        grabberMode = GrabberMode.HOLD;
     }
 }
\ No newline at end of file
diff --git a/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIO.java b/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIO.java
index e0c9166..4c45dec 100644
--- a/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIO.java
+++ b/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIO.java
@@ -5,21 +5,27 @@ public interface DeAlgifierIO {
 
     @AutoLog
     public static class DeAlgifierIOInputs {
-        public double armMotorVoltage = 0.0;
-        public double armSupplyVoltage = 0.0;
-        public double armCurrentAmps = 0.0;
-        public double armPositionRad = 0.0;
+        public double lateratorMotorVoltage = 0.0;
+        public double lateratorSupplyVoltage = 0.0;
+        public double lateratorCurrentAmps = 0.0;
+        public double lateratorPositionRad = 0.0;
+        public double lateratorVelocityRadPerSec = 0.0;
 
-        public double kickerMotorVoltage = 0.0;
-        public double kickerSupplyVoltage = 0.0;
-        public double kickerCurrentAmps = 0.0;
-        public double kickerVelocityRPM = 0.0;
+        public double grabberMotorVoltage = 0.0;
+        public double grabberSupplyVoltage = 0.0;
+        public double grabberCurrentAmps = 0.0;
+        public double grabberVelocityRPM = 0.0;
+
+        public boolean frontLimitSwitch = false;
+        public boolean backLimitSwitch = false;
     }
         /** Updates the set of loggable inputs. */
-        public default void updateInputs(DeAlgifierIOInputs inputs) {}
+        public void updateInputs(DeAlgifierIOInputs inputs);
         
         /** Set voltage command */
-        public default void setArmVoltage(double voltage) {}
+        public default void setLateratorVoltage(double voltage) {}
+
+        public default void setGrabberVoltage( double voltage ) {}
 
-        public default void setKickerVoltage( double voltage ) {}
+        public default void setGrabberBrake(boolean enable) {} 
 }
diff --git a/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOReal.java b/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOReal.java
index d786ccb..69f9737 100644
--- a/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOReal.java
+++ b/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOReal.java
@@ -1,60 +1,83 @@
 package frc.robot.subsystems.DeAlgifier;
 
+import static edu.wpi.first.units.Units.RPM;
 import static edu.wpi.first.units.Units.Radian;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.StaticBrake;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkMaxConfig;
-
+import edu.wpi.first.wpilibj.DigitalInput;
 import frc.robot.Constants.CAN;
 import frc.robot.Constants.DeAlgifierConstants;
 
 public class DeAlgifierIOReal implements DeAlgifierIO {
 
-    private TalonFX arm;
-    private SparkMax kicker;
+    private TalonFX laterator;
+    private TalonFX grabber;
+    private StaticBrake staticBrake;
+    private VoltageOut voltageControl;
+
+    private DigitalInput frontLimitSwitch;
+    private DigitalInput backLimitSwitch;
 
     public DeAlgifierIOReal() {
-        arm = new TalonFX(CAN.kDeAlgifierArm, "Elevator");
-        TalonFXConfiguration config = new TalonFXConfiguration();
-        config.CurrentLimits.StatorCurrentLimit = DeAlgifierConstants.kDeAlgifierCurrentLimit;
-        config.CurrentLimits.StatorCurrentLimitEnable = true;
-        config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
-        config.Feedback.SensorToMechanismRatio = DeAlgifierConstants.kArmGearRatio;
-        arm.getConfigurator().apply(config);
-        arm.setPosition(0.0);
-
-        kicker = new SparkMax(CAN.kDeAlgifierKicker, MotorType.kBrushless );
-        SparkMaxConfig config2 = new SparkMaxConfig();
-        config2.alternateEncoder.inverted(false);
-        config2.alternateEncoder.velocityConversionFactor(DeAlgifierConstants.kKickerGearRatio);
-        kicker.configure(config2, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        laterator = new TalonFX(CAN.kDeAlgifierLaterator, "Elevator");
+        TalonFXConfiguration lateratorConfig = new TalonFXConfiguration();
+        lateratorConfig.CurrentLimits.StatorCurrentLimit = 20;
+        lateratorConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+        lateratorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+        lateratorConfig.Feedback.SensorToMechanismRatio = DeAlgifierConstants.kLateratorGearRatio;
+        laterator.getConfigurator().apply(lateratorConfig);
+
+        grabber = new TalonFX(CAN.kDeAlgifierGrabber, "Elevator");
+        TalonFXConfiguration grabberConfig = new TalonFXConfiguration();
+        grabberConfig.CurrentLimits.StatorCurrentLimit = DeAlgifierConstants.kDeAlgifierGrabberCurrentLimit;
+        grabberConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+        grabberConfig.CurrentLimits.SupplyCurrentLimit = 30;
+        grabberConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+        grabberConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+        grabberConfig.Feedback.SensorToMechanismRatio = DeAlgifierConstants.kGrabberGearRatio;
+        grabber.getConfigurator().apply(grabberConfig);
+
+        staticBrake = new StaticBrake();
+
+        voltageControl = new VoltageOut(0.0);
+        grabber.setControl(voltageControl);
+
+        frontLimitSwitch = new DigitalInput(DeAlgifierConstants.kFrontLimitSwitchChannel);
+        backLimitSwitch = new DigitalInput(DeAlgifierConstants.kBackLimitSwitchChannel);
     }
 
     public void updateInputs(DeAlgifierIOInputs inputs){
-        inputs.armMotorVoltage = arm.getMotorVoltage().getValueAsDouble();
-        inputs.armCurrentAmps = arm.getStatorCurrent().getValueAsDouble();
-        inputs.armSupplyVoltage = arm.getSupplyVoltage().getValueAsDouble();
-        inputs.armPositionRad = arm.getPosition().getValue().in(Radian);
-
-        inputs.kickerMotorVoltage = kicker.getAppliedOutput();
-        inputs.kickerSupplyVoltage = kicker.getBusVoltage();
-        inputs.kickerCurrentAmps = kicker.getOutputCurrent();
-        inputs.kickerVelocityRPM = kicker.getEncoder().getVelocity();
+        inputs.lateratorMotorVoltage = laterator.getMotorVoltage().getValueAsDouble();
+        inputs.lateratorCurrentAmps = laterator.getStatorCurrent().getValueAsDouble();
+        inputs.lateratorSupplyVoltage = laterator.getSupplyVoltage().getValueAsDouble();
+        inputs.lateratorPositionRad = laterator.getPosition().getValue().in(Radian);
+        inputs.lateratorVelocityRadPerSec = laterator.getVelocity().getValue().in(RadiansPerSecond);
+
+        inputs.grabberMotorVoltage = grabber.getMotorVoltage().getValueAsDouble();
+        inputs.grabberSupplyVoltage = grabber.getSupplyVoltage().getValueAsDouble();
+        inputs.grabberCurrentAmps = grabber.getSupplyCurrent().getValueAsDouble();
+        inputs.grabberVelocityRPM = grabber.getVelocity().getValue().in(RPM);
+
+        inputs.frontLimitSwitch = frontLimitSwitch.get();
+        inputs.backLimitSwitch = backLimitSwitch.get();
     }
 
     @Override
-    public void setArmVoltage(double voltage){
-        arm.setVoltage(voltage);
+    public void setLateratorVoltage(double voltage){
+        laterator.setVoltage(voltage);
     }
 
     @Override
-    public void setKickerVoltage( double voltage ) {
-        kicker.setVoltage(voltage);
+    public void setGrabberVoltage( double voltage ) {
+        voltageControl.Output = voltage;
+    }
+
+    public void setGrabberBrake( boolean enable ) {
+        grabber.setControl(enable ? staticBrake : voltageControl);
     }
 }
diff --git a/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOSim.java b/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOSim.java
index e7e3220..f66e94a 100644
--- a/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOSim.java
+++ b/2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOSim.java
@@ -7,38 +7,48 @@
 
 public class DeAlgifierIOSim implements DeAlgifierIO {
     
-    private static final DCMotor arm = DCMotor.getFalcon500(1);
-    private static final DCMotor kicker = DCMotor.getNeo550(1);
+    private static final DCMotor laterator = DCMotor.getKrakenX60(1);
+    private static final DCMotor grabber = DCMotor.getFalcon500(1);
 
-    private final DCMotorSim armMotor =
+    private final DCMotorSim lateratorMotor =
       new DCMotorSim(
-          LinearSystemId.createDCMotorSystem(arm, 0.025, 1.0),
-          arm);
+          LinearSystemId.createDCMotorSystem(laterator, 0.025, DeAlgifierConstants.kLateratorGearRatio),
+          laterator);
     
-    private final DCMotorSim kickerMotor =
+    private final DCMotorSim grabberMotor =
       new DCMotorSim(
-          LinearSystemId.createDCMotorSystem(kicker, 0.025, DeAlgifierConstants.kKickerGearRatio),
-          kicker);
+          LinearSystemId.createDCMotorSystem(grabber, 0.025, DeAlgifierConstants.kGrabberGearRatio),
+          grabber);
 
     public void updateInputs(DeAlgifierIOInputs inputs){ //TODO: fix these update inputs
-        inputs.armMotorVoltage = armMotor.getInputVoltage();
-        inputs.armSupplyVoltage = armMotor.getInputVoltage();
-        inputs.armCurrentAmps = armMotor.getCurrentDrawAmps();
-        inputs.armPositionRad = armMotor.getAngularPositionRad();
-
-        inputs.kickerMotorVoltage = kickerMotor.getInputVoltage();
-        inputs.kickerSupplyVoltage = kickerMotor.getInputVoltage();
-        inputs.kickerCurrentAmps = kickerMotor.getCurrentDrawAmps();
-        inputs.kickerVelocityRPM = kickerMotor.getAngularVelocityRPM();
+        inputs.lateratorMotorVoltage = lateratorMotor.getInputVoltage();
+        inputs.lateratorCurrentAmps = lateratorMotor.getCurrentDrawAmps();
+        inputs.lateratorSupplyVoltage = lateratorMotor.getInputVoltage();
+        inputs.lateratorPositionRad = lateratorMotor.getAngularPositionRad();
+        inputs.lateratorVelocityRadPerSec = lateratorMotor.getAngularVelocityRadPerSec();
+
+        inputs.grabberMotorVoltage = grabberMotor.getInputVoltage();
+        inputs.grabberSupplyVoltage = grabberMotor.getInputVoltage();
+        inputs.grabberCurrentAmps = grabberMotor.getCurrentDrawAmps();
+        inputs.grabberVelocityRPM = grabberMotor.getAngularVelocityRPM();
+
+        inputs.frontLimitSwitch = inputs.lateratorPositionRad > DeAlgifierConstants.kLateratorOutPositionRad;
+        inputs.backLimitSwitch = inputs.lateratorPositionRad < DeAlgifierConstants.kLateratorInPositionRad;
+    }
+
+    @Override
+    public void setLateratorVoltage(double voltage){
+        lateratorMotor.setInputVoltage(voltage);
     }
 
     @Override
-    public void setArmVoltage( double voltage ) {
-        armMotor.setInputVoltage(voltage);
+    public void setGrabberVoltage( double voltage ) {
+        grabberMotor.setInputVoltage(voltage);
     }
 
     @Override
-    public void setKickerVoltage( double voltage ) {
-        kickerMotor.setInputVoltage(voltage);
+    public void setGrabberBrake( boolean enable ) {
+        if( enable )
+            grabberMotor.setInputVoltage(0);
     }
 }


Reviews:
user: login: tom-greier
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-23T19:46:12Z
user: login: tom-greier
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-23T19:48:20Z
user: login: tom-greier
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-23T19:49:58Z
user: login: tom-greier
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-23T19:50:57Z
user: login: tom-greier
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-23T19:51:38Z
user: login: tom-greier
body: I think you should update the subsystem to put the state machine into the periodic function instead of using the methods to change voltage levels and run the pid.
Periodic should run pids so that they are always being updated with latest position information.
state: CHANGES_REQUESTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-23T19:54:02Z
user: login: indoorfish1
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-03-23T20:11:45Z
user: login: tom-greier
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-25T10:33:08Z
user: login: tom-greier
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-25T10:33:56Z

Review comments:
diff_hunk: @@ -385,11 +369,19 @@ private void configureButtonBindings() {
 
     outtake.setDefaultCommand(new InstantCommand(() -> outtake.stop(), outtake ) );
     operatorController.leftTrigger(0.8).whileTrue(new RepeatCommand( new InstantCommand( () -> outtake.outtake(), outtake )));
-    //operatorController.leftTrigger(0.8).whileFalse(new InstantCommand(() -> outtake.processCoral(), outtake ));
-    deAlgifier.setDefaultCommand(new InstantCommand(() -> deAlgifier.deAlgify(operatorController.getRightY()), deAlgifier));
 
-    operatorController.povUp().and(operatorController.start().and(operatorController.rightStick())).whileTrue(new InstantCommand(() -> climber.setClimbState(true)));
-    
+
+    //axis 4 is Right X
+    operatorController.axisGreaterThan(4, 0.8).onTrue( new RepeatCommand(new InstantCommand( () -> deAlgifier.lateratorOut() )));
+    operatorController.axisLessThan(4, -0.8).onTrue( new RepeatCommand(new InstantCommand( () -> deAlgifier.lateratorIn() )));
+
+    operatorController.rightTrigger(0.8).whileTrue(new RepeatCommand(new InstantCommand( () -> deAlgifier.intake() )));
+    operatorController.rightBumper().whileTrue(new RepeatCommand(new InstantCommand( () -> deAlgifier.outtake() )));
+    operatorController.rightTrigger(0.8)
+      .and(operatorController.rightBumper())
path: 2025-Janice/src/main/java/frc/robot/RobotContainer.java
user: login: tom-greier
body: interesting using a combination of buttons, plus the two states of one of the buttons.
created_at: 2025-03-23T19:46:12Z
author_association: CONTRIBUTOR
diff_hunk: @@ -11,45 +10,58 @@ public class DeAlgifier extends SubsystemBase{
     private DeAlgifierIO io;
     private final DeAlgifierIOInputsAutoLogged inputs = new DeAlgifierIOInputsAutoLogged();
 
-    private final PIDController armPidController;
-    private final PIDController kickerPidController;
+    private final PIDController lateratorPidController;
+    private final PIDController grabberPidController;
+
     
     public DeAlgifier(DeAlgifierIO io){
         this.io = io;
-        this.armPidController = new PIDController(DeAlgifierConstants.ARM_P, 0, DeAlgifierConstants.ARM_D);
-        this.kickerPidController = new PIDController(DeAlgifierConstants.KICKER_P, 0, DeAlgifierConstants.KICKER_D);
-        armPidController.reset();
-        kickerPidController.reset();
+        lateratorPidController = new PIDController(DeAlgifierConstants.kLateratorPVelocity, 0, DeAlgifierConstants.kLateratorDVelocity);
+        grabberPidController = new PIDController(DeAlgifierConstants.kGrabberP, 0, DeAlgifierConstants.kGrabberD);
+
+        lateratorPidController.reset();
+        grabberPidController.reset();
     }
     
     public void periodic(){
         io.updateInputs(inputs);
         Logger.processInputs("DeAlgifier", inputs);
         
         if (DriverStation.isDisabled()) {
-             io.setArmVoltage(0.0);
-             io.setKickerVoltage(0.0);
+             io.setLateratorVoltage(0.0);
+             io.setGrabberBrake(true);
         } 
-        else {
-            //io.setArmVoltage(armPidController.calculate(inputs.armPositionRad));
-            // io.setKickerVoltage( kickerPidController.getSetpoint() == 0.0 ? 0.0 : -5.0 ); //using the pid controller as a state machine
-            // System.out.println(kickerPidController.getSetpoint());
-        }
     }
 
-    public void deAlgify(double input) {
-        io.setArmVoltage(input);
-        kickerPidController.setSetpoint(Math.abs(input) > Constants.JOYSTICK_DEADBAND ? DeAlgifierConstants.kKickerSetpointRPM : 0.0);
-        // System.out.println("Dealgifying");
+    /**
+     * Called once to request laterator out
+     */
+    public void lateratorOut() {
+        if( !inputs.frontLimitSwitch )
+            io.setLateratorVoltage(lateratorPidController.calculate(inputs.lateratorVelocityRadPerSec, DeAlgifierConstants.kLateratorVelocityGoalRadPerSec));
path: 2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifier.java
user: login: tom-greier
body: The PID really should run in the periodic loop and use these methods to set state requests. 
The state machine itself though should be in the periodic.
created_at: 2025-03-23T19:48:20Z
author_association: CONTRIBUTOR
diff_hunk: @@ -11,45 +10,58 @@ public class DeAlgifier extends SubsystemBase{
     private DeAlgifierIO io;
     private final DeAlgifierIOInputsAutoLogged inputs = new DeAlgifierIOInputsAutoLogged();
 
-    private final PIDController armPidController;
-    private final PIDController kickerPidController;
+    private final PIDController lateratorPidController;
+    private final PIDController grabberPidController;
+
     
     public DeAlgifier(DeAlgifierIO io){
         this.io = io;
-        this.armPidController = new PIDController(DeAlgifierConstants.ARM_P, 0, DeAlgifierConstants.ARM_D);
-        this.kickerPidController = new PIDController(DeAlgifierConstants.KICKER_P, 0, DeAlgifierConstants.KICKER_D);
-        armPidController.reset();
-        kickerPidController.reset();
+        lateratorPidController = new PIDController(DeAlgifierConstants.kLateratorPVelocity, 0, DeAlgifierConstants.kLateratorDVelocity);
+        grabberPidController = new PIDController(DeAlgifierConstants.kGrabberP, 0, DeAlgifierConstants.kGrabberD);
+
+        lateratorPidController.reset();
+        grabberPidController.reset();
     }
     
     public void periodic(){
         io.updateInputs(inputs);
         Logger.processInputs("DeAlgifier", inputs);
         
         if (DriverStation.isDisabled()) {
-             io.setArmVoltage(0.0);
-             io.setKickerVoltage(0.0);
+             io.setLateratorVoltage(0.0);
+             io.setGrabberBrake(true);
         } 
-        else {
-            //io.setArmVoltage(armPidController.calculate(inputs.armPositionRad));
-            // io.setKickerVoltage( kickerPidController.getSetpoint() == 0.0 ? 0.0 : -5.0 ); //using the pid controller as a state machine
-            // System.out.println(kickerPidController.getSetpoint());
-        }
     }
 
-    public void deAlgify(double input) {
-        io.setArmVoltage(input);
-        kickerPidController.setSetpoint(Math.abs(input) > Constants.JOYSTICK_DEADBAND ? DeAlgifierConstants.kKickerSetpointRPM : 0.0);
-        // System.out.println("Dealgifying");
+    /**
+     * Called once to request laterator out
+     */
+    public void lateratorOut() {
+        if( !inputs.frontLimitSwitch )
+            io.setLateratorVoltage(lateratorPidController.calculate(inputs.lateratorVelocityRadPerSec, DeAlgifierConstants.kLateratorVelocityGoalRadPerSec));
+        else io.setLateratorVoltage(0.0);
     }
 
-    public void resetToZero() {
-        armPidController.setSetpoint(0.0);
-        kickerPidController.setSetpoint(0.0);
+    /**
+     * Called once to request laterator in
+     */
+    public void lateratorIn() {
+        if( !inputs.backLimitSwitch)
+            io.setLateratorVoltage(lateratorPidController.calculate(inputs.lateratorVelocityRadPerSec, -DeAlgifierConstants.kLateratorVelocityGoalRadPerSec));
+        else io.setLateratorVoltage(0.0);
     }
-    
-    public void stop() {
-        armPidController.setSetpoint(inputs.armPositionRad);
-        kickerPidController.setSetpoint(0.0);
+
+    public void intake() {
+        io.setGrabberBrake(false);
+        io.setGrabberVoltage(grabberPidController.calculate(inputs.grabberVelocityRPM, DeAlgifierConstants.kGrabberIntakeSetpointRPM));
path: 2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifier.java
user: login: tom-greier
body: Same for intake and outtake. These should all be called in the periodic loop.
Have the intake/outtake methods set state requests which are then processed by state machine in periodic loop.
Otherwise, the last voltage command will remain active when you stop calling this method.
created_at: 2025-03-23T19:49:58Z
author_association: CONTRIBUTOR
diff_hunk: @@ -1,60 +1,83 @@
 package frc.robot.subsystems.DeAlgifier;
 
path: 2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOReal.java
user: login: tom-greier
body: This all looks good.
created_at: 2025-03-23T19:50:57Z
author_association: CONTRIBUTOR
diff_hunk: @@ -7,38 +7,48 @@
 
 public class DeAlgifierIOSim implements DeAlgifierIO {
     
-    private static final DCMotor arm = DCMotor.getFalcon500(1);
-    private static final DCMotor kicker = DCMotor.getNeo550(1);
+    private static final DCMotor laterator = DCMotor.getKrakenX60(1);
+    private static final DCMotor grabber = DCMotor.getFalcon500(1);
 
-    private final DCMotorSim armMotor =
+    private final DCMotorSim lateratorMotor =
       new DCMotorSim(
-          LinearSystemId.createDCMotorSystem(arm, 0.025, 1.0),
-          arm);
+          LinearSystemId.createDCMotorSystem(laterator, 0.025, DeAlgifierConstants.kLateratorGearRatio),
+          laterator);
     
-    private final DCMotorSim kickerMotor =
+    private final DCMotorSim grabberMotor =
       new DCMotorSim(
-          LinearSystemId.createDCMotorSystem(kicker, 0.025, DeAlgifierConstants.kKickerGearRatio),
-          kicker);
+          LinearSystemId.createDCMotorSystem(grabber, 0.025, DeAlgifierConstants.kGrabberGearRatio),
+          grabber);
 
     public void updateInputs(DeAlgifierIOInputs inputs){ //TODO: fix these update inputs
-        inputs.armMotorVoltage = armMotor.getInputVoltage();
-        inputs.armSupplyVoltage = armMotor.getInputVoltage();
-        inputs.armCurrentAmps = armMotor.getCurrentDrawAmps();
-        inputs.armPositionRad = armMotor.getAngularPositionRad();
-
-        inputs.kickerMotorVoltage = kickerMotor.getInputVoltage();
-        inputs.kickerSupplyVoltage = kickerMotor.getInputVoltage();
-        inputs.kickerCurrentAmps = kickerMotor.getCurrentDrawAmps();
-        inputs.kickerVelocityRPM = kickerMotor.getAngularVelocityRPM();
+        inputs.lateratorMotorVoltage = lateratorMotor.getInputVoltage();
+        inputs.lateratorCurrentAmps = lateratorMotor.getCurrentDrawAmps();
+        inputs.lateratorSupplyVoltage = lateratorMotor.getInputVoltage();
+        inputs.lateratorPositionRad = lateratorMotor.getAngularPositionRad();
+        inputs.lateratorVelocityRadPerSec = lateratorMotor.getAngularVelocityRadPerSec();
+
+        inputs.grabberMotorVoltage = grabberMotor.getInputVoltage();
+        inputs.grabberSupplyVoltage = grabberMotor.getInputVoltage();
+        inputs.grabberCurrentAmps = grabberMotor.getCurrentDrawAmps();
+        inputs.grabberVelocityRPM = grabberMotor.getAngularVelocityRPM();
+
+        inputs.frontLimitSwitch = inputs.lateratorPositionRad > DeAlgifierConstants.kLateratorOutPositionRad;
+        inputs.backLimitSwitch = inputs.lateratorPositionRad < DeAlgifierConstants.kLateratorInPositionRad;
path: 2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOSim.java
user: login: tom-greier
body: Nice sim logic. Does it work?
created_at: 2025-03-23T19:51:38Z
author_association: CONTRIBUTOR
diff_hunk: @@ -7,38 +7,48 @@
 
 public class DeAlgifierIOSim implements DeAlgifierIO {
     
-    private static final DCMotor arm = DCMotor.getFalcon500(1);
-    private static final DCMotor kicker = DCMotor.getNeo550(1);
+    private static final DCMotor laterator = DCMotor.getKrakenX60(1);
+    private static final DCMotor grabber = DCMotor.getFalcon500(1);
 
-    private final DCMotorSim armMotor =
+    private final DCMotorSim lateratorMotor =
       new DCMotorSim(
-          LinearSystemId.createDCMotorSystem(arm, 0.025, 1.0),
-          arm);
+          LinearSystemId.createDCMotorSystem(laterator, 0.025, DeAlgifierConstants.kLateratorGearRatio),
+          laterator);
     
-    private final DCMotorSim kickerMotor =
+    private final DCMotorSim grabberMotor =
       new DCMotorSim(
-          LinearSystemId.createDCMotorSystem(kicker, 0.025, DeAlgifierConstants.kKickerGearRatio),
-          kicker);
+          LinearSystemId.createDCMotorSystem(grabber, 0.025, DeAlgifierConstants.kGrabberGearRatio),
+          grabber);
 
     public void updateInputs(DeAlgifierIOInputs inputs){ //TODO: fix these update inputs
-        inputs.armMotorVoltage = armMotor.getInputVoltage();
-        inputs.armSupplyVoltage = armMotor.getInputVoltage();
-        inputs.armCurrentAmps = armMotor.getCurrentDrawAmps();
-        inputs.armPositionRad = armMotor.getAngularPositionRad();
-
-        inputs.kickerMotorVoltage = kickerMotor.getInputVoltage();
-        inputs.kickerSupplyVoltage = kickerMotor.getInputVoltage();
-        inputs.kickerCurrentAmps = kickerMotor.getCurrentDrawAmps();
-        inputs.kickerVelocityRPM = kickerMotor.getAngularVelocityRPM();
+        inputs.lateratorMotorVoltage = lateratorMotor.getInputVoltage();
+        inputs.lateratorCurrentAmps = lateratorMotor.getCurrentDrawAmps();
+        inputs.lateratorSupplyVoltage = lateratorMotor.getInputVoltage();
+        inputs.lateratorPositionRad = lateratorMotor.getAngularPositionRad();
+        inputs.lateratorVelocityRadPerSec = lateratorMotor.getAngularVelocityRadPerSec();
+
+        inputs.grabberMotorVoltage = grabberMotor.getInputVoltage();
+        inputs.grabberSupplyVoltage = grabberMotor.getInputVoltage();
+        inputs.grabberCurrentAmps = grabberMotor.getCurrentDrawAmps();
+        inputs.grabberVelocityRPM = grabberMotor.getAngularVelocityRPM();
+
+        inputs.frontLimitSwitch = inputs.lateratorPositionRad > DeAlgifierConstants.kLateratorOutPositionRad;
+        inputs.backLimitSwitch = inputs.lateratorPositionRad < DeAlgifierConstants.kLateratorInPositionRad;
path: 2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifierIOSim.java
user: login: indoorfish1
body: It does I just have to confirm motor direction on the real robot. But clockwise is positive so it should be good. 
created_at: 2025-03-23T20:11:45Z
author_association: COLLABORATOR
diff_hunk: @@ -2,54 +2,117 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
 import frc.robot.Constants.DeAlgifierConstants;
 
 import org.littletonrobotics.junction.Logger;
 
+import com.fasterxml.jackson.annotation.JsonTypeInfo.Id;
+
 public class DeAlgifier extends SubsystemBase{
     private DeAlgifierIO io;
     private final DeAlgifierIOInputsAutoLogged inputs = new DeAlgifierIOInputsAutoLogged();
 
-    private final PIDController armPidController;
-    private final PIDController kickerPidController;
+    private final PIDController lateratorPidController;
+    private final PIDController grabberPidController;
+
+    private LateratorMode lateratorMode;
+    private GrabberMode grabberMode;
+
+    enum LateratorMode {
+        IN,
+        OUT,
+        IDLE
+    }
+
+    enum GrabberMode {
+        INTAKE,
+        OUTTAKE,
+        HOLD,
+        IDLE
+    }
+
     
     public DeAlgifier(DeAlgifierIO io){
         this.io = io;
-        this.armPidController = new PIDController(DeAlgifierConstants.ARM_P, 0, DeAlgifierConstants.ARM_D);
-        this.kickerPidController = new PIDController(DeAlgifierConstants.KICKER_P, 0, DeAlgifierConstants.KICKER_D);
-        armPidController.reset();
-        kickerPidController.reset();
+        lateratorPidController = new PIDController(DeAlgifierConstants.kLateratorPVelocity, 0, DeAlgifierConstants.kLateratorDVelocity);
+        grabberPidController = new PIDController(DeAlgifierConstants.kGrabberP, 0, DeAlgifierConstants.kGrabberD);
+
+        lateratorPidController.reset();
+        grabberPidController.reset();
+
+        lateratorMode = LateratorMode.IDLE;
+        grabberMode = GrabberMode.IDLE;
     }
     
     public void periodic(){
         io.updateInputs(inputs);
         Logger.processInputs("DeAlgifier", inputs);
         
         if (DriverStation.isDisabled()) {
-             io.setArmVoltage(0.0);
-             io.setKickerVoltage(0.0);
+             io.setLateratorVoltage(0.0);
+             io.setGrabberBrake(true);
+
+             lateratorMode = LateratorMode.IDLE;
+             grabberMode = GrabberMode.IDLE; //for safety upon reenable
+
+             return;
         } 
-        else {
-            //io.setArmVoltage(armPidController.calculate(inputs.armPositionRad));
-            // io.setKickerVoltage( kickerPidController.getSetpoint() == 0.0 ? 0.0 : -5.0 ); //using the pid controller as a state machine
-            // System.out.println(kickerPidController.getSetpoint());
+
+        switch( lateratorMode ) {
+            case IN -> {
+                if( !inputs.backLimitSwitch)
+                    io.setLateratorVoltage(lateratorPidController.calculate(
+                        inputs.lateratorVelocityRadPerSec, -DeAlgifierConstants.kLateratorVelocityGoalRadPerSec));
+                else io.setLateratorVoltage(0.0);
path: 2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifier.java
user: login: tom-greier
body: You should still call the pid with a 0 velocity so that it resets itself.
created_at: 2025-03-25T10:33:08Z
author_association: CONTRIBUTOR
diff_hunk: @@ -2,54 +2,117 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
 import frc.robot.Constants.DeAlgifierConstants;
 
 import org.littletonrobotics.junction.Logger;
 
+import com.fasterxml.jackson.annotation.JsonTypeInfo.Id;
+
 public class DeAlgifier extends SubsystemBase{
     private DeAlgifierIO io;
     private final DeAlgifierIOInputsAutoLogged inputs = new DeAlgifierIOInputsAutoLogged();
 
-    private final PIDController armPidController;
-    private final PIDController kickerPidController;
+    private final PIDController lateratorPidController;
+    private final PIDController grabberPidController;
+
+    private LateratorMode lateratorMode;
+    private GrabberMode grabberMode;
+
+    enum LateratorMode {
+        IN,
+        OUT,
+        IDLE
+    }
+
+    enum GrabberMode {
+        INTAKE,
+        OUTTAKE,
+        HOLD,
+        IDLE
+    }
+
     
     public DeAlgifier(DeAlgifierIO io){
         this.io = io;
-        this.armPidController = new PIDController(DeAlgifierConstants.ARM_P, 0, DeAlgifierConstants.ARM_D);
-        this.kickerPidController = new PIDController(DeAlgifierConstants.KICKER_P, 0, DeAlgifierConstants.KICKER_D);
-        armPidController.reset();
-        kickerPidController.reset();
+        lateratorPidController = new PIDController(DeAlgifierConstants.kLateratorPVelocity, 0, DeAlgifierConstants.kLateratorDVelocity);
+        grabberPidController = new PIDController(DeAlgifierConstants.kGrabberP, 0, DeAlgifierConstants.kGrabberD);
+
+        lateratorPidController.reset();
+        grabberPidController.reset();
+
+        lateratorMode = LateratorMode.IDLE;
+        grabberMode = GrabberMode.IDLE;
     }
     
     public void periodic(){
         io.updateInputs(inputs);
         Logger.processInputs("DeAlgifier", inputs);
         
         if (DriverStation.isDisabled()) {
-             io.setArmVoltage(0.0);
-             io.setKickerVoltage(0.0);
+             io.setLateratorVoltage(0.0);
+             io.setGrabberBrake(true);
+
+             lateratorMode = LateratorMode.IDLE;
+             grabberMode = GrabberMode.IDLE; //for safety upon reenable
+
+             return;
         } 
-        else {
-            //io.setArmVoltage(armPidController.calculate(inputs.armPositionRad));
-            // io.setKickerVoltage( kickerPidController.getSetpoint() == 0.0 ? 0.0 : -5.0 ); //using the pid controller as a state machine
-            // System.out.println(kickerPidController.getSetpoint());
+
+        switch( lateratorMode ) {
+            case IN -> {
+                if( !inputs.backLimitSwitch)
+                    io.setLateratorVoltage(lateratorPidController.calculate(
+                        inputs.lateratorVelocityRadPerSec, -DeAlgifierConstants.kLateratorVelocityGoalRadPerSec));
+                else io.setLateratorVoltage(0.0);
+            }
+            case OUT -> {
+                if( !inputs.frontLimitSwitch )
+                    io.setLateratorVoltage(lateratorPidController.calculate(
+                        inputs.lateratorVelocityRadPerSec, DeAlgifierConstants.kLateratorVelocityGoalRadPerSec));
+                else io.setLateratorVoltage(0.0);
path: 2025-Janice/src/main/java/frc/robot/subsystems/DeAlgifier/DeAlgifier.java
user: login: tom-greier
body: If you are setting voltage to 0, call the pid with a 0 velocity so that it clears itself. Or, simply reset the pid.
created_at: 2025-03-25T10:33:56Z
author_association: CONTRIBUTOR

PR comments:
user: login: indoorfish1
created_at: 2025-03-23T04:19:20Z
body: I'll have another look at this tomorrow but its getting late.
author_association: COLLABORATOR
user: login: indoorfish1
created_at: 2025-03-25T00:38:41Z
body: > I think you should update the subsystem to put the state machine into the periodic function instead of using the methods to change voltage levels and run the pid. 

Updated with clearer states and state machine.


author_association: COLLABORATOR
user: login: tom-greier
created_at: 2025-03-25T10:35:25Z
body: Much nicer. Only comment is if you are setting voltages directly to 0, then you should also reset the PID so that it is in correct state next time you call calculate.
author_association: CONTRIBUTOR