calculateCustomSetPoint added - 1 error left

DeepBlueRobotics · Mar 2, 2024 · 5d98af8 · 5d98af8
1 parent aba73c3
commit 5d98af8
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -121,7 +121,11 @@ public void periodic() {
         autoCancelArmCommand();
     }
 
-  public void autoCancelArmCommand() {
+    public TrapezoidProfile.State calculateCustomSetPoint(double goalSeconds, TrapezoidProfile.State currentPoint, TrapezoidProfile.State goalState) {
+        return armProfile.calculate(goalSeconds, currentPoint, goalState);
+    }
+
+    public void autoCancelArmCommand() {
         if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return;
 
         double requestedSpeeds = ((ArmTeleop) getDefaultCommand()).getRequestedSpeeds();
@@ -135,7 +139,7 @@ public void autoCancelArmCommand() {
     }
 
     //#region Drive Methods
-    public void driveArm(double goalAngle ){
+    public void driveArm(double goalAngle){
       TrapezoidProfile.State goalState = new TrapezoidProfile.State(goalAngle, 0);
       TrapezoidProfile.State setPoint = armProfile.calculate(kDt, getCurrentArmState(), goalState);
       double armFeedVolts = armFeed.calculate(goalState.velocity, 0);