fix #27 by dynamic ff calculation

DeepBlueRobotics · Mar 9, 2024 · 1d1355a · 1d1355a
1 parent a534422
commit 1d1355a
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 6 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -34,7 +34,7 @@ public static final class Arm {
 		public static final int ARM_MOTOR_PORT_MASTER = 13;
 		public final static int ARM_MOTOR_PORT_FOLLOWER = 18;
 		// Config for motors
-		public static final boolean MOTOR_INVERTED_MASTER = false; 
+		public static final boolean MOTOR_INVERTED_MASTER = false;
 		public static final boolean MOTOR_INVERTED_FOLLOWER = true; //verifyed by design
 		public static final double ROTATION_TO_RAD = 2 * Math.PI;
 		public static final boolean ENCODER_INVERTED = false;
@@ -64,15 +64,14 @@ public static final class Arm {
 		public static final double kA = 0.1;
 		public static final double IZONE_RAD = .09;
 		//fine for now, change it later before use - ("Incorect use of setIZone()" Issue #22)
-		public static final double MAX_FF_VEL_RAD_P_S = 8.44470886; // rad / s WORK: w = at, a=max accel, t = sqrt([2*max angular position]/a) -> t=sqrt([7pi/6]/a) -> at = 8.44470886
-		public static final double MAX_FF_ACCEL_RAD_P_S = 19.4569; // rad / s^2 WORK: a=I/T*gear ratio -> I=1/2mr^2(metric units) -> m=6.80389kg, r =.6855m, -> I=1.16741703, T=3.6 newton meters -> a=(I/T)*60[gear ratio] 
+		//public static final double MAX_FF_VEL_RAD_P_S = 8.44470886; // rad / s WORK: w = at, a=max accel, t = sqrt([2*max angular position]/a) -> t=sqrt([7pi/6]/a) -> at = 8.44470886
+		public static final double MAX_FF_ACCEL_RAD_P_S = 19.4569; // rad / s^2 WORK: a=I/T*gear ratio -> I=1/2mr^2(metric units) -> m=6.80389kg, r =.6855m, -> I=1.16741703, T=3.6 newton meters -> a=(I/T)*60[gear ratio]
 
 		// if needed
 		public static final double COM_ARM_LENGTH_METERS = 0.381;
 		public static final double ARM_MASS_KG = 9.59302503;
 
-		public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
-				MAX_FF_VEL_RAD_P_S, MAX_FF_ACCEL_RAD_P_S);
+		public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS;//initalized by arm constructor 
 		// other0;
 
 		// public static final double MARGIN_OF_ERROR = Math.PI / 18;

diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
@@ -68,7 +68,7 @@ public double getRequestedSpeeds() {
     if (Math.abs(joystick.getAsDouble()) <= Constants.OI.JOY_THRESH) {
       rawArmVel = 0.0;
     } else {
-      rawArmVel = MAX_FF_VEL_RAD_P_S * joystick.getAsDouble();
+      rawArmVel = armSubsystem.getMaxAccelRad() * joystick.getAsDouble();
     }
 
     return rawArmVel;

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -108,6 +108,11 @@ public Arm() {
         armPIDMaster.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT_RAD);
         armPIDMaster.setIZone(IZONE_RAD);
 
+        TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
+            armFeed.maxAchievableVelocity(12, Math.PI/2, 0), 
+            MAX_FF_ACCEL_RAD_P_S);
+        //^ worst case scenario
+
         SmartDashboard.putData("Arm", this);
 
         setpoint = getCurrentArmState();
@@ -255,4 +260,8 @@ public double getArmClampedGoal(double goal) {
         return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI),
                 LOWER_ANGLE_LIMIT_RAD, UPPER_ANGLE_LIMIT_RAD);
     }
+
+    public double getMaxAccelRad(){
+      return armFeed.maxAchievableVelocity(12, getArmPos(), getArmVel());
+    }
 }