fixed driveArm

DeepBlueRobotics · Mar 2, 2024 · 3160103 · 3160103
1 parent 07eec62
commit 3160103
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 11 deletions.
diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
@@ -53,7 +53,7 @@ public void execute() {
     goalArmRad = MathUtil.clamp(goalArmRad, LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT);
     goalState = new TrapezoidProfile.State(goalArmRad, 0);
     TrapezoidProfile.State setpoint = armSubsystem.calculateCustomSetPoint(armTimer.get(), armSubsystem.getCurrentArmState(), goalState);
-    armSubsystem.COMBINE_PID_FF_TRAPEZOID(setpoint);
+    armSubsystem.driveArm(setpoint);
     lastTime = currTime;
   }
 

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -135,17 +135,14 @@ public void autoCancelArmCommand() {
     }
 
     //#region Drive Methods
-
-    public void driveArm(double goalAngle) {
-        double targetRPS = SmartDashboard.getNumber("Shooter RPS", 0);
-        TrapezoidProfile.State setPoint = armProfile.calculate(kDt, getCurrentArmState(), goalState);
-        double armFeedVolts = armFeed.calculate(goalState.velocity, 0);
-        double feed = armFeed.calculate(targetRPS);
-        armPID1.setReference(targetRPS * 60, CANSparkBase.ControlType.kVelocity, 0, feed);
-        armPID2.setReference(targetRPS * 60, CANSparkBase.ControlType.kVelocity, 0, feed);
+    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);
+
+      armPID1.setReference(setPoint.position, CANSparkBase.ControlType.kVelocity, 0, armFeedVolts);
     }
 
-
     public void setArmTarget(double targetPos) {
         targetPos = getArmClampedGoal(targetPos);
 
@@ -161,7 +158,7 @@ public void setArmTarget(double targetPos) {
     public void resetGoal() {
         double armPos = getArmPos();
 
-        armProfile = new TrapezoidProfile(trapConstraints, );
+        armProfile = new TrapezoidProfile(trapConstraints);
 
     }