Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[examples] Add Feedforward to ElevatorProfiledPid (#5300)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Joseph Ruan
2023-07-23 21:40:34 -07:00
committed by GitHub
parent 873c2a6c10
commit 1938251436
2 changed files with 36 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
@@ -12,8 +13,17 @@ import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
@SuppressWarnings("PMD.RedundantFieldInitializer")
public class Robot extends TimedRobot {
private static double kDt = 0.02;
private static double kMaxVelocity = 1.75;
private static double kMaxAcceleration = 0.75;
private static double kP = 1.3;
private static double kI = 0.0;
private static double kD = 0.7;
private static double kS = 1.1;
private static double kG = 1.2;
private static double kV = 1.3;
private final Joystick m_joystick = new Joystick(1);
private final Encoder m_encoder = new Encoder(1, 2);
@@ -22,9 +32,10 @@ public class Robot extends TimedRobot {
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
private final TrapezoidProfile.Constraints m_constraints =
new TrapezoidProfile.Constraints(1.75, 0.75);
new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration);
private final ProfiledPIDController m_controller =
new ProfiledPIDController(1.3, 0.0, 0.7, m_constraints, kDt);
new ProfiledPIDController(kP, kI, kD, m_constraints, kDt);
private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward(kS, kG, kV);
@Override
public void robotInit() {
@@ -40,6 +51,8 @@ public class Robot extends TimedRobot {
}
// Run controller and update motor output
m_motor.set(m_controller.calculate(m_encoder.getDistance()));
m_motor.setVoltage(
m_controller.calculate(m_encoder.getDistance())
+ m_feedforward.calculate(m_controller.getSetpoint().velocity));
}
}