From d88b586f588d87757c6cf9bae6d5ea5ce45086fe Mon Sep 17 00:00:00 2001
From: Miya <miyahcate@gmail.com>
Date: Tue, 19 Sep 2023 16:12:52 -0700
Subject: [PATCH] fixed fixmes in elevator subsystem/commands

---
 src/main/java/frc/robot/Constants.java        |  1 -
 .../commands/ElevatorPositionCommand.java     |  8 ++---
 .../robot/subsystems/ElevatorSubsystem.java   | 29 +++++++------------
 3 files changed, 14 insertions(+), 24 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 7f06783..449962f 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -28,7 +28,6 @@
 import java.nio.file.Path;
 import java.util.List;
 import java.util.Map;
-import java.util.NodeSelectorUtility.ScoreTypeIdentifier;
 import java.util.Set;
 
 @SuppressWarnings("java:S1118")
diff --git a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java
index 63e17f3..09d7b92 100644
--- a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java
+++ b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java
@@ -10,16 +10,16 @@
 public class ElevatorPositionCommand extends CommandBase {
   private final ElevatorSubsystem ElevatorSubsystem;
   private final double targetHeight;
-  private final double desiredAngle; // FIXME rename to "targetAngle"
+  private final double targetAngle; // FIXME rename to "targetAngle"
 
   /** Creates a new ArmPositionCommand. */
   public ElevatorPositionCommand(
-      ElevatorSubsystem elevatorSubsystem, double targetHeight, double desiredAngle) {
+      ElevatorSubsystem elevatorSubsystem, double targetHeight, double targetAngle) {
     // FIXME make a second constructor that requires an ElevatorState instead of two doubles for
     // height and angle
     this.ElevatorSubsystem = elevatorSubsystem;
     this.targetHeight = targetHeight;
-    this.desiredAngle = desiredAngle;
+    this.targetAngle = targetAngle;
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(elevatorSubsystem);
   }
@@ -28,7 +28,7 @@ public ElevatorPositionCommand(
   @Override
   public void initialize() {
     ElevatorSubsystem.setTargetHeight(targetHeight);
-    ElevatorSubsystem.setDesiredAngle(desiredAngle);
+    ElevatorSubsystem.setTargetAngle(targetAngle);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index b296cde..1edf339 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -26,9 +26,8 @@ public class ElevatorSubsystem extends SubsystemBase {
   private TalonFX wristMotor;
   private double currentHeight;
   private double targetHeight;
-  private double desiredAngle;
-  // FIXME change "desiredAngle" to "targetAngle" - keep naming consistent!
-  // FIXME also add a currentAngle variable
+  private double currentAngle;
+  private double targetAngle;
 
   private PIDController heightController;
   private PIDController wristController;
@@ -58,7 +57,8 @@ public ElevatorSubsystem() {
 
     currentHeight = 0.0;
     targetHeight = 0.0;
-    desiredAngle = 0.0;
+    currentAngle = 0.0;
+    targetAngle = 0.0;
 
     right_motor.configFactoryDefault();
     left_motor.configFactoryDefault();
@@ -91,22 +91,12 @@ public static double ticksToHeight(double ticks) {
 
   // FIXME getCurrentTicks is for the elevator, not the wrist
   private double getCurrentTicks() {
-    return wristMotor.getSelectedSensorPosition();
+    return rightMotor.getSelectedSensorPosition();
   }
 
   // FIXME you can just use the cancoder for this, that's what it's for
   public double getCurrentAngleDegrees() {
-    return getCurrentRotation() * Elevator.WRIST_DEGREES;
-  }
-
-  // FIXME unnecessary once you fix getCurrentAngle method
-  public double getCurrentRotation() {
-    return (getCurrentTicks() / Elevator.WRIST_TICKS) * Elevator.WRIST_GEAR_RATIO;
-  }
-
-  // FIXME unnecessary once you fix getCurrentAngle method
-  public static double ticksToAngleDegree(double ticks) {
-    return (ticks / Elevator.WRIST_TICKS) * Elevator.WRIST_GEAR_RATIO * Elevator.WRIST_DEGREES;
+    return canCoder.getAbsolutePosition();
   }
 
   public void setTargetHeight(double targetHeight) {
@@ -114,8 +104,8 @@ public void setTargetHeight(double targetHeight) {
   }
 
   // FIXME rename with consistent naming
-  public void setDesiredAngle(double desiredAngle) {
-    this.desiredAngle = desiredAngle;
+  public void setTargetAngle(double targetAngle) {
+    this.targetAngle = targetAngle;
   }
 
   public double getTargetHeight() {
@@ -131,13 +121,14 @@ public double getHeight() {
   @Override
   public void periodic() {
     currentHeight = getHeight();
+    currentAngle = getCurrentAngleDegrees();
     // FIXME update the current angle using the cancoder
     double motorPower = heightController.calculate(currentHeight, targetHeight);
     right_motor.set(TalonFXControlMode.PercentOutput, motorPower);
     wristMotor.set(
         TalonFXControlMode.PercentOutput,
         MathUtil.clamp(
-            wristController.calculate(canCoder.getAbsolutePosition(), desiredAngle), -0.25, 0.25));
+            wristController.calculate(currentAngle, targetAngle), -0.25, 0.25));
     // FIXME replace all uses of "canCOder.getAbsolutePosition()" with currentAngle (variable)
   }
 }