Proxy sensor implemented

Iron-Panthers · Oct 16, 2023 · a0f77c5 · a0f77c5
1 parent ac36182
commit a0f77c5
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -38,6 +39,8 @@ public class ElevatorSubsystem extends SubsystemBase {
 
   private double filterOutput;
 
+  private DigitalInput proxySensor;
+
   // add soft limits - check 2022 frc code
 
   // stator limits
@@ -55,6 +58,7 @@ public ElevatorSubsystem() {
     heightController = new PIDController(0.0001, 0, 0);
     wristController = new PIDController(0, 0, 0);
     canCoder = new CANCoder(0);
+    proxySensor = new DigitalInput(0);
 
     currentHeight = 0.0;
     targetHeight = 0.0;
@@ -134,7 +138,8 @@ public void periodic() {
     currentHeight = getHeight();
     currentAngle = getCurrentAngleDegrees();
 
-    if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit) {
+    if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit
+        || proxySensor.get() == false) {
       double motorPower = heightController.calculate(currentHeight, targetHeight);
       rightMotor.set(TalonFXControlMode.PercentOutput, motorPower);
     } else {