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

[wpilib] Clamp input voltage in sim classes (#2955)

Browse Source
This commit is contained in:
Matt
2020-12-28 13:03:31 -08:00
committed by GitHub
parent dd494d4ab7
commit 9005cd59e5
5 changed files with 66 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
View File

@@ -10,6 +10,7 @@
#include <units/current.h>
#include <units/time.h>
#include "frc/RobotController.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/LinearSystem.h"
@@ -82,7 +83,9 @@ class LinearSystemSim {
*
* @param u The system inputs.
*/
void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) { m_u = u; }
void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) {
m_u = ClampInput(u);
}
/*
* Sets the system inputs.
@@ -90,7 +93,10 @@ class LinearSystemSim {
* @param row The row in the input matrix to set.
* @param value The value to set the row to.
*/
void SetInput(int row, double value) { m_u(row, 0) = value; }
void SetInput(int row, double value) {
m_u(row, 0) = value;
ClampInput(m_u);
}
/**
* Sets the system state.
@@ -123,6 +129,19 @@ class LinearSystemSim {
return m_plant.CalculateX(currentXhat, u, dt);
}
/**
* Clamp the input vector such that no element exceeds the given voltage. If
* any does, the relative magnitudes of the input will be maintained.
*
* @param u The input vector.
* @return The normalized input.
*/
Eigen::Matrix<double, Inputs, 1> ClampInput(
Eigen::Matrix<double, Inputs, 1> u) {
return frc::NormalizeInputVector<Inputs>(
u, frc::RobotController::GetInputVoltage());
}
LinearSystem<States, Inputs, Outputs> m_plant;
Eigen::Matrix<double, States, 1> m_x;