Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +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

14
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
@@ -121,7 +122,7 @@ public class DifferentialDrivetrainSim {
* @param rightVoltageVolts The right voltage.
*/
public void setInputs(double leftVoltageVolts, double rightVoltageVolts) {
m_u = VecBuilder.fill(leftVoltageVolts, rightVoltageVolts);
m_u = clampInput(VecBuilder.fill(leftVoltageVolts, rightVoltageVolts));
}
@SuppressWarnings("LocalVariableName")
@@ -278,6 +279,17 @@ public class DifferentialDrivetrainSim {
return xdot;
}
/**
* 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.
*/
protected Matrix<N2, N1> clampInput(Matrix<N2, N1> u) {
return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
}
enum State {
kX(0),
kY(1),

15
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.RobotController;
import org.ejml.MatrixDimensionException;
import org.ejml.simple.SimpleMatrix;
@@ -113,7 +114,7 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
* @param u The system inputs.
*/
public void setInput(Matrix<Inputs, N1> u) {
this.m_u = u;
this.m_u = clampInput(u);
}
/**
@@ -124,6 +125,7 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
*/
public void setInput(int row, double value) {
m_u.set(row, 0, value);
m_u = clampInput(m_u);
}
/**
@@ -170,4 +172,15 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
Matrix<Inputs, N1> u, double dtSeconds) {
return m_plant.calculateX(currentXhat, u, dtSeconds);
}
/**
* 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.
*/
protected Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> u) {
return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
}
}