From a6703c2449b0fb63dddb9a0610143b7a3e86d4c8 Mon Sep 17 00:00:00 2001
From: Zachary Runge Cormack <cormack.zachary@gmail.com>
Date: Tue, 21 Mar 2023 19:49:13 -0400
Subject: [PATCH] basic outline for PID controlled arm. due to design
 constraints, builtin calculator cannot be used; replacement likely will need
 more work.

---
 .../robot/commands/elevator/PidRotate.java    | 139 ++++++++++++------
 .../frc/robot/constants/ArmConstants.java     |  13 +-
 2 files changed, 100 insertions(+), 52 deletions(-)

diff --git a/src/main/java/frc/robot/commands/elevator/PidRotate.java b/src/main/java/frc/robot/commands/elevator/PidRotate.java
index 6672da3..b56174c 100644
--- a/src/main/java/frc/robot/commands/elevator/PidRotate.java
+++ b/src/main/java/frc/robot/commands/elevator/PidRotate.java
@@ -5,54 +5,97 @@
 import frc.robot.constants.ArmConstants;
 import frc.robot.subsystems.Arm;
 
-public class PidRotate extends CommandBase {
-    private final Arm arm;
+public class PIDRotate extends CommandBase {
+  private final Arm arm;
+  
+  private double setpointAngle;
+
+  /**
+   * Autonomously rotates the arm to a set position for scoring.
+   * 
+   * @param arm
+   * @param angle in radians.
+   */
+  public PIDRotate(Arm arm, double setpointAngle) {
+    this.arm = arm;
+    this.setpointAngle = setpointAngle;
+    
+    addRequirements(arm);
+  }
+
+  @Override 
+  public void initialize() {
+    this.arm.resetEncoder();
+  }
+
+  @Override 
+  public void execute(
+    double armAngle,
+    double unixEpochTime
+  ) {
+    double armAngleNew = this.arm.getAngle();
+    double unixEpochTimeNew = System.currentTimeMillis() / 1000;
+    double armAngularVelocity = (
+      chargeStationAngleNew - chargeStationAngle
+    ) / (
+      unixEpochTimeNew - unixEpochTime
+    );
+
+    double armDistanceFromSetpoint = this.setPointAngle - armAngleNew;
+
+    // Built in calculate() method does not support variability
+    // in torque to voltage ratio; too much effort to reengineer.
+    // bootleg solution proposed:
+
+    this.arm.setMotor(
+      (
+        (
+          ArmConstants.ARM_SPEED_FACTOR
+        ) - (
+          armAngularVelocity
+        )
+      ) * (
+        armDistanceFromSetpoint
+      ) + Math.cos(
+        armAngleNew
+      ) * (
+        ArmConstants.ARM_MASS_FACTOR
+      )
+    );
     
-    private double setpointAngle;
-
-    private boolean setpointReached;
-
-    /**
-     * Autonomously rotates the arm to a set position for scoring. 
-     * 
-     * @param arm
-     * @param angle in radians. 
-     */
-    public PidRotate(Arm arm, double setpointAngle) {
-        this.arm = arm; 
-        this.setpointAngle = setpointAngle;
-        this.setpointReached = false; 
-        
-        addRequirements(arm); 
-    }
-
-    @Override 
-    public void initialize() {
-        this.arm.resetEncoder();
-    }
-
-    @Override 
-    public void execute() {
-        if (!setpointReached) {
-            double pid = arm.getPIDController().calculate(
-                this.arm.getAngle(), setpointAngle);
-            this.arm.setMotor(pid);
-            DriverStation.reportWarning(Double.toString(pid), false);
-
-            if (Math.abs(this.arm.getEncoder().getDistance() - setpointAngle) < ArmConstants.ARM_TOLERANCE) {
-                this.setpointReached = true; 
-            }
-        }
-    }
-
-    @Override 
-    public boolean isFinished() {
-        return this.setpointReached;
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        this.arm.resetEncoder(); 
-    }
+    DriverStation.reportWarning(Double.toString(pid), false);
+  }
+
+  public void raise(double raiseDeltaTime) {
+    this.setPointAngle += raiseDeltaTime * ArmConstants.ARM_SPEED_FACTOR;
+
+    if (this.setPointAngle > ArmConstants.ARM_UPPER_BOUND)
+      this.setPointAngle = ArmConstants.ARM_UPPER_BOUND;
+  }
+
+  public void lower(double lowerDeltaTime) {
+    this.setPointAngle -= lowerDeltaTime * ArmConstants.ARM_SENSITIVITY;
+
+    if (this.setpointAngle < ArmConstants.ARM_LOWER_BOUND)
+      this.setPointAngle = ArmConstants.ARM_LOWER_BOUND;
+  }
+
+  @Override 
+  public boolean isFinished() {
+    return Math.pow(
+      (
+        this.arm.getAngle
+      ) / (
+        57
+      ) - (
+        this.setPointAngle
+      ), - 2
+    ) > ArmConstants.ARM_TOLERANCE;
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    this.arm.resetEncoder(); 
+  }
 
 }
diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java
index b4f9148..b8a0807 100644
--- a/src/main/java/frc/robot/constants/ArmConstants.java
+++ b/src/main/java/frc/robot/constants/ArmConstants.java
@@ -1,8 +1,13 @@
 package frc.robot.constants;
 
 public class ArmConstants {
-    public static final double ARM_SPEED_FACTOR = 0.1;
-    public static final double ARM_TOLERANCE = 5 * Math.PI / 180;
-    public static final int LEFT_MOTOR_PORT = 8; 
-    public static final int RIGHT_MOTOR_PORT = 5; 
+    public static final double ARM_SPEED_FACTOR = 0.3;
+    public static final double ARM_TOLERANCE = 120;
+    public static final int LEFT_MOTOR_PORT = 8;
+    public static final int RIGHT_MOTOR_PORT = 5;
+    // todo: calibrate
+    public static final double ARM_UPPER_BOUND = 1.5;
+    public static final double ARM_LOWER_BOUND = -.5;
+    public static final double ARM_MASS_FACTOR = 0.5;
+    public static final double ARM_SENSITIVITY = 0.1;
 }
\ No newline at end of file