Safe Lift Height (#72)

* Co-authored-by: Owen-Zeng <[email protected]> * Safe Height Constant Change --------- Co-authored-by: Awang70 <[email protected]> Co-authored-by: BenG49 <[email protected]>
StuyPulse · Mar 12, 2024 · 2013ed0 · 2013ed0
1 parent 8ce92ee
commit 2013ed0
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 2 deletions.
diff --git a/simgui.json b/simgui.json
@@ -72,6 +72,12 @@
       "open": true
     },
     "SmartDashboard": {
+      "Amper": {
+        "open": true
+      },
+      "Climber": {
+        "open": true
+      },
       "Shooter": {
         "open": true
       },
@@ -90,6 +96,7 @@
     },
     "Server": {
       "open": true
-    }
+    },
+    "visible": true
   }
 }
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -240,13 +240,14 @@ private void configureOperatorBindings() {
         operator.getDPadLeft()
             .onTrue(new GandalfToAmp())
             .onFalse(new ConveyorStop());
+
 
         operator.getRightButton()
                 .onTrue(new AmperToHeight(Settings.Amper.Lift.AMP_SCORE_HEIGHT));
         operator.getLeftButton()
                 .onTrue(new AmperToHeight(Settings.Amper.Lift.TRAP_SCORE_HEIGHT));
         operator.getBottomButton()
-            .onTrue(new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT));
+                .onTrue(new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT));
 
         // human player attention button
         operator.getRightMenuButton()

diff --git a/src/main/java/com/stuypulse/robot/commands/climber/ClimberDrive.java b/src/main/java/com/stuypulse/robot/commands/climber/ClimberDrive.java
@@ -10,6 +10,7 @@
 import com.stuypulse.stuylib.streams.numbers.IStream;
 
 import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.subsystems.amper.Amper;
 import com.stuypulse.robot.subsystems.climber.Climber;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -34,6 +35,10 @@ public void execute() {
         climber.setVoltageOverride(voltage.get());
 
         SmartDashboard.putNumber("Climber/Gamepad Voltage", voltage.get());
+
+        if (voltage.get() < 0){
+            Amper.getInstance().setTargetHeight(Settings.Amper.Lift.SAFE_CLIMB_HEIGHT);
+        }
     }
 
     @Override

diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -103,6 +103,7 @@ public interface Lift {
             double CARRIAGE_MASS = 10; // kg
 
             double MIN_HEIGHT = 0;
+            double SAFE_CLIMB_HEIGHT = Units.inchesToMeters(2);
             double MAX_HEIGHT = Units.inchesToMeters(22.0); // amp 14.75
 
             double VISUALIZATION_MIN_LENGTH = 0.5;