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

[wpimath] Add DifferentialDriveFeedforward classes which wrap LinearPlantInversionFeedforward (#4598)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
CarloWoolsey
2022-11-10 16:54:51 -08:00
committed by GitHub
parent 93890c528b
commit dbcc1de37f
5 changed files with 359 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp Normal file
View File

@@ -0,0 +1,37 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/controller/DifferentialDriveFeedforward.h"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
DifferentialDriveFeedforward::DifferentialDriveFeedforward(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_rad_per_s) kVAngular,
decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
: m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular, trackwidth)} {}
DifferentialDriveFeedforward::DifferentialDriveFeedforward(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular)
: m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular)} {}
DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
units::meters_per_second_t currentLeftVelocity,
units::meters_per_second_t nextLeftVelocity,
units::meters_per_second_t currentRightVelocity,
units::meters_per_second_t nextRightVelocity, units::second_t dt) {
frc::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
frc::Vectord<2> r{currentLeftVelocity, currentRightVelocity};
frc::Vectord<2> nextR{nextLeftVelocity, nextRightVelocity};
auto u = feedforward.Calculate(r, nextR);
return {units::volt_t{u(0)}, units::volt_t{u(1)}};
}