From 38387e317346d4892cab22ca7460bcc08053b0e4 Mon Sep 17 00:00:00 2001
From: 66945 <66945@psdschools.org>
Date: Mon, 24 Jan 2022 19:09:20 -0700
Subject: [PATCH] Added limits

---
 src/main/java/frc4388/robot/Constants.java          | 5 +++++
 src/main/java/frc4388/robot/subsystems/Climber.java | 6 ++++++
 2 files changed, 11 insertions(+)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 4076a30..c6b23bf 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -93,6 +93,11 @@ public final class Constants {
     public static final double SHOULDER_RESTING_ANGLE = 0;
     public static final double ELBOW_RESTING_ANGLE = 0;
 
+    public static final double SHOULDER_MAX_ANGLE = 135;
+    public static final double ELBOW_MAX_ANGLE = 180;
+
+    public static final double GEAR_BOX_RATIO = 144.d * 60.d / 24.d;
+
     // PID Constants
     public static final int SHOULDER_SLOT_IDX = 0;
     public static final int SHOULDER_PID_LOOP_IDX = 1;
diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java
index 7f9b883..374f44f 100644
--- a/src/main/java/frc4388/robot/subsystems/Climber.java
+++ b/src/main/java/frc4388/robot/subsystems/Climber.java
@@ -165,12 +165,18 @@ public class Climber extends SubsystemBase {
   }
 
   public void setJointAngles(double shoulderAngle, double elbowAngle) {
+    shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
+    elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
+
     // Convert radians to ticks
     System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
 
     double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
     double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
 
+    shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO;
+    elbowAngle *= ClimberConstants.GEAR_BOX_RATIO;
+
     shoulderPosition += m_shoulderOffset;
     elbowPosition += m_elbowOffset;