Add starter class for elevator limits and servo

Author: gitpaperclip

src/main/java/frc/robot/Robot.java

@@ -16,6 +16,7 @@
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.generated.TunerConstants;
@@ -119,6 +120,10 @@ public void robotPeriodic() {
     // Threads.setCurrentThreadPriority(false, 10);
   }
 
+  public static boolean isJITing() {
+    return Timer.getTimestamp() < 45.0;
+  }
+
   /** This function is called once when the robot is disabled. */
   @Override
   public void disabledInit() {}

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

@@ -2,13 +2,13 @@
 
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
 import frc.robot.util.LoggedTunableNumber;
 import java.util.function.BooleanSupplier;
-import java.util.function.Supplier;
 import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
 
 public class Elevator extends SubsystemBase {
 
@@ -49,6 +49,7 @@ public class Elevator extends SubsystemBase {
       new LoggedTunableNumber("Elevator/Climbing/AutoZeroVoltage");
 
   private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
+  private final LockIOInputsAutoLogged lockInputs = new LockIOInputsAutoLogged();
 
   private final Debouncer motorConnectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
   private final Debouncer followerMotorConnectedDebouncer =
@@ -64,18 +65,36 @@ public class Elevator extends SubsystemBase {
   @AutoLogOutput private boolean brakeModeEnabled = true;
 
   private final ElevatorIO io;
+  private final LockIO lockio;
 
-  public Elevator(ElevatorIO io) {
+  public Elevator(ElevatorIO io, LockIO lock) {
     this.io = io;
+    this.lockio = lock;
   }
 
   public void periodic() {
     io.updateInputs(inputs);
-  }
-  //
-  //  private static final LoggedTunableNumber LimitDIO = new
-  // LoggedTunableNumber("Elevator/Limit/LimitValue");
-  //  private static final LoggedTunableNumber ServoPosition = new
-  // LoggedTunableNumber("Elevator/Servo/Position");
+    lockio.updateInputs(lockInputs);
+
+
+    Logger.processInputs("Elevator", inputs);
+    Logger.processInputs("ElevatorSafeties", lockInputs);
 
+    motorDisconnectedAlert.set(
+        !motorConnectedDebouncer.calculate(inputs.data.masterConnected()) && !Robot.isJITing());
+    followerDisconnectedAlert.set(
+        !followerMotorConnectedDebouncer.calculate(inputs.data.followerConnected())
+            && !Robot.isJITing());
+
+    // Update tunable numbers (configure before builds and during tuning)
+    if (kP.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
+      io.setPID(kP.get(), 0.0, kD.get());
+    }
+    //
+    //  private static final LoggedTunableNumber LimitDIO = new
+    // LoggedTunableNumber("Elevator/Limit/LimitValue");
+    //  private static final LoggedTunableNumber ServoPosition = new
+    // LoggedTunableNumber("Elevator/Servo/Position");
+
+  }
 }

src/main/java/frc/robot/subsystems/elevator/LockIO.java

@@ -0,0 +1,27 @@
+package frc.robot.subsystems.elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface LockIO {
+  @AutoLog
+  class LockIOInputs {
+    public LockIOData data =
+        new LockIOData(
+            false,
+            0.0,
+            0.0);
+  }
+
+  record LockIOData(
+      boolean limit,
+      double servoAngle,
+      double servoPosition
+      ) {}
+
+  default void update(LockIOInputs inputs) {}
+
+  default void setServoAngle(double angleDeg) {}
+
+  default void setServo(double position) {}
+
+}

src/main/java/frc/robot/subsystems/elevator/LockIOServoAndMagswitch.java

@@ -0,0 +1,41 @@
+package frc.robot.subsystems.elevator;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Servo;
+
+public class LockIOServoAndMagswitch implements LockIO {
+
+    private final DigitalInput limit;
+    private final Servo servo;
+
+    private final double servoPosition;
+    private final double servoAngle;
+    private final boolean limitValue;
+
+    public LockIOServoAndMagswitch() {
+        limit = new DigitalInput(0); // Adjust the channel as needed
+        servo = new Servo(1); // Adjust the channel as needed
+        servoPosition = servo.getPosition(); // Default servo position
+        servoAngle = servo.getAngle(); // Default servo position
+        limitValue = limit.get(); // Default limit value
+    }
+
+    @Override
+    public void update(LockIOInputs inputs) {
+        inputs.data = new LockIOData(
+            limit.get(),
+            servo.get(), 
+            servo.getAngle()
+            );
+    }
+
+    @Override
+    public void setServoAngle(double angleDeg) {
+        servo.setAngle(angleDeg);
+    }
+
+    @Override
+    public void setServo(double position) {
+        servo.set(position);
+    }
+}