From 68cbae6754f0ffe3965ee9440fa5a77a9abd4497 Mon Sep 17 00:00:00 2001
From: Naowal Rahman <naowal.ar@gmail.com>
Date: Thu, 1 Feb 2024 19:09:19 -0500
Subject: [PATCH] Add top limit switch to amper (#20)

* add top limit switch to Amper

* add top limit switch to AmperSim

* Update sensor ports

---------

Co-authored-by: BenG49 <ben@goldfisher.com>
---
 src/main/java/com/stuypulse/robot/constants/Ports.java   | 9 +++++----
 .../java/com/stuypulse/robot/subsystems/amper/Amper.java | 1 +
 .../com/stuypulse/robot/subsystems/amper/AmperImpl.java  | 9 ++++++++-
 .../com/stuypulse/robot/subsystems/amper/AmperSim.java   | 7 ++++++-
 4 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java
index a0a1a801..9efef71e 100644
--- 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
index 7bed7f31..9fd2ddee 100644
--- 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
index e5a79709..4a525c2f 100644
--- 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
index e2f47cca..e802ff51 100644
--- 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());