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

Add feedforward components (#2045)

Browse Source 
Add helper classes for computing feedforwards with parameters supplied by the characterization tool.
This commit is contained in:
Oblarg
2019-11-09 23:16:42 -05:00
committed by Peter Johnson
parent 5f33d6af12
commit 7dc7c71b58
13 changed files with 457 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
View File

@@ -10,16 +10,10 @@
using namespace frc2;
using namespace units;
template <typename T>
int sgn(T val) {
return (T(0) < val) - (val < T(0));
}
RamseteCommand::RamseteCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::RamseteController controller, volt_t ks,
units::unit_t<voltsecondspermeter> kv,
units::unit_t<voltsecondssquaredpermeter> ka,
frc::RamseteController controller,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::DifferentialDriveKinematics kinematics,
std::function<units::meters_per_second_t()> leftSpeed,
std::function<units::meters_per_second_t()> rightSpeed,
@@ -29,9 +23,7 @@ RamseteCommand::RamseteCommand(
: m_trajectory(trajectory),
m_pose(pose),
m_controller(controller),
m_ks(ks),
m_kv(kv),
m_ka(ka),
m_feedforward(feedforward),
m_kinematics(kinematics),
m_leftSpeed(leftSpeed),
m_rightSpeed(rightSpeed),
@@ -52,9 +44,6 @@ RamseteCommand::RamseteCommand(
: m_trajectory(trajectory),
m_pose(pose),
m_controller(controller),
m_ks(0),
m_kv(0),
m_ka(0),
m_kinematics(kinematics),
m_outputVel(output),
m_usePID(false) {
@@ -84,12 +73,12 @@ void RamseteCommand::Execute() {
if (m_usePID) {
auto leftFeedforward =
m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
m_feedforward.Calculate(targetWheelSpeeds.left,
(targetWheelSpeeds.left - m_prevSpeeds.left) / dt);
auto rightFeedforward =
m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
m_feedforward.Calculate(targetWheelSpeeds.right,
(targetWheelSpeeds.right - m_prevSpeeds.right) / dt);
auto leftOutput =
volt_t(m_leftController->Calculate(