Added feedforward to pivot

Iron-Panthers · Sep 18, 2024 · 6df6f1b · 6df6f1b
1 parent 72fe070
commit 6df6f1b
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
@@ -57,6 +57,7 @@ public PivotSubsystem() {
     pivotMotor.setInverted(true);
     pivotMotor.clearStickyFaults();
     pivotMotor.set(0);
+    pivotMotor.setPosition(0);
 
     pivotMotor.setNeutralMode(NeutralModeValue.Brake);
 
@@ -153,6 +154,11 @@ public double getAngularError() {
     return targetDegrees - getCurrentAngle();
   }
 
+  public void setPower(double power){
+    this.power = power;
+  }
+
+
   @Override
   public void periodic() {
 
@@ -170,6 +176,6 @@ public void periodic() {
 
     pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -10, 10);
 
-    pivotMotor.setVoltage(pidVoltageOutput);
+    pivotMotor.set(getFeedForward()+power);
   }
 }