From a0f77c55aca71e271154778591eb9602a31cb660 Mon Sep 17 00:00:00 2001
From: Derek <derekchen1815@gmail.com>
Date: Mon, 16 Oct 2023 16:29:50 -0700
Subject: [PATCH] Proxy sensor implemented

---
 src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index a42b610..608e003 100644
--- 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 {