Add elevator to advantatekit

src/main/java/frc4388/robot/RobotContainer.java

@@ -53,10 +53,10 @@ import frc4388.robot.constants.Constants.SimConstants.Mode;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 
-import frc4388.robot.subsystems.Elevator;
 // Subsystems
 import frc4388.robot.subsystems.LED;
-import frc4388.robot.subsystems.Elevator.CoordinationState;
+import frc4388.robot.subsystems.elevator.Elevator;
+import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
 import frc4388.robot.subsystems.lidar.LiDAR;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
@@ -80,7 +80,7 @@ public class RobotContainer {
     /* Subsystems */
     public final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
-    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED);
+    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 

src/main/java/frc4388/robot/RobotMap.java

@@ -21,14 +21,16 @@ import frc4388.robot.constants.Constants.ElevatorConstants;
 import frc4388.robot.constants.Constants.LiDARConstants;
 import frc4388.robot.constants.Constants.SimConstants;
 import frc4388.robot.constants.Constants.VisionConstants;
+import frc4388.robot.subsystems.elevator.ElevatorIO;
+import frc4388.robot.subsystems.elevator.ElevatorReal;
 import frc4388.robot.subsystems.lidar.LiDAR;
 import frc4388.robot.subsystems.lidar.LidarIO;
-import frc4388.robot.subsystems.lidar.LidarLiteV2;
+import frc4388.robot.subsystems.lidar.LidarReal;
 import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
 import frc4388.robot.subsystems.swerve.SwerveIO;
-import frc4388.robot.subsystems.swerve.SwervePhoenix;
+import frc4388.robot.subsystems.swerve.SwerveReal;
 import frc4388.robot.subsystems.vision.VisionIO;
-import frc4388.robot.subsystems.vision.VisionPhotonvision;
+import frc4388.robot.subsystems.vision.VisionReal;
 import frc4388.utility.status.FaultCANCoder;
 import frc4388.utility.status.FaultPhotonCamera;
 import frc4388.utility.status.FaultPidgeon2;
@@ -58,13 +60,7 @@ public class RobotMap {
     public final SwerveIO swerveDrivetrain;
 
     /* Elevator Subsystem */
-    public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
+    public final ElevatorIO elevatorIO;
-    public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
-    
-
-    public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
-    public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
-    public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
 
     public RobotMap(SimConstants.Mode mode) {
         switch (mode) {
@@ -73,28 +69,40 @@ public class RobotMap {
                 PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
                 PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
 
-                leftCamera =  new VisionPhotonvision(leftCameraReal, VisionConstants.LEFT_CAMERA_POS);                ;
+                leftCamera =  new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS);                ;
-                rightCamera = new VisionPhotonvision(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
+                rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
 
                 FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
                 FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
 
                 // Configure LiDAR
-                reefLidar = new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
+                reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
-                reverseLidar = new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
+                reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
 
                 // Configure swerve drive train
-
                 SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new, 
                     SwerveDriveConstants.DrivetrainConstants, 
                     SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
                     SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
                 );
 
-                swerveDrivetrain = new SwervePhoenix(swerveDrivetrainReal);
+                swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
+
+                // Configure elevator
+
+                TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
+                TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
+                
+
+                DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
+                DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
+                DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
+
+                elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
 
 
 
+                // Fault
                 FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
 
                 FaultTalonFX.addDevice(elevator, "Elevator");
@@ -122,6 +130,7 @@ public class RobotMap {
                 reefLidar = new LidarIO() {};
                 reverseLidar = new LidarIO() {};
                 swerveDrivetrain = new SwerveIO() {};
+                elevatorIO = new ElevatorIO() {};
                 break;
         }
     }

src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java

@@ -5,7 +5,7 @@
 package frc4388.robot.commands.wait;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Elevator;
+import frc4388.robot.subsystems.elevator.Elevator;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class waitElevatorRefrence extends Command {

src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java

@@ -5,7 +5,7 @@
 package frc4388.robot.commands.wait;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Elevator;
+import frc4388.robot.subsystems.elevator.Elevator;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class waitEndefectorRefrence extends Command {

src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java

@@ -5,7 +5,7 @@
 package frc4388.robot.commands.wait;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Elevator;
+import frc4388.robot.subsystems.elevator.Elevator;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class waitFeedCoral extends Command {

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2025RidgeScape";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 172;
+  public static final int GIT_REVISION = 173;
-  public static final String GIT_SHA = "8e34bfe35427e1fb86ab2af21dd706d387bf4874";
+  public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513";
-  public static final String GIT_DATE = "2025-07-15 13:42:25 MDT";
+  public static final String GIT_DATE = "2025-07-17 09:15:19 MDT";
   public static final String GIT_BRANCH = "advantagekit";
-  public static final String BUILD_DATE = "2025-07-16 16:20:33 MDT";
+  public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT";
-  public static final long BUILD_UNIX_TIME = 1752704433011L;
+  public static final long BUILD_UNIX_TIME = 1752774931371L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/subsystems/DiffDrive.java

@@ -1,107 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.subsystems;
-
-import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.hardware.Pigeon2;
-import com.ctre.phoenix6.hardware.TalonFX;
-
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
-// import frc4388.utility.RobotGyro;
-import frc4388.utility.compute.RobotTime;
-import frc4388.utility.status.Status;
-import frc4388.utility.status.FaultReporter;
-import frc4388.utility.status.Queryable;
-
-/**
- * Add your docs here.
- */
-public class DiffDrive extends SubsystemBase implements Queryable {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-  private RobotTime m_robotTime = RobotTime.getInstance();
-
-  private TalonFX m_leftFrontMotor;
-  private TalonFX m_rightFrontMotor;
-  private TalonFX m_leftBackMotor;
-  private TalonFX m_rightBackMotor;
-  private DifferentialDrive m_driveTrain;
-  // private Pigeon2 m_gyro;
-
-  /**
-   * Add your docs here.
-   */
-  public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
-      TalonFX rightBackMotor, DifferentialDrive driveTrain, Pigeon2 gyro) {
-    
-    FaultReporter.register(this);
-
-    m_leftFrontMotor = leftFrontMotor;
-    m_rightFrontMotor = rightFrontMotor;
-    m_leftBackMotor = leftBackMotor;
-    m_rightBackMotor = rightBackMotor;
-    m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
-    m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
-    m_driveTrain = driveTrain;
-    // m_gyro = gyro;
-  }
-
-  @Override
-  public void periodic() {
-    if (m_robotTime.m_frameNumber % SwerveDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
-      updateSmartDashboard();
-    }
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(move, steer);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void tankDriveWithInput(double leftMove, double rightMove) {
-    m_leftFrontMotor.set(leftMove);
-    m_rightFrontMotor.set(rightMove);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  private void updateSmartDashboard() {
-
-    // Examples of the functionality of RobotGyro
-    // SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
-    // SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
-    // SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
-    //SmartDashboard.putData(m_gyro);
-  }
-
-
-  @Override
-  public String getName() {
-      return "Diff Drive";
-  }
-
-  // @Override
-  // public void queryStatus() {
-  //     // TODO: Add Stuff
-  // }
-
-  @Override
-  public Status diagnosticStatus() {
-      // Log("Diagnostic info for this has not been inplemented!"); //TODO
-      return new Status();
-  }
-}

src/main/java/frc4388/robot/subsystems/elevator/Elevator.java

@@ -2,7 +2,10 @@
 // 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 frc4388.robot.subsystems;
+package frc4388.robot.subsystems.elevator;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
 
 import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -14,38 +17,31 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants.AutoConstants;
 import frc4388.robot.constants.Constants.ElevatorConstants;
 import frc4388.robot.constants.Constants.LEDConstants;
+import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.elevator.ElevatorIO.ElevatorState;
 import frc4388.utility.status.Status;
 import frc4388.utility.status.FaultReporter;
 import frc4388.utility.status.Queryable;
 import frc4388.utility.status.Status.ReportLevel;
 
 public class Elevator extends SubsystemBase implements Queryable {
+  ElevatorIO io;
+  ElevatorStateAutoLogged state = new ElevatorStateAutoLogged();
+
   /** Creates a new Elevator. */
-  private TalonFX elevatorMotor;
-  private TalonFX endeffectorMotor;
   private LED led;
 
-  // @AutoLog
+  @SuppressWarnings("unused")
-  // private class ElevatorState {
+  public long wait = 0;
-    @SuppressWarnings("unused")
+  public long maxWait = 1000;
-    public long wait = 0;
-    public long maxWait = 1000;
 
-    public double elevatorRefrence = 0;
+  public boolean elevatorManualStop = true;
-    public double endeffectorRefrence = 0;
+  public boolean endefectorManualStop = true;
 
-    public boolean elevatorManualStop = true;
+  public boolean disableAutoIntake = false;
-    public boolean endefectorManualStop = true;
 
-    public boolean disableAutoIntake = false;
+  public boolean seededZeroEndefector = false;
-
+  public boolean seededZeroElevator = false;
-    public boolean seededZeroEndefector = false;
-    public boolean seededZeroElevator = false;
-
-    public DigitalInput basinBeamBreak;
-    public DigitalInput endeffectorLimitSwitch;
-    public DigitalInput intakeIR;
-  // }
 
   // private ElevatorState state = new ElevatorState();
 
@@ -69,43 +65,15 @@ public class Elevator extends SubsystemBase implements Queryable {
   private CoordinationState currentState;
 
   // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
-  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
+  public Elevator(ElevatorIO io, LED led) {
-    elevatorMotor = elevatorTalonFX;
+    this.io = io;
-    endeffectorMotor = endeffectorTalonFX;
     this.led = led;
 
-    this.basinBeamBreak = basinLimitSwitch;
-    this.endeffectorLimitSwitch = endeffectorLimitSwitch;
-    this.intakeIR = intakeDigitalInput;
-
-    elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
-    endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
-    
-    elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
-    endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
     currentState = CoordinationState.Ready;
 
     FaultReporter.register(this);
   }
 
-  //PID methods
-
-  private void PIDPosition(TalonFX motor, double position) {
-    if (motor == elevatorMotor) elevatorRefrence = position;
-    else endeffectorRefrence = position;
-
-    var request = new PositionDutyCycle(position);
-    motor.setControl(request);
-  }
-
-  public void elevatorStop() {
-    elevatorMotor.set(0);
-  }
-
-  public void endeffectorStop() {
-    endeffectorMotor.set(0);
-  }
-
 
   public void transitionState(CoordinationState state) {
     // elevatorMotor.enable();
@@ -115,98 +83,98 @@ public class Elevator extends SubsystemBase implements Queryable {
     switch (currentState) {
       case Waiting: {
         wait = System.currentTimeMillis() + maxWait;
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_ELEVATOR);
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
         led.setMode(LEDConstants.WAITING_PATTERN);
         break;
       }
 
       case WatingBeamTripped: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
+        io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         led.setMode(LEDConstants.DOWN_PATTERN);
         break;
       }
 
       case Ready: {
-        PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
+        io.elevatorToPosition(ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         led.setMode(LEDConstants.DOWN_PATTERN);
         break;
       }
 
       case Hovering: {
-        PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR);
+        io.elevatorToPosition(ElevatorConstants.HOVERING_POSITION_ELEVATOR);
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         led.setMode(LEDConstants.READY_PATTERN);
         break;
       }
 
       case L2Score: {
-        PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case L2ScoreLeave: {
-        PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
       
       case PrimedFour: {
-        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
+        io.endeffectorToPosition(ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case ScoringFour: {
-        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case PrimedThree: {
-        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
+        io.endeffectorToPosition(ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
       
       case ScoringThree: {
-        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case BallRemoverL2Primed: {
-        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
+        io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case BallRemoverL2Go: {
-        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case BallRemoverL3Primed: {
-        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
+        io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
         break;
       }
 
       case BallRemoverL3Go: {
-        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
@@ -223,27 +191,25 @@ public class Elevator extends SubsystemBase implements Queryable {
   }
 
   public boolean elevatorAtReference() {
-    // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
+    double diffrence = state.elevatorRefrence - state.elevatorPosition;
-    double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
-    double diffrence = elevatorRefrence - elevatorPosition;
 
     boolean headedUp = diffrence < 0;
-    boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
-    boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
 
-    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
+    return (Math.abs(diffrence) <= 0.5 
+      || (state.elevatorReverseLimit && headedUp) 
+      || (state.elevatorForwardLimit && !headedUp)
+    );
   }
 
   public boolean endeffectorAtReference() {
-    // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
+    double diffrence = state.endeffectorRefrence - state.endeffectorPosition;
-    double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
-    double diffrence = endeffectorRefrence - endeffectorPosition;
 
     boolean headedUp = diffrence < 0;
-    boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
-    boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
 
-    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
+    return (Math.abs(diffrence) <= 0.5 
+      || (state.elevatorReverseLimit && headedUp) 
+      || (state.endeffectorForwardLimit && !headedUp)
+    );
   }
   // public void driveElevatorStick(Translation2d stick) {
   //   if (stick.getNorm() > 0.05) {
@@ -252,11 +218,11 @@ public class Elevator extends SubsystemBase implements Queryable {
   // }
 
   public boolean getEndeffectorLimit() {
-    return endeffectorLimitSwitch.get();
+    return state.endeffectorLimitSwitch;
   }
 
   private void periodicWaiting() {
-    if (!basinBeamBreak.get()) 
+    if (!state.basinBeamBreak) 
       transitionState(CoordinationState.Ready);
     // if(!endeffectorLimitSwitch.get())
     //   transitionState(CoordinationState.Hovering);
@@ -268,39 +234,39 @@ public class Elevator extends SubsystemBase implements Queryable {
   // }
   
   private void periodicReady() {
-    if (elevatorAtReference() && !endeffectorLimitSwitch.get())
+    if (elevatorAtReference() && !state.endeffectorLimitSwitch)
       transitionState(CoordinationState.Hovering);
-    if(elevatorAtReference() && endeffectorLimitSwitch.get())
+    if(elevatorAtReference() && state.endeffectorLimitSwitch)
       transitionState(CoordinationState.Hovering);
   }
 
   @SuppressWarnings("unused")
   private void periodicScoring() {
-    if (!endeffectorLimitSwitch.get()) 
+    if (!state.endeffectorLimitSwitch) 
       transitionState(CoordinationState.Waiting);
   }
 
   public void manualElevatorVel(double velocity) {
     if (Math.abs(velocity) > 0.1) {
-      elevatorMotor.set(velocity);
+      io.elevatorToVelocity(velocity);
       elevatorManualStop = false;
       return;
     }
     if (!elevatorManualStop) {
       elevatorManualStop = true;
-      elevatorMotor.set(0);
+      io.elevatorToVelocity(0);
     }
   }
 
   public void manualEndeffectorVel(double velocity) {
     if (Math.abs(velocity) > 0.1) {
-      endeffectorMotor.set(velocity);
+      io.endeffectorToVelocity(velocity);
       endefectorManualStop = false;
       return;
     }
     if (!endefectorManualStop) {
       endefectorManualStop = true;
-      endeffectorMotor.set(0);
+      io.endeffectorToVelocity(0);
     }
   }
 
@@ -320,18 +286,21 @@ public class Elevator extends SubsystemBase implements Queryable {
     // This method will be called once per scheduler run
     // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
     // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
-    SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
+    // SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
-    SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
+    // SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
-    SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
+    // SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
-    SmartDashboard.putString("State", currentState.toString());
+    // SmartDashboard.putString("State", currentState.toString());
 
-    if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) {
+    io.updateInputs(state);
-      endeffectorMotor.setPosition(0);
+    Logger.processInputs("Elevator", state);
+    
+    if (!seededZeroEndefector && state.endeffectorForwardLimit) {
+      io.endeffectorToPosition(0);
       seededZeroEndefector = !seededZeroEndefector;
     }
 
-    if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) {
+    if (!seededZeroElevator && state.elevatorReverseLimit) {
-      elevatorMotor.setPosition(0);
+      io.endeffectorToPosition(0);
       seededZeroElevator = !seededZeroElevator;
     }
     
@@ -345,7 +314,7 @@ public class Elevator extends SubsystemBase implements Queryable {
       periodicReady();
     }
 
-    if(!intakeIR.get()){
+    if(!state.intakeIR){
       led.setMode(LEDConstants.DOWN_PATTERN);
     }
 
@@ -355,6 +324,11 @@ public class Elevator extends SubsystemBase implements Queryable {
     // }
   }
 
+  @AutoLogOutput(key="Elevator/state")
+  public String getState() {
+    return currentState.toString();
+  }
+
   public boolean isL4Primed() {
     return currentState == CoordinationState.PrimedFour;
   }
@@ -364,16 +338,24 @@ public class Elevator extends SubsystemBase implements Queryable {
   }
 
   public boolean hasCoral() {
-    return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
+    return elevatorAtReference() && currentState == CoordinationState.Hovering || !state.endeffectorLimitSwitch;
+  }
+
+  public void elevatorStop() {
+    io.elevatorToVelocity(0);
+  }
+
+  public void endeffectorStop() {
+    io.endeffectorToVelocity(0);
   }
 
   public boolean readyToMove() {
-    return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get();
+    return !state.intakeIR || hasCoral() || !state.endeffectorLimitSwitch;
     // return hasCoral();
   }
 
   public void armShuffle(){
-    if(!basinBeamBreak.get()){
+    if(!state.basinBeamBreak){
       //shuffle the coral with the arm until coral hits beam break
     }
   }

src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java

@@ -0,0 +1,34 @@
+package frc4388.robot.subsystems.elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc4388.robot.subsystems.lidar.LidarIO.LidarState;
+
+public interface ElevatorIO {
+    @AutoLog
+    public class ElevatorState {
+        public double elevatorRefrence;
+        public double elevatorPosition;
+        public boolean elevatorForwardLimit;
+        public boolean elevatorReverseLimit;
+
+        public double endeffectorRefrence;
+        public double endeffectorPosition;
+        public boolean endeffectorForwardLimit;
+        public boolean endeffectorReverseLimit;
+
+        
+        public boolean basinBeamBreak;
+        public boolean endeffectorLimitSwitch;
+        public boolean intakeIR;
+
+    }
+
+    public default void elevatorToPosition(double position) {}
+    public default void endeffectorToPosition(double position) {}
+    public default void elevatorToVelocity(double velocity) {}
+    public default void endeffectorToVelocity(double velocity) {}
+
+    public default void updateInputs(ElevatorState state) {}
+}

src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java

@@ -0,0 +1,80 @@
+package frc4388.robot.subsystems.elevator;
+
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc4388.robot.constants.Constants.ElevatorConstants;
+
+public class ElevatorReal implements ElevatorIO {
+    TalonFX elevatorMotor;
+    TalonFX endeffectorMotor;
+    
+    DigitalInput basinLimitSwitch;
+    DigitalInput endeffectorLimitSwitch;
+    DigitalInput intakeIR;
+
+    
+    double elevatorRefrence = 0;
+    double endeffectorRefrence = 0;
+
+    public ElevatorReal(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeIR) {
+        this.elevatorMotor = elevatorTalonFX;
+        this.endeffectorMotor = endeffectorTalonFX;
+
+        this.basinLimitSwitch = basinLimitSwitch;
+        this.endeffectorLimitSwitch = endeffectorLimitSwitch;
+        this.intakeIR = intakeIR;
+
+        
+        elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
+        endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
+        
+        elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
+        endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
+    }
+
+    @Override
+    public void updateInputs(ElevatorState state) {
+        state.elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
+        state.elevatorRefrence = elevatorRefrence;
+        state.elevatorForwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
+        state.elevatorReverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+        state.endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
+        state.endeffectorRefrence = endeffectorRefrence;
+        state.endeffectorForwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
+        state.endeffectorReverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+
+        state.basinBeamBreak = basinLimitSwitch.get();
+        state.endeffectorLimitSwitch = endeffectorLimitSwitch.get();
+        state.intakeIR = intakeIR.get();
+    }
+
+    @Override
+    public void elevatorToPosition(double position) {
+        elevatorRefrence = position; 
+        var request = new PositionDutyCycle(position);
+        elevatorMotor.setControl(request);
+    }
+
+    @Override
+    public void endeffectorToPosition(double position) {
+        endeffectorRefrence = position;
+        var request = new PositionDutyCycle(position);
+        endeffectorMotor.setControl(request);
+    }
+
+    @Override
+    public void elevatorToVelocity(double velocity) {
+        elevatorMotor.set(velocity);
+    }
+
+    
+    @Override
+    public void endeffectorToVelocity(double velocity) {
+        endeffectorMotor.set(velocity);
+    }
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java

@@ -4,12 +4,12 @@ import edu.wpi.first.wpilibj.Counter;
 import frc4388.robot.constants.Constants.LiDARConstants;
 
 // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
-public class LidarLiteV2 implements LidarIO {
+public class LidarReal implements LidarIO {
 
     
     private Counter LidarPWM;
 
-    public LidarLiteV2(int port) {
+    public LidarReal(int port) {
         LidarPWM = new Counter(port);
         LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
         LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -4,16 +4,9 @@
 
 package frc4388.robot.subsystems.swerve;
 
-import java.util.Optional;
+import org.littletonrobotics.junction.AutoLogOutput;
-
 import org.littletonrobotics.junction.Logger;
 
-import com.ctre.phoenix6.Utils;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.swerve.SwerveDrivetrain;
-import com.ctre.phoenix6.swerve.SwerveModule;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -22,7 +15,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants.AutoConstants;
-import frc4388.robot.subsystems.swerve.SwerveIO.SwerveState;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.compute.TimesNegativeOne;
 import frc4388.utility.status.Status;
@@ -56,7 +48,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
     public double lastOdomSpeed;
 
-    public Pose2d initalPose2d = null;
+    public Pose2d initalPose2d = new Pose2d();
 
 
     public double rotTarget = 0.0;
@@ -353,9 +345,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         SmartDashboard.putNumber("RotTartget", rotTarget);
 
         io.updateInputs(state);
+        Logger.processInputs("SwerveDrive", state);
         
         vision.setLastOdomPose(state.currentPose);
-        setLastOdomSpeed(state.currentPose, state.lastPose, state.frequency);
+        setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
 
         if (vision.isTag()) {
             Pose2d pose = vision.getPose2d();
@@ -448,6 +441,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
             lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq;
         }
     }
+
+    @AutoLogOutput(key="SwerveDrive/speed ")
+    public double getOdometrySpeed() {
+        return lastOdomSpeed;
+    }
     
 
 

src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java

@@ -16,7 +16,7 @@ public interface SwerveIO {
         public Pose2d currentPose = null;
         public Pose2d lastPose = null;
         public ChassisSpeeds speeds = null;
-        public double frequency = 1;
+        public double odometryRate = 1;
     }
 
     public default void setControl(SwerveRequest ctrl) {}

src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java

@@ -10,15 +10,13 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain;
 import com.ctre.phoenix6.swerve.SwerveModule;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import frc4388.robot.subsystems.lidar.LidarIO.LidarState;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
 
-public class SwervePhoenix implements SwerveIO {
+public class SwerveReal implements SwerveIO {
     SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
 
-    public SwervePhoenix(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
+    public SwerveReal(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
         this.swerveDriveTrain = swerveDriveTrain;
         swerveDriveTrain.getOdometryFrequency();
     }
@@ -26,9 +24,9 @@ public class SwervePhoenix implements SwerveIO {
     @Override
     public void updateInputs(SwerveState state) {
         double time = Vision.getTime();
-        state.frequency = swerveDriveTrain.getOdometryFrequency();
+        state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency();
         state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null);
-        state.lastPose = swerveDriveTrain.samplePoseAt(time - state.frequency).orElse(null);
+        state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null);
         state.speeds = swerveDriveTrain.getState().Speeds;
     }
 

src/main/java/frc4388/robot/subsystems/vision/VisionReal.java

@@ -11,7 +11,7 @@ import org.photonvision.targeting.PhotonPipelineResult;
 import frc4388.robot.constants.Constants.FieldConstants;
 import frc4388.robot.constants.Constants.VisionConstants;
 
-public class VisionPhotonvision implements VisionIO {
+public class VisionReal implements VisionIO {
     // private PhotonCamera[] cameras;
     // private PhotonPoseEstimator[] estimators;
     
@@ -21,7 +21,7 @@ public class VisionPhotonvision implements VisionIO {
     // public List<EstimatedRobotPose> poses = new ArrayList<>();
 
     
-    public VisionPhotonvision(PhotonCamera camera, Transform3d position){
+    public VisionReal(PhotonCamera camera, Transform3d position){
         this.camera = camera;
         estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position);
         estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);