resolved some comments B)

DeepBlueRobotics · Mar 1, 2024 · 1957d0c · 1957d0c
1 parent 9092c78
commit 1957d0c
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 21 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -37,9 +37,7 @@ public static final class Arm {
 		//if needed
 		public static final double UPPER_ANGLE_LIMIT = Math.toRadians(70); 
 		public static final double LOWER_ANGLE_LIMIT = Math.toRadians(0);
-		public static final double ARM_DICONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI;
-		//Motor Controllers: pid, FF
-		public static final double[] pidVals = new double[] { 0.1, 0.0, 0.1 };
+		public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI;
 		// Feedforward
 		public static final double kS = 0.1;
 		public static final double kG = 0.1;

diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -5,7 +5,7 @@
 package org.carlmontrobotics;
 //199 files
 // import org.carlmontrobotics.subsystems.*;
-// import org.carlmontrobotics.commands.*;
+import org.carlmontrobotics.commands.*;
 import static org.carlmontrobotics.Constants.OI;
 
 import org.carlmontrobotics.Constants.OI;
@@ -50,8 +50,6 @@ public RobotContainer() {
 
   private void setDefaultCommands() {
 
-    //arm.setDefaultCommand(new InstantCommand(() -> {arm.driveArm());
-
     // drivetrain.setDefaultCommand(new TeleopDrive(
     //   drivetrain,
     //   () -> ProcessedAxisValue(driverController, Axis.kLeftY)),

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -42,7 +42,6 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 
-
 public class Arm extends SubsystemBase {
     private final CANSparkMax armMotor1 = MotorControllerFactory.createSparkMax(LEFT_MOTOR_PORT,MotorConfig.NEO);
     //private final CANSparkMax armMotor2 = MotorControllerFactory.createSparkMax(Constants.Arm.RIGHT_MOTOR_PORT,MotorConfig.NEO);
@@ -103,13 +102,14 @@ public TrapezoidProfile.State calculateCustomSetPoint(double goalSeconds, Trapez
 
     public void COMBINE_PID_FF_TRAPEZOID(TrapezoidProfile.State setPoint) {
       // feed forward still needs the math part
+      double setClampedPointPosition = getArmClampedGoal(setPoint.position);
       double armFeedVolts = armFeed.calculate(setPoint.velocity, 0);
-      armPID.setReference(setPoint.position, CANSparkMax.ControlType.kPosition,0, armFeedVolts);
+      armPID.setReference(setClampedPointPosition, CANSparkMax.ControlType.kPosition,0, armFeedVolts);
     }
 
     public double getArmPos() {
-      return MathUtil.inputModulus(armEncoder.getPosition(), Constants.Arm.ARM_DICONT_RAD,
-              Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI);
+      return MathUtil.inputModulus(armEncoder.getPosition(), Constants.Arm.ARM_DISCONT_RAD,
+              Constants.Arm.ARM_DISCONT_RAD + 2 * Math.PI);
     } 
 
 
@@ -131,7 +131,7 @@ public double getArmVel() {
 
     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
-      return MathUtil.clamp(MathUtil.inputModulus(goal, Constants.Arm.ARM_DICONT_RAD, Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI), Constants.Arm.LOWER_ANGLE_LIMIT, Constants.Arm.UPPER_ANGLE_LIMIT);
+      return MathUtil.clamp(goal, Constants.Arm.LOWER_ANGLE_LIMIT, Constants.Arm.UPPER_ANGLE_LIMIT);
     }
 
     public TrapezoidProfile.State getCurrentArmState() {
@@ -161,15 +161,5 @@ public void periodic() {
       // kD = SmartDashboard.getNumber("kD", kD);
       // kI = SmartDashboard.getNumber("kI", kI);
 
-      if (armPID.getP() != kP) {
-          armPID.setP(kP);
-      }
-      if (armPID.getD() != kD) {
-          armPID.setD(kD);
-      }
-      if (armPID.getI() != kI) {
-          armPID.setI(kI);
-      }
-
 		}
 }