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

---
 src/main/java/frc4388/robot/subsystems/Climber.java | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java
index 374f44f..0e55952 100644
--- a/src/main/java/frc4388/robot/subsystems/Climber.java
+++ b/src/main/java/frc4388/robot/subsystems/Climber.java
@@ -165,6 +165,9 @@ public class Climber extends SubsystemBase {
   }
 
   public void setJointAngles(double shoulderAngle, double elbowAngle) {
+    shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
+    elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
+
     shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
     elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;