Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add mechanism specific SetState overloads to physics sims (#5534)

Browse Source
This commit is contained in:
Ryan Blue
2023-08-12 18:21:07 -04:00
committed by GitHub
parent 8121566258
commit a4b7fde767
12 changed files with 112 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
View File

@@ -24,6 +24,11 @@ DCMotorSim::DCMotorSim(const DCMotor& gearbox, double gearing,
: DCMotorSim(LinearSystemId::DCMotorSystem(gearbox, moi, gearing), gearbox,
gearing, measurementStdDevs) {}
void DCMotorSim::SetState(units::radian_t angularPosition,
units::radians_per_second_t angularVelocity) {
SetState(Vectord<2>{angularPosition, angularVelocity});
}
units::radian_t DCMotorSim::GetAngularPosition() const {
return units::radian_t{GetOutput(0)};
}

9
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -25,8 +25,7 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
m_maxHeight(maxHeight),
m_gearing(gearing),
m_simulateGravity(simulateGravity) {
SetState(
frc::Vectord<2>{std::clamp(startingHeight, minHeight, maxHeight), 0.0});
SetState(startingHeight, 0_mps);
}
ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
@@ -40,6 +39,12 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
gearbox, gearing, drumRadius, minHeight, maxHeight,
simulateGravity, startingHeight, measurementStdDevs) {}
void ElevatorSim::SetState(units::meter_t position,
units::meters_per_second_t velocity) {
SetState(
Vectord<2>{std::clamp(position, m_minHeight, m_maxHeight), velocity});
}
bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const {
return elevatorHeight <= m_minHeight;
}

4
wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
View File

@@ -24,6 +24,10 @@ FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing,
: FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing),
gearbox, gearing, measurementStdDevs) {}
void FlywheelSim::SetState(units::radians_per_second_t velocity) {
LinearSystemSim::SetState(Vectord<1>{velocity.value()});
}
units::radians_per_second_t FlywheelSim::GetAngularVelocity() const {
return units::radians_per_second_t{GetOutput(0)};
}

7
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -28,7 +28,7 @@ SingleJointedArmSim::SingleJointedArmSim(
m_gearbox(gearbox),
m_gearing(gearing),
m_simulateGravity(simulateGravity) {
SetState(frc::Vectord<2>{startingAngle, 0.0});
SetState(startingAngle, 0_rad_per_s);
}
SingleJointedArmSim::SingleJointedArmSim(
@@ -42,6 +42,11 @@ SingleJointedArmSim::SingleJointedArmSim(
gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity,
startingAngle, measurementStdDevs) {}
void SingleJointedArmSim::SetState(units::radian_t angle,
units::radians_per_second_t velocity) {
SetState(Vectord<2>{std::clamp(angle, m_minAngle, m_maxAngle), velocity});
}
bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const {
return armAngle <= m_minAngle;
}

11
wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h
View File

@@ -48,6 +48,17 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> {
units::kilogram_square_meter_t moi,
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
using LinearSystemSim::SetState;
/**
* Sets the state of the DC motor.
*
* @param angularPosition The new position
* @param angularVelocity The new velocity
*/
void SetState(units::radian_t angularPosition,
units::radians_per_second_t angularVelocity);
/**
* Returns the DC motor position.
*

10
wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
View File

@@ -65,6 +65,16 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
using LinearSystemSim::SetState;
/**
* Sets the elevator's state. The new position will be limited between the
* minimum and maximum allowed heights.
* @param position The new position
* @param velocity The new velocity
*/
void SetState(units::meter_t position, units::meters_per_second_t velocity);
/**
* Returns whether the elevator would hit the lower limit.
*

9
wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
View File

@@ -47,6 +47,15 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
units::kilogram_square_meter_t moi,
const std::array<double, 1>& measurementStdDevs = {0.0});
using LinearSystemSim::SetState;
/**
* Sets the flywheel's state.
*
* @param velocity The new velocity
*/
void SetState(units::radians_per_second_t velocity);
/**
* Returns the flywheel velocity.
*

11
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
View File

@@ -64,6 +64,17 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs = {0.0});
using LinearSystemSim::SetState;
/**
* Sets the arm's state. The new angle will be limited between the minimum and
* maximum allowed limits.
*
* @param angle The new angle.
* @param velocity The new angular velocity.
*/
void SetState(units::radian_t angle, units::radians_per_second_t velocity);
/**
* Returns whether the arm would hit the lower limit.
*

11
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
@@ -84,6 +85,16 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
m_gearing = gearing;
}
/**
* Sets the state of the DC motor.
*
* @param position The new position
* @param velocity The new velocity
*/
void setState(double angularPositionRad, double angularVelocityRadPerSec) {
setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec));
}
/**
* Returns the DC motor position.
*

16
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -67,8 +67,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
m_maxHeight = maxHeightMeters;
m_simulateGravity = simulateGravity;
setState(
VecBuilder.fill(MathUtil.clamp(startingHeightMeters, minHeightMeters, maxHeightMeters), 0));
setState(startingHeightMeters, 0);
}
/**
@@ -174,6 +173,19 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
null);
}
/**
* Sets the elevator's state. The new position will be limited between the minimum and maximum
* allowed heights.
*
* @param positionMeters The new position in meters.
* @param velocityMetersPerSecond New velocity in meters per second.
*/
public void setState(double positionMeters, double velocityMetersPerSecond) {
setState(
VecBuilder.fill(
MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond));
}
/**
* Returns whether the elevator would hit the lower limit.
*

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
@@ -84,6 +85,15 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
m_gearing = gearing;
}
/**
* Sets the flywheel's state.
*
* @param velocityRadPerSec The new velocity in radians per second.
*/
public void setState(double velocityRadPerSec) {
setState(VecBuilder.fill(velocityRadPerSec));
}
/**
* Returns the flywheel velocity.
*

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

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
@@ -66,7 +67,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
m_maxAngle = maxAngleRads;
m_simulateGravity = simulateGravity;
setState(VecBuilder.fill(startingAngleRads, 0));
setState(startingAngleRads, 0.0);
}
/**
@@ -172,6 +173,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
measurementStdDevs);
}
/**
* Sets the arm's state. The new angle will be limited between the minimum and maximum allowed
* limits.
*
* @param angleRadians The new angle.
* @param velocityRadPerSec The new angular velocity.
*/
void setState(double angleRadians, double velocityRadPerSec) {
setState(
VecBuilder.fill(MathUtil.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec));
}
/**
* Returns whether the arm would hit the lower limit.
*