Merged some code

DeepBlueRobotics · Feb 3, 2024 · edc775e · edc775e
1 parent aedeadd
commit edc775e
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 17 deletions.
diff --git 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
@@ -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);
-    }
 }