From 94a0af64d014bb34e03a94d2673c6111ba3c5786 Mon Sep 17 00:00:00 2001
From: dj12ha <90010681+dj12ha@users.noreply.github.com>
Date: Thu, 20 Jan 2022 17:10:07 -0700
Subject: [PATCH] Added input controls

---
 src/main/java/frc4388/robot/Constants.java         |  2 ++
 src/main/java/frc4388/robot/RobotContainer.java    |  7 ++++++-
 .../java/frc4388/robot/subsystems/Climber.java     | 14 +++++++++++++-
 3 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index f514404..227779f 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -85,6 +85,8 @@ public final class Constants {
 
     public static final double MAX_ARM_LENGTH = LOWER_ARM_LENGTH + UPPER_ARM_LENGTH;
     public static final double MIN_ARM_LENGTH = Math.abs(LOWER_ARM_LENGTH + UPPER_ARM_LENGTH);
+    
+    public static final double MOVE_SPEED = 50; // cm per second
   }
   /**
    * The OIConstants class contains the ID for the XBox controllers
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index bc62e9f..ef171b2 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -42,7 +42,7 @@ public class RobotContainer {
 
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
-  private final Climber m_climber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro);
+  private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro);
   /* Controllers */
   private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -59,6 +59,11 @@ public class RobotContainer {
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
             getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
 
+    // moves climber in xy space with two-axis input from the operator controller
+    m_robotClimber.setDefaultCommand(
+        new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(),
+            getOperatorController().getLeftYAxis()), m_robotClimber));
+
     // continually sends updates to the Blinkin LED controller to keep the lights on
     m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
   }
diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java
index 2cd8cdd..74f08c3 100644
--- a/src/main/java/frc4388/robot/subsystems/Climber.java
+++ b/src/main/java/frc4388/robot/subsystems/Climber.java
@@ -15,6 +15,8 @@ public class Climber extends SubsystemBase {
   private WPI_TalonFX m_elbow;
 
   private WPI_PigeonIMU m_gyro;
+
+  private double[] position = {0.d, 0.d};
   
   public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) {
     m_shoulder = shoulder;
@@ -77,7 +79,7 @@ public class Climber extends SubsystemBase {
   public double getRobotTilt() {
     double[] ypr = new double[3];
     m_gyro.getYawPitchRoll(ypr);
-    return ypr[1]; // Pitch
+    return Math.toRadians(ypr[1]); // Pitch
   }
 
   public void setJointAngles(double[] angles) {
@@ -87,4 +89,14 @@ public class Climber extends SubsystemBase {
   public void setJointAngles(double shoulderAngle, double elbowAngle) {
     // Set PIDs
   }
+
+  public void controlWithInput(double xInput, double yInput) {
+    position[0] += xInput * ClimberConstants.MOVE_SPEED;
+    position[1] += yInput * ClimberConstants.MOVE_SPEED;
+
+    double tiltAngle = getRobotTilt();
+
+    double[] jointAngles = getJointAngles(position[0], position[1], tiltAngle);
+    setJointAngles(jointAngles);
+  }
 }