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 ArmFeedforward calculate() overload that takes current and next velocity instead of acceleration (#6540)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-04-28 15:01:08 -04:00
committed by GitHub
parent 1727c74b80
commit 1ec089c7f9
8 changed files with 355 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.math.controller.proto.ArmFeedforwardProto;
import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -100,6 +101,22 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
return calculate(positionRadians, velocity, 0);
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param currentAngle The current angle in radians. This angle should be measured from the
* horizontal (i.e. if the provided angle is 0, the arm should be parallel to the floor). If
* your encoder does not follow this convention, an offset should be added.
* @param currentVelocity The current velocity setpoint in radians per second.
* @param nextVelocity The next velocity setpoint in radians per second.
* @param dt Time between velocity setpoints in seconds.
* @return The computed feedforward in volts.
*/
public double calculate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
return WPIMathJNI.calculate(ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
}
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below: