Trapezoid Math Stuff :]

DeepBlueRobotics · Mar 2, 2024 · 9d871f8 · 9d871f8
1 parent ab2ef91
commit 9d871f8
Showing 1 changed file with 29 additions and 2 deletions.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -98,9 +98,36 @@ public TrapezoidProfile.State calculateCustomSetPoint(double goalSeconds, Trapez
 
     }
 
-    public void maxacceleration(){
-      armFeed.maxAchievableAcceleration(MAX_VOLTAGE, armEncoder.getVelocity()); 
+
+    //public void maxacceleration(){
+      //armFeed.maxAchievableAcceleration(MAX_VOLTAGE, armEncoder.getVelocity()); 
+    //}
+
+    double currentVelocity = armEncoder.getVelocity();
+
+    public double maxacceleration(){
+      double maxAccel = armFeed.maxAchievableAcceleration(MAX_VOLTAGE, currentVelocity); 
+      return maxAccel;
+    }
+
+    //public void maxvelocity(){
+      //armFeed.maxAchievableVelocity(MAX_VOLTAGE, );
+    //}
+
+    public double calculateTrapTime(double goalAngle, double currentAngle){
+      double distToCover = goalAngle - currentAngle;
+      double maxAccel = maxacceleration();
+
+      double timeOfAccelTriangle = MAX_FF_VEL / maxAccel;
+      double distCovrdTriangle = maxAccel / 2 * timeOfAccelTriangle;
+
+      double distLeft = distToCover - (2 * distCovrdTriangle);
+      double timeOfRect = distLeft / MAX_FF_VEL;
+
+      double answer = timeOfAccelTriangle*2 + timeOfRect;
+      return answer;
     }
+
 
     // public void driveArm(double timeToTarget, TrapezoidProfile.State goalState) {
     //   TrapezoidProfile.State setPoint = profile.calculate(timeToTarget, getCurrentArmState(), goalState);