changed trapezoid profile to not use deprecated stuff

DeepBlueRobotics · Feb 19, 2024 · bbeca40 · bbeca40
1 parent b2b20e2
commit bbeca40
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 6 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -54,6 +54,10 @@ public static final class Arm {
 		public static final double MAX_FF_VEL = 1; // rot / s
 		public static final double MAX_FF_ACCEL = 1; // rot / s^2
 		public static TrapezoidProfile.Constraints trapConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL);
+
+		//trapezoid profile numbers
+		public static final double kMaxV = 0;
+		public static final double kMaxA = 0;
 
 	}
 	public static final class IntakeShooter {

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -7,7 +7,7 @@
 import java.util.function.DoubleSupplier;
 
 import org.carlmontrobotics.Constants;
-import org.carlmontrobotics.Constants.Arm.*;
+import static org.carlmontrobotics.Constants.Arm.*;
 import org.carlmontrobotics.lib199.MotorConfig;
 import org.carlmontrobotics.lib199.MotorControllerFactory;
 
@@ -33,16 +33,21 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 
+
 public class Arm extends SubsystemBase {
-    private final CANSparkMax armMotor1 = MotorControllerFactory.createSparkMax(Constants.Arm.LEFT_MOTOR_PORT,MotorConfig.NEO);
+    private final CANSparkMax armMotor1 = MotorControllerFactory.createSparkMax(LEFT_MOTOR_PORT,MotorConfig.NEO);
     //private final CANSparkMax armMotor2 = MotorControllerFactory.createSparkMax(Constants.Arm.RIGHT_MOTOR_PORT,MotorConfig.NEO);
     //there is only one arm motor. 
-    private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(Constants.Arm.kS, Constants.Arm.kV);
+    private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(kS, kV);
     private final SparkAbsoluteEncoder armEncoder = armMotor1.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
-    private final PIDController armPID = new PIDController(Constants.Arm.kP, Constants.Arm.kI, Constants.Arm.kD);
+    private final PIDController armPID = new PIDController(kP, kI, kD);
     public static TrapezoidProfile.State[] goalState = { new TrapezoidProfile.State(-Math.PI / 2, 0), new TrapezoidProfile.State(Math.toRadians(43), 0) };
-    private TrapezoidProfile armProfile = new TrapezoidProfile(Constants.Arm.trapConstraints, goalState[0], getCurrentArmState());
-    public Arm() {
+
+    TrapezoidProfile.Constraints constraints =new TrapezoidProfile.Constraints(kMaxV, kMaxA);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+
+
 			//arm 
       /*
        have 3 set positions
@@ -60,6 +65,7 @@ public void setArmGoal(double targetPosition, double targetVelocity) {
       //these values are in constants
       //pass in where scorign and use switch statement to alternate between each angle needed
       targetPosition = getArmClampedGoal(targetPosition);
+
 
     }