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 SimpleMotorFeedforward::Calculate(velocity, nextVelocity) overload (#3183)

Browse Source 
This is often more convenient than using the overload with velocity and
acceleration.

Fixes #3160.
This commit is contained in:
Tyler Veness
2021-05-21 23:44:10 -07:00
committed by GitHub
parent 0768c39036
commit 04dae799a2
4 changed files with 147 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.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 permanent-magnet DC motor. */
@SuppressWarnings("MemberName")
public class SimpleMotorFeedforward {
@@ -47,6 +51,24 @@ public class SimpleMotorFeedforward {
return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @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) {
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 ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
}
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below: