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] change G to gearing in LinearSystemId factories (#5834)

Browse Source
This commit is contained in:
DeltaDizzy
2023-11-12 22:22:39 -06:00
committed by GitHub
parent 9ada181866
commit 78ebc6e9ec
3 changed files with 84 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
View File

@@ -9,23 +9,24 @@ using namespace frc;
LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
units::kilogram_t mass,
units::meter_t radius,
double G) {
double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
if (radius <= 0_m) {
throw std::domain_error("radius must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
{0.0, (-std::pow(gearing, 2) * motor.Kt /
(motor.R * units::math::pow<2>(radius) * mass * motor.Kv))
.value()}};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * radius * mass)).value()};
Matrixd<2, 1> B{0.0,
(gearing * motor.Kt / (motor.R * radius * mass)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
@@ -33,18 +34,19 @@ LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
}
LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double G) {
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
{0.0,
(-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
@@ -119,17 +121,17 @@ LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem(
}
LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem(
DCMotor motor, units::kilogram_square_meter_t J, double G) {
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<1, 1> A{
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
(-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
Matrixd<1, 1> B{(gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 1> C{1.0};
Matrixd<1, 1> D{0.0};
@@ -137,18 +139,19 @@ LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem(
}
LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem(
DCMotor motor, units::kilogram_square_meter_t J, double G) {
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
{0.0,
(-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{0.0, 0.0};
@@ -157,7 +160,7 @@ LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem(
LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem(
const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
units::meter_t rb, units::kilogram_square_meter_t J, double G) {
units::meter_t rb, units::kilogram_square_meter_t J, double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
@@ -170,13 +173,13 @@ LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem(
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
auto C1 = -std::pow(G, 2) * motor.Kt /
auto C1 = -std::pow(gearing, 2) * motor.Kt /
(motor.Kv * motor.R * units::math::pow<2>(r));
auto C2 = G * motor.Kt / (motor.R * r);
auto C2 = gearing * motor.Kt / (motor.R * r);
Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass - units::math::pow<2>(rb) / J) * C1).value()},