Skip to content

Commit

Permalink
[wpimath] Add feedforward constant constructor to ElevatorSim (wpilib…
Browse files Browse the repository at this point in the history 
…suite#5823)

Adds a subclass of ElevatorSim that uses kG, kV, and kA from sysId to simulate an Elevator.
  • Loading branch information
@narmstro2020
narmstro2020 authored Nov 2, 2023
1 parent c6aff2c commit ddc8db6
Show file tree
Hide file tree
Showing 3 changed files with 117 additions and 37 deletions.
30 changes: 21 additions & 9 deletions wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,17 +13,14 @@ using namespace frc;
using namespace frc::sim;

ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
const DCMotor& gearbox, double gearing,
units::meter_t drumRadius, units::meter_t minHeight,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim(plant, measurementStdDevs),
m_gearbox(gearbox),
m_drumRadius(drumRadius),
m_minHeight(minHeight),
m_maxHeight(maxHeight),
m_gearing(gearing),
m_simulateGravity(simulateGravity) {
SetState(startingHeight, 0_mps);
}
Expand All @@ -36,8 +33,21 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
const std::array<double, 1>& measurementStdDevs)
: ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
drumRadius, gearing),
gearbox, gearing, drumRadius, minHeight, maxHeight,
simulateGravity, startingHeight, measurementStdDevs) {}
gearbox, minHeight, maxHeight, simulateGravity,
startingHeight, measurementStdDevs) {}

template <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
: ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), gearbox,
minHeight, maxHeight, simulateGravity, startingHeight,
measurementStdDevs) {}

void ElevatorSim::SetState(units::meter_t position,
units::meters_per_second_t velocity) {
Expand Down Expand Up @@ -74,10 +84,12 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor
// is spinning 10x faster than the output.

// v = r w, so w = v / r
double kA = 1.0 / m_plant.B(1, 0);
using Kv_t = units::unit_t<units::compound_unit<
units::volt, units::inverse<units::meters_per_second>>>;
Kv_t Kv = Kv_t{kA * m_plant.A(1, 1)};
units::meters_per_second_t velocity{m_x(1)};
units::radians_per_second_t motorVelocity =
velocity / m_drumRadius * m_gearing * 1_rad;
units::radians_per_second_t motorVelocity = velocity * Kv * m_gearbox.Kv;

// Perform calculation and return.
return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) *
Expand Down
38 changes: 31 additions & 7 deletions wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,15 @@ namespace frc::sim {
*/
class ElevatorSim : public LinearSystemSim<2, 1, 1> {
public:
template <typename Distance>
using Velocity_t = units::unit_t<
units::compound_unit<Distance, units::inverse<units::seconds>>>;

template <typename Distance>
using Acceleration_t = units::unit_t<units::compound_unit<
units::compound_unit<Distance, units::inverse<units::seconds>>,
units::inverse<units::seconds>>>;

/**
* Constructs a simulated elevator mechanism.
*
Expand All @@ -27,18 +36,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
* LinearSystemId::ElevatorSystem().
* @param gearbox The type of and number of motors in your
* elevator gearbox.
* @param gearing The gearing of the elevator (numbers greater
* than 1 represent reductions).
* @param drumRadius The radius of the drum that your cable is
* wrapped around.
* @param minHeight The minimum allowed height of the elevator.
* @param maxHeight The maximum allowed height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeight The starting height of the elevator.
* @param measurementStdDevs The standard deviation of the measurements.
*/
ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox,
double gearing, units::meter_t drumRadius,
units::meter_t minHeight, units::meter_t maxHeight,
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
Expand All @@ -65,6 +69,28 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});

/**
* Constructs a simulated elevator mechanism.
*
* @param kV The velocity gain.
* @param kA The acceleration gain.
* @param gearbox The type of and number of motors in your
* elevator gearbox.
* @param minHeight The minimum allowed height of the elevator.
* @param maxHeight The maximum allowed height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeight The starting height of the elevator.
* @param measurementStdDevs The standard deviation of the measurements.
*/
template <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
using LinearSystemSim::SetState;

/**
Expand Down Expand Up @@ -146,10 +172,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {

private:
DCMotor m_gearbox;
units::meter_t m_drumRadius;
units::meter_t m_minHeight;
units::meter_t m_maxHeight;
double m_gearing;
bool m_simulateGravity;
};
} // namespace frc::sim
86 changes: 65 additions & 21 deletions wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,6 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
// Gearbox for the elevator.
private final DCMotor m_gearbox;

// Gearing between the motors and the output.
private final double m_gearing;

// The radius of the drum that the elevator spool is wrapped around.
private final double m_drumRadius;

// The min allowable height for the elevator.
private final double m_minHeight;

Expand All @@ -41,8 +35,6 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
* {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
* double, double)}.
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
* @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
Expand All @@ -52,17 +44,13 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
DCMotor gearbox,
double gearing,
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_drumRadius = drumRadiusMeters;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_simulateGravity = simulateGravity;
Expand All @@ -77,8 +65,6 @@ public ElevatorSim(
* {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
* double, double)}.
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
* @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param startingHeightMeters The starting height of the elevator.
Expand All @@ -87,24 +73,81 @@ public ElevatorSim(
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
DCMotor gearbox,
double gearing,
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters) {
this(
plant,
gearbox,
gearing,
drumRadiusMeters,
minHeightMeters,
maxHeightMeters,
simulateGravity,
startingHeightMeters,
null);
}

/**
* Creates a simulated elevator mechanism.
*
* @param kV The velocity gain.
* @param kA The acceleration gain.
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
*/
public ElevatorSim(
double kV,
double kA,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters) {
this(
kV,
kA,
gearbox,
minHeightMeters,
maxHeightMeters,
simulateGravity,
startingHeightMeters,
null);
}

/**
* Creates a simulated elevator mechanism.
*
* @param kV The velocity gain.
* @param kA The acceleration gain.
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
* @param measurementStdDevs The standard deviations of the measurements.
*/
public ElevatorSim(
double kV,
double kA,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
this(
LinearSystemId.identifyPositionSystem(kV, kA),
gearbox,
minHeightMeters,
maxHeightMeters,
simulateGravity,
startingHeightMeters,
measurementStdDevs);
}

/**
* Creates a simulated elevator mechanism.
*
Expand All @@ -131,8 +174,6 @@ public ElevatorSim(
this(
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
gearbox,
gearing,
drumRadiusMeters,
minHeightMeters,
maxHeightMeters,
simulateGravity,
Expand Down Expand Up @@ -253,7 +294,10 @@ public double getCurrentDrawAmps() {
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
// spinning 10x faster than the output
// v = r w, so w = v/r
double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing;
double kA = 1 / m_plant.getB().get(1, 0);
double kV = -m_plant.getA().get(1, 1) * kA;
double motorVelocityRadPerSec =
getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt;
var appliedVoltage = m_u.get(0, 0);
return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
* Math.signum(appliedVoltage);
Expand Down

0 comments on commit ddc8db6

Please sign in to comment.