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

[wpimath] Add ElevatorFeedforward.calculate(currentV, nextV) overload (#5715)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
narmstro2020
2023-10-09 11:16:45 -04:00
committed by GitHub
parent a789632052
commit faa1e665ba
4 changed files with 122 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
View File

@@ -4,6 +4,10 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.system.plant.LinearSystemId;
/**
* A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
* against the force of gravity).
@@ -53,6 +57,48 @@ public class ElevatorFeedforward {
return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* <p>Note this method is inaccurate when the velocity crosses 0.
*
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @param dtSeconds Time between velocity setpoints in seconds.
* @return The computed feedforward.
*/
public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
// Discretize the affine model.
//
// dx/dt = Ax + Bu + c
// dx/dt = Ax + B(u + B⁺c)
// xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
// xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
// xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
//
// Solve for uₖ.
//
// B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
//
// For an elevator with the model
// dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
// A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
// assuming sgn(x) is a constant for the duration of the step.
//
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
}
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).