Merge branch 'main' of github.com:StuyPulse/BigWang

StuyPulse · Feb 2, 2024 · 91e3681 · 91e3681
2 parents 32923ab + 68cbae6
commit 91e3681
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 6 deletions.
diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java
@@ -17,10 +17,10 @@ public interface Climber {
         int LEFT_MOTOR = 60;
         int RIGHT_MOTOR = 61;
 
-        int TOP_RIGHT_LIMIT = 8;
-        int TOP_LEFT_LIMIT = 7;
-        int BOTTOM_RIGHT_LIMIT = 6;
-        int BOTTOM_LEFT_LIMIT = 5;        
+        int TOP_RIGHT_LIMIT = 9;
+        int TOP_LEFT_LIMIT = 8;
+        int BOTTOM_RIGHT_LIMIT = 7;
+        int BOTTOM_LEFT_LIMIT = 6;        
     }
 
     public interface Amper {
@@ -29,6 +29,7 @@ public interface Amper {
 
         int ALIGNED_BUMP_SWITCH = 3;
         int LIFT_BOTTOM_LIMIT = 4;
+        int LIFT_TOP_LIMIT = 5;
         int AMP_IR = 2;
     }
 

diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java
@@ -82,6 +82,7 @@ public final double getTargetHeight() {
     public abstract void stopRoller();
 
     public abstract boolean liftAtBottom();
+    public abstract boolean liftAtTop();
     public abstract double getLiftHeight();
     public abstract void stopLift();
 

diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java
@@ -24,6 +24,7 @@ public class AmperImpl extends Amper {
 
     private final DigitalInput alignedSwitch;
     private final DigitalInput minSwitch;
+    private final DigitalInput maxSwitch;
     private final DigitalInput ampIRSensor;
 
     private final Controller controller;
@@ -38,6 +39,7 @@ public AmperImpl() {
 
         alignedSwitch = new DigitalInput(Ports.Amper.ALIGNED_BUMP_SWITCH);
         minSwitch = new DigitalInput(Ports.Amper.LIFT_BOTTOM_LIMIT);
+        maxSwitch = new DigitalInput(Ports.Amper.LIFT_TOP_LIMIT);
         ampIRSensor = new DigitalInput(Ports.Amper.AMP_IR);
 
         Motors.Amper.LIFT_MOTOR.configure(liftMotor);
@@ -61,6 +63,11 @@ public boolean liftAtBottom() {
         return !minSwitch.get();
     }
 
+    @Override
+    public boolean liftAtTop() {
+        return !maxSwitch.get();
+    }
+
     @Override
     public double getLiftHeight() {
         return liftEncoder.getPosition();
@@ -97,7 +104,7 @@ public void periodic() {
 
         controller.update(getTargetHeight(), getLiftHeight());
 
-        if (liftAtBottom() && controller.getOutput() < 0) {
+        if (liftAtBottom() && controller.getOutput() < 0 || liftAtTop() && controller.getOutput() > 0) {
             stopLift();
         } else {
             liftMotor.setVoltage(controller.getOutput());

diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java
@@ -60,6 +60,11 @@ public boolean liftAtBottom() {
         return sim.hasHitLowerLimit();
     }
 
+    @Override
+    public boolean liftAtTop() {
+        return sim.hasHitUpperLimit();
+    }
+
     @Override
     public boolean touchingAmp() {
         return false;
@@ -84,7 +89,7 @@ public void periodic() {
 
         controller.update(getTargetHeight(), getLiftHeight());
 
-        if (liftAtBottom() && controller.getOutput() < 0) {
+        if (liftAtBottom() && controller.getOutput() < 0 || liftAtTop() && controller.getOutput() > 0) {
             stopLift();
         } else {
             sim.setInputVoltage(controller.getOutput());