feat: elevator pid is quite sexy

flamingchickens1540 · Jan 5, 2024 · 5732d8b · 5732d8b
1 parent 235517e
commit 5732d8b
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/drivetrain/Drivetrain.java
@@ -50,8 +50,10 @@ public void periodic() {
         poseEstimator.update(getYaw(), getWheelPositions());
         if (isClosedLoop && closedLoopSetpoint != null) {
             setVoltage(
-                    velocityPID.calculate(getWheelSpeeds().leftMetersPerSecond, closedLoopSetpoint.leftMetersPerSecond),
+                    velocityPID.calculate(getWheelSpeeds().leftMetersPerSecond, closedLoopSetpoint.leftMetersPerSecond)
+                    + feedforward.calculate(closedLoopSetpoint.leftMetersPerSecond),
                     velocityPID.calculate(getWheelSpeeds().rightMetersPerSecond, closedLoopSetpoint.rightMetersPerSecond)
+                    + feedforward.calculate(closedLoopSetpoint.rightMetersPerSecond)
             );
         }