Feedforward calculations using WPILIB updates

Technical Programming
1

With the new WPILIB update they’ve changed how feed forward values are returned. Prior to this update we used this

image
image1197×339 41.7 KB

however this method is going to be removed and several other methods have taken its place. I wanted to know which of the methods below would move an elevator to a setpoint fastest. Any help is greatly appreciated!

image
image1135×663 74.5 KB

image
image1357×853 118 KB

image
image1234×796 110 KB

2

Use ElevatorFeedforward.calculateWithVelocities(double currentVelocity, double nextVelocity) with a motion profile.

package frc.robot;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.trajectory.ExponentialProfile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;

public class Robot extends TimedRobot {
  public static final double dt = 0.01;
  public static final double offset = 0.005;

  public static final double Ks = 0.1;
  public static final double Kg = 4.9;
  public static final double Kv = 3.0;
  public static final double Ka = 0.5;

  public static final double Kp = 0.1;
  public static final double Kd = 0.1;

  public Encoder encoder = new Encoder(0, 1);
  public PWMSparkMax motor = new PWMSparkMax(0);

  public ExponentialProfile profile =
      new ExponentialProfile(ExponentialProfile.Constraints.fromCharacteristics(10.0, Kv, Ka));

  public ElevatorFeedforward feedforward = new ElevatorFeedforward(Ks, Kg, Kv, Ka, dt);
  public PIDController feedback = new PIDController(Kp, 0.0, Kd, dt);

  public ExponentialProfile.State currentSetpoint = new ExponentialProfile.State();
  public ExponentialProfile.State goal = new ExponentialProfile.State();

  public Robot() {
    addPeriodic(this::controller, dt, offset);
  }

  public void setGoal(double goal) {
    this.goal.position = goal;
  }

  public void controller() {
    if (isDisabled()) {
      return;
    }

    var nextSetpoint = profile.calculate(dt, currentSetpoint, goal);
    motor.setVoltage(
        feedforward.calculateWithVelocities(currentSetpoint.velocity, nextSetpoint.velocity)
            + feedback.calculate(encoder.getDistance(), currentSetpoint.position));
    currentSetpoint = nextSetpoint;
  }
}
1 Like