From 2a08f6b65c19ec91dfce3fb905ed68b2ab4608ec Mon Sep 17 00:00:00 2001
From: Alvin Zhang <41vin2h4n9@gmail.com>
Date: Thu, 4 Jan 2024 09:13:53 -0800
Subject: [PATCH] feat: more elevator functionality

---
 .../team1540/advantagekitdemo/Constants.java   |  3 ++-
 .../subsystems/elevator/Elevator.java          | 18 ++++++++++++++++++
 .../subsystems/elevator/ElevatorIO.java        |  3 +++
 .../subsystems/elevator/ElevatorIOSim.java     |  6 ++++--
 4 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/src/main/java/org/team1540/advantagekitdemo/Constants.java b/src/main/java/org/team1540/advantagekitdemo/Constants.java
index c135f96..c865f38 100644
--- a/src/main/java/org/team1540/advantagekitdemo/Constants.java
+++ b/src/main/java/org/team1540/advantagekitdemo/Constants.java
@@ -72,7 +72,8 @@ public static final class DrivetrainConstants {
     public static final class ElevatorConstants {
         public static final double GEAR_RATIO = 4.21;
         public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(0.75);
-        public static final double MAX_HEIGHT_METERS = Units.inchesToMeters(86);
+        public static final double MAX_HEIGHT_METERS = Units.inchesToMeters(70);
+        public static final double STAGE_1_HEIGHT_METERS = Units.inchesToMeters(38);
         public static final double CARRIAGE_MASS_KG = 20;
 
         public static final double KP = 1;
diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/Elevator.java
index fd69f61..fdc50a0 100644
--- a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/Elevator.java
+++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/Elevator.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
 import org.littletonrobotics.junction.Logger;
 
@@ -40,6 +41,23 @@ public void setPosition(double positionMeters) {
         setGoal(positionMeters);
     }
 
+    public void stop() {
+        if (RobotState.isDisabled()) setVoltage(0);
+        else setPosition(getPositionMeters());
+    }
+
+    public double getPositionMeters() {
+        return getMeasurement();
+    }
+
+    public double getSetpoint() {
+        return getController().getGoal().position;
+    }
+
+    public double getProfilePosition() {
+        return getController().getSetpoint().position;
+    }
+
     @Override
     protected void useOutput(double output, TrapezoidProfile.State setpoint) {
         setVoltage(output + feedforward.calculate(setpoint.velocity));
diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIO.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIO.java
index 7b346df..7c10f78 100644
--- a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIO.java
+++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIO.java
@@ -10,6 +10,9 @@ class ElevatorIOInputs {
         public double positionMeters = 0;
         public double appliedVolts = 0;
         public double currentAmps = 0;
+
+        public boolean upperLimit = false;
+        public boolean lowerLimit = false;
     }
 
     default void updateInputs(ElevatorIOInputs inputs) {}
diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIOSim.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIOSim.java
index 5086917..6611903 100644
--- a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIOSim.java
+++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIOSim.java
@@ -14,7 +14,7 @@ public class ElevatorIOSim implements ElevatorIO {
                     CARRIAGE_MASS_KG,
                     DRUM_RADIUS_METERS,
                     0,
-                    MAX_HEIGHT_METERS/2, // divide by 2 due to cascade rigging
+                    MAX_HEIGHT_METERS,
                     true,
                     0,
                     null
@@ -25,9 +25,11 @@ public class ElevatorIOSim implements ElevatorIO {
     @Override
     public void updateInputs(ElevatorIOInputs inputs) {
         sim.update(0.02);
-        inputs.positionMeters = sim.getPositionMeters() * 2; // multiplied by 2 due to cascade rigging
+        inputs.positionMeters = sim.getPositionMeters();
         inputs.appliedVolts = appliedVolts;
         inputs.currentAmps = sim.getCurrentDrawAmps();
+        inputs.upperLimit = sim.hasHitUpperLimit();
+        inputs.lowerLimit = sim.hasHitLowerLimit();
     }
 
     @Override