mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpilib] FlywheelSim cleanup (#6629)
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
committed by
GitHub
GitHub
parent
8c107e4b75
commit
f42bc45ee8
@@ -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.
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user