feat: more elevator functionality

flamingchickens1540 · Jan 4, 2024 · 2a08f6b · 2a08f6b
1 parent 6872dbf
commit 2a08f6b
Show file tree

Hide file tree

Showing 4 changed files with 27 additions and 3 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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