Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] FlywheelSim cleanup (#6629)

Browse Source 
This is a cleanup of the FlywheelSim class with a few added features.

- One FlywheelSim constructor that takes a plant, DCMotor, and a optional number of measurementStdDevs. The documentation now states how to construct the plant either through LinearSystemId.createFlywheelSystem or identifyVelocitySystem.

- The gearbox, gearing and moment of Inertia (J) are now private final fields. The gearing is determined from the plant in the constructor as well as the moment of inertia. There are getter methods that allow the flywheelSim to return the gearbox, gearing, and moment of inertia.

- The getCurrentDrawAmps function now uses m_x instead of getAngularVelocityRadPerSec in accordance with more accuracy and matches the patter in other sims.

- Added getter methods for the InputVoltage, angularAcceleration and torque

- (Java only) A third getter method for returning the AngularVelocity of the flywheel using a MutableMeasure as a backing field that is set when getAngularVelocity is called. This summarily returns the angularVelocity as just a Measure object. This allows the user of this class to handle unit conversions with less numerical manipulation. Alterations in C++ for this feature were not needed.
This commit is contained in:
Nicholas Armstrong
2024-05-24 19:05:14 -04:00
committed by GitHub
parent 8c107e4b75
commit f42bc45ee8
6 changed files with 225 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,19 +12,32 @@ using namespace frc;
using namespace frc::sim;
FlywheelSim::FlywheelSim(const LinearSystem<1, 1, 1>& plant,
const DCMotor& gearbox, double gearing,
const DCMotor& gearbox,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim<1, 1, 1>(plant, measurementStdDevs),
m_gearbox(gearbox),
m_gearing(gearing) {}
// By theorem 6.10.1 of
// https://file.tavsys.net/control/controls-engineering-in-frc.pdf, the
// flywheel state-space model is:
//
// dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
// A = -G²Kₜ/(KᵥRJ)
// B = GKₜ/(RJ)
//
// Solve for G.
//
// A/B = -G/Kᵥ
// G = -KᵥA/B
//
// Solve for J.
//
// B = GKₜ/(RJ)
// J = GKₜ/(RB)
m_gearing(-gearbox.Kv.value() * m_plant.A(0, 0) / m_plant.B(0, 0)),
m_j(m_gearing * gearbox.Kt.value() /
(gearbox.R.value() * m_plant.B(0, 0))) {}
FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing,
units::kilogram_square_meter_t moi,
const std::array<double, 1>& measurementStdDevs)
: FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing),
gearbox, gearing, measurementStdDevs) {}
void FlywheelSim::SetState(units::radians_per_second_t velocity) {
void FlywheelSim::SetVelocity(units::radians_per_second_t velocity) {
LinearSystemSim::SetState(Vectord<1>{velocity.value()});
}
@@ -32,6 +45,16 @@ units::radians_per_second_t FlywheelSim::GetAngularVelocity() const {
return units::radians_per_second_t{GetOutput(0)};
}
units::radians_per_second_squared_t FlywheelSim::GetAngularAcceleration()
const {
return units::radians_per_second_squared_t{
(m_plant.A() * m_x + m_plant.B() * m_u)(0, 0)};
}
units::newton_meter_t FlywheelSim::GetTorque() const {
return units::newton_meter_t{GetAngularAcceleration().value() * m_j.value()};
}
units::ampere_t FlywheelSim::GetCurrentDraw() const {
// I = V / R - omega / (Kv * R)
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor
@@ -41,6 +64,10 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const {
wpi::sgn(m_u(0));
}
units::volt_t FlywheelSim::GetInputVoltage() const {
return units::volt_t{GetInput(0)};
}
void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Vectord<1>{voltage.value()});
ClampInput(frc::RobotController::GetBatteryVoltage().value());

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

@@ -4,8 +4,10 @@
#pragma once
#include <units/angular_acceleration.h>
#include <units/angular_velocity.h>
#include <units/moment_of_inertia.h>
#include <units/torque.h>
#include "frc/simulation/LinearSystemSim.h"
#include "frc/system/LinearSystem.h"
@@ -22,54 +24,59 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
*
* @param plant The linear system representing the flywheel. This
* system can be created with
* LinearSystemId::FlywheelSystem().
* LinearSystemId::FlywheelSystem() or
* LinearSystemId::IdentifyVelocitySystem().
* @param gearbox The type of and number of motors in the flywheel
* gearbox.
* @param gearing The gearing of the flywheel (numbers greater than
* 1 represent reductions).
* @param measurementStdDevs The standard deviation of the measurement noise.
*/
FlywheelSim(const LinearSystem<1, 1, 1>& plant, const DCMotor& gearbox,
double gearing,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
* Creates a simulated flywheel mechanism.
*
* @param gearbox The type of and number of motors in the flywheel
* gearbox.
* @param gearing The gearing of the flywheel (numbers greater than
* 1 represent reductions).
* @param moi The moment of inertia of the flywheel.
* @param measurementStdDevs The standard deviation of the measurement noise.
*/
FlywheelSim(const DCMotor& gearbox, double gearing,
units::kilogram_square_meter_t moi,
const std::array<double, 1>& measurementStdDevs = {0.0});
using LinearSystemSim::SetState;
/**
* Sets the flywheel's state.
* Sets the flywheel's angular velocity.
*
* @param velocity The new velocity
*/
void SetState(units::radians_per_second_t velocity);
void SetVelocity(units::radians_per_second_t velocity);
/**
* Returns the flywheel velocity.
* Returns the flywheel's velocity.
*
* @return The flywheel velocity.
* @return The flywheel's velocity.
*/
units::radians_per_second_t GetAngularVelocity() const;
/**
* Returns the flywheel current draw.
* Returns the flywheel's acceleration.
*
* @return The flywheel current draw.
* @return The flywheel's acceleration
*/
units::radians_per_second_squared_t GetAngularAcceleration() const;
/**
* Returns the flywheel's torque.
*
* @return The flywheel's torque
*/
units::newton_meter_t GetTorque() const;
/**
* Returns the flywheel's current draw.
*
* @return The flywheel's current draw.
*/
units::ampere_t GetCurrentDraw() const;
/**
* Gets the input voltage for the flywheel.
*
* @return The flywheel input voltage.
*/
units::volt_t GetInputVoltage() const;
/**
* Sets the input voltage for the flywheel.
*
@@ -77,8 +84,24 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
*/
void SetInputVoltage(units::volt_t voltage);
/**
* Returns the gearbox.
*/
DCMotor Gearbox() const { return m_gearbox; }
/**
* Returns the gearing;
*/
double Gearing() const { return m_gearing; }
/**
* Returns the moment of inertia
*/
units::kilogram_square_meter_t J() const { return m_j; }
private:
DCMotor m_gearbox;
double m_gearing;
units::kilogram_square_meter_t m_j;
};
} // namespace frc::sim

2
wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
View File

@@ -25,7 +25,7 @@ TEST(StateSpaceSimTest, FlywheelSim) {
const frc::LinearSystem<1, 1, 1> plant =
frc::LinearSystemId::IdentifyVelocitySystem<units::radian>(
0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq);
frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2), 1.0};
frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2)};
frc::PIDController controller{0.2, 0.0, 0.0};
frc::SimpleMotorFeedforward<units::radian> feedforward{
0_V, 0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};

7
wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp
View File

@@ -92,8 +92,11 @@ class Robot : public frc::TimedRobot {
static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia =
0.5 * 1.5_lb * 4_in * 4_in;
frc::sim::FlywheelSim m_flywheelSim{frc::DCMotor::NEO(1), kFlywheelGearing,
kFlywheelMomentOfInertia};
frc::DCMotor m_gearbox = frc::DCMotor::NEO(1);
frc::LinearSystem<1, 1, 1> m_plant{frc::LinearSystemId::FlywheelSystem(
m_gearbox, kFlywheelMomentOfInertia, kFlywheelGearing)};
frc::sim::FlywheelSim m_flywheelSim{m_plant, m_gearbox};
frc::sim::EncoderSim m_encoderSim{m_encoder};
};

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

@@ -4,12 +4,19 @@
package edu.wpi.first.wpilibj.simulation;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
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;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.wpilibj.RobotController;
/** Represents a simulated flywheel mechanism. */
@@ -20,84 +27,168 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
// The gearing from the motors to the output.
private final double m_gearing;
// The moment of inertia for the flywheel mechanism.
private final double m_jKgMetersSquared;
// The angular velocity of the system.
private final MutableMeasure<Velocity<Angle>> m_angularVelocity =
MutableMeasure.zero(RadiansPerSecond);
// The angular acceleration of the system.
private final MutableMeasure<Velocity<Velocity<Angle>>> m_angularAcceleration =
MutableMeasure.zero(RadiansPerSecondPerSecond);
/**
* Creates a simulated flywheel mechanism.
*
* @param plant The linear system that represents the flywheel.
* @param plant The linear system that represents the flywheel. Use either {@link
* LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants
* or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system
* characterization.
* @param gearbox The type of and number of motors in the flywheel gearbox.
* @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions).
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* noise is desired. If present must have 1 element for velocity.
*/
public FlywheelSim(
LinearSystem<N1, N1, N1> plant,
DCMotor gearbox,
double gearing,
double... measurementStdDevs) {
LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double... measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
// By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf,
// the flywheel state-space model is:
//
// dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
// A = -G²Kₜ/(KᵥRJ)
// B = GKₜ/(RJ)
//
// Solve for G.
//
// A/B = -G/Kᵥ
// G = -KᵥA/B
//
// Solve for J.
//
// B = GKₜ/(RJ)
// J = GKₜ/(RB)
m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(0, 0) / plant.getB(0, 0);
m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(0, 0));
}
/**
* Creates a simulated flywheel mechanism.
*
* @param gearbox The type of and number of motors in the flywheel gearbox.
* @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions).
* @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, use the
* {@link #FlywheelSim(LinearSystem, DCMotor, double, double...)} constructor.
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* noise is desired. If present must have 1 element for velocity.
*/
public FlywheelSim(
DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) {
super(
LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing),
measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
}
/**
* Sets the flywheel's state.
* Sets the flywheel's angular velocity.
*
* @param velocityRadPerSec The new velocity in radians per second.
*/
public void setState(double velocityRadPerSec) {
public void setAngularVelocity(double velocityRadPerSec) {
setState(VecBuilder.fill(velocityRadPerSec));
}
/**
* Returns the flywheel velocity.
* Returns the gear ratio of the flywheel.
*
* @return The flywheel velocity.
* @return the flywheel's gear ratio.
*/
public double getGearing() {
return m_gearing;
}
/**
* Returns the moment of inertia in kilograms meters squared.
*
* @return The flywheel's moment of inertia.
*/
public double getJKgMetersSquared() {
return m_jKgMetersSquared;
}
/**
* Returns the gearbox for the flywheel.
*
* @return The flywheel's gearbox.
*/
public DCMotor getGearBox() {
return m_gearbox;
}
/**
* Returns the flywheel's velocity in Radians Per Second.
*
* @return The flywheel's velocity in Radians Per Second.
*/
public double getAngularVelocityRadPerSec() {
return getOutput(0);
}
/**
* Returns the flywheel velocity in RPM.
* Returns the flywheel's velocity in RPM.
*
* @return The flywheel velocity in RPM.
* @return The flywheel's velocity in RPM.
*/
public double getAngularVelocityRPM() {
return Units.radiansPerSecondToRotationsPerMinute(getOutput(0));
return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
}
/**
* Returns the flywheel current draw.
* Returns the flywheel's velocity.
*
* @return The flywheel current draw.
* @return The flywheel's velocity
*/
public Measure<Velocity<Angle>> getAngularVelocity() {
m_angularVelocity.mut_setMagnitude(getAngularVelocityRadPerSec());
return m_angularVelocity;
}
/**
* Returns the flywheel's acceleration in Radians Per Second Squared.
*
* @return The flywheel's acceleration in Radians Per Second Squared.
*/
public double getAngularAccelerationRadPerSecSq() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(0, 0);
}
/**
* Returns the flywheel's acceleration.
*
* @return The flywheel's acceleration.
*/
public Measure<Velocity<Velocity<Angle>>> getAngularAcceleration() {
m_angularAcceleration.mut_setMagnitude(getAngularAccelerationRadPerSecSq());
return m_angularAcceleration;
}
/**
* Returns the flywheel's torque in Newton-Meters.
*
* @return The flywheel's torque in Newton-Meters.
*/
public double getTorqueNewtonMeters() {
return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared;
}
/**
* Returns the flywheel's current draw.
*
* @return The flywheel's current draw.
*/
public double getCurrentDrawAmps() {
// I = V / R - omega / (Kv * R)
// Reductions are output over input, so a reduction of 2:1 means the motor is spinning
// 2x faster than the flywheel
return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
return m_gearbox.getCurrent(m_x.get(0, 0) * m_gearing, m_u.get(0, 0))
* Math.signum(m_u.get(0, 0));
}
/**
* Gets the input voltage for the flywheel.
*
* @return The flywheel input voltage.
*/
public double getInputVoltage() {
return getInput(0);
}
/**
* Sets the input voltage for the flywheel.
*

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java
View File

@@ -6,7 +6,10 @@ package edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller;
import edu.wpi.first.math.controller.BangBangController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
@@ -54,8 +57,12 @@ public class Robot extends TimedRobot {
private static final double kFlywheelMomentOfInertia =
0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2);
private final FlywheelSim m_flywheelSim =
new FlywheelSim(DCMotor.getNEO(1), kFlywheelGearing, kFlywheelMomentOfInertia);
private final DCMotor m_gearbox = DCMotor.getNEO(1);
private final LinearSystem<N1, N1, N1> m_plant =
LinearSystemId.createFlywheelSystem(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia);
private final FlywheelSim m_flywheelSim = new FlywheelSim(m_plant, m_gearbox);
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
@Override