From edc775ee3f2ab38ac93819be4442fc172a24beb1 Mon Sep 17 00:00:00 2001
From: Kenneth-Choothakan
 <134165881+Kenneth-Choothakan@users.noreply.github.com>
Date: Sat, 3 Feb 2024 01:18:50 +0000
Subject: [PATCH] Merged some code

---
 .../java/org/carlmontrobotics/Constants.java   |  2 --
 .../org/carlmontrobotics/subsystems/Arm.java   | 18 +++---------------
 2 files changed, 3 insertions(+), 17 deletions(-)

diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
index 19ebb674..83b757f5 100644
--- a/src/main/java/org/carlmontrobotics/Constants.java
+++ b/src/main/java/org/carlmontrobotics/Constants.java
@@ -33,8 +33,6 @@ public static final class Arm {
 		public static final double UPPER_ANGLE = Math.toRadians(70); 
 		public static final double LOWER_ANGLE = Math.toRadians(0);
 		public static final double ARM_DICONT_RAD = (LOWER_ANGLE + UPPER_ANGLE) /2 - Math.PI;
-		//Motor Controllers: pid, FF
-		public static final double[] pidVals = new double[] { 0.1, 0.0, 0.1 };
 		// Feedforward
 		public static final double kS = 0.1;
 		public static final double kG = 0.1;
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index c3b57ccf..4c75e974 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -39,7 +39,7 @@ public class Arm extends SubsystemBase {
     //there is only one arm motor. 
     private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(Constants.Arm.kS, Constants.Arm.kV);
     private final SparkAbsoluteEncoder armEncoder = armMotor1.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
-    private final PIDController armPID = new PIDController(Constants.Arm.pidVals[0], Constants.Arm.pidVals[1], Constants.Arm.pidVals[2]);
+    private final PIDController armPID = new PIDController(Constants.Arm.kP[0], Constants.Arm.kI[0], Constants.Arm.kD[0]);
     public Arm() {
 			//arm 
       /*
@@ -85,6 +85,8 @@ public void driveArm(TrapezoidProfile.State state) {
             (getArmPos() < Constants.Arm.LOWER_ANGLE && state.velocity < 0)) {
             armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0);
     }
+    double volts = armFeedVolts + armPIDVolts;
+    armMotor1.setVoltage(volts);
   }
     public double getArmClampedGoal(double goal) {
       //Find the limits of the arm. Used to move it and ensure that the arm does not move past the amount
@@ -95,18 +97,4 @@ public double getArmClampedGoal(double goal) {
     public void periodic() {
   
 		}
-
-    public void driveArm(TrapezoidProfile.State state){
-        // set voltage to armMotor
-        double kgv = getKg();
-        double armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(state.velocity, 0);
-        double armPIDVolts = armPID.calculate(getArmPos(), state.position);
-        if ((getArmPos() > ARM_UPPER_LIMIT_RAD && state.velocity > 0) || 
-            (getArmPos() < ARM_LOWER_LIMIT_RAD && state.velocity < 0)) {
-              forbFlag = true;  
-            armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0);
-        }
-        double volts = armFeedVolts + armPIDVolts;
-        armMotor.setVoltage(volts);
-    }
 }