FlywheelSim vs DCMotorSim
199 lines
// Copyright (c) FIRST and other WPILib contributors.
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.simulation;
package edu.wpi.first.wpilibj.simulation;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
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.math.util.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularAcceleration;
import edu.wpi.first.units.measure.AngularAcceleration;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngle;
import edu.wpi.first.units.measure.MutAngularAcceleration;
import edu.wpi.first.units.measure.MutAngularAcceleration;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.RobotController;
/** Represents a simulated flywheel mechanism. */
/** Represents a simulated DC motor mechanism. */
public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
// Gearbox for the flywheel.
// Gearbox for the DC motor.
private final DCMotor m_gearbox;
private final DCMotor m_gearbox;
// The gearing from the motors to the output.
// The gearing from the motors to the output.
private final double m_gearing;
private final double m_gearing;
// The moment of inertia for the flywheel mechanism.
// The moment of inertia for the DC motor mechanism.
private final double m_jKgMetersSquared;
private final double m_jKgMetersSquared;
// The angle of the system.
private final MutAngle m_angle = Radians.mutable(0.0);
// The angular velocity of the system.
// The angular velocity of the system.
private final MutAngularVelocity m_angularVelocity = RadiansPerSecond.mutable(0);
private final MutAngularVelocity m_angularVelocity = RadiansPerSecond.mutable(0.0);
// The angular acceleration of the system.
// The angular acceleration of the system.
private final MutAngularAcceleration m_angularAcceleration = RadiansPerSecondPerSecond.mutable(0);
private final MutAngularAcceleration m_angularAcceleration =
RadiansPerSecondPerSecond.mutable(0.0);
/**
/**
* Creates a simulated flywheel mechanism.
* Creates a simulated DC motor mechanism.
*
*
* @param plant The linear system that represents the flywheel. Use either {@link
* @param plant The linear system representing the DC motor. This system can be created with
* LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants
* {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double,
* or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system
* double)} or {@link
* characterization.
* edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}. If
* @param gearbox The type of and number of motors in the flywheel gearbox.
* {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}
* is used, the distance unit must be radians.
* @param gearbox The type of and number of motors in the DC motor gearbox.
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* noise is desired. If present must have 1 element for velocity.
* noise is desired. If present must have 2 elements. The first element is for position. The
* second element is for velocity.
*/
*/
public FlywheelSim(
public DCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double... measurementStdDevs) {
LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double... measurementStdDevs) {
super(plant, measurementStdDevs);
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearbox = gearbox;
// By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf,
// By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf,
// the flywheel state-space model is:
// the DC motor state-space model is:
//
//
// dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
// dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
// A = -G²Kₜ/(KᵥRJ)
// A = -G²Kₜ/(KᵥRJ)
// B = GKₜ/(RJ)
// B = GKₜ/(RJ)
//
//
// Solve for G.
// Solve for G.
//
//
// A/B = -G/Kᵥ
// A/B = -G/Kᵥ
// G = -KᵥA/B
// G = -KᵥA/B
//
//
// Solve for J.
// Solve for J.
//
//
// B = GKₜ/(RJ)
// B = GKₜ/(RJ)
// J = GKₜ/(RB)
// J = GKₜ/(RB)
m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(0, 0) / plant.getB(0, 0);
m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(1, 1) / plant.getB(1, 0);
m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(0, 0));
m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(1, 0));
}
}
/**
/**
* Sets the flywheel's angular velocity.
* Sets the state of the DC motor.
*
*
* @param velocityRadPerSec The new velocity in radians per second.
* @param angularPositionRad The new position in radians.
* @param angularVelocityRadPerSec The new velocity in radians per second.
*/
*/
public void setAngularVelocity(double velocityRadPerSec) {
public void setState(double angularPositionRad, double angularVelocityRadPerSec) {
setState(VecBuilder.fill(velocityRadPerSec));
setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec));
}
}
/**
/**
* Returns the gear ratio of the flywheel.
* Sets the DC motor's angular position.
*
*
* @return the flywheel's gear ratio.
* @param angularPositionRad The new position in radians.
*/
public void setAngle(double angularPositionRad) {
setState(angularPositionRad, getAngularVelocityRadPerSec());
}
/**
* Sets the DC motor's angular velocity.
*
* @param angularVelocityRadPerSec The new velocity in radians per second.
*/
public void setAngularVelocity(double angularVelocityRadPerSec) {
setState(getAngularPositionRad(), angularVelocityRadPerSec);
}
/**
* Returns the gear ratio of the DC motor.
*
* @return the DC motor's gear ratio.
*/
*/
public double getGearing() {
public double getGearing() {
return m_gearing;
return m_gearing;
}
}
/**
/**
* Returns the moment of inertia in kilograms meters squared.
* Returns the moment of inertia of the DC motor.
*
*
* @return The flywheel's moment of inertia.
* @return The DC motor's moment of inertia.
*/
*/
public double getJKgMetersSquared() {
public double getJKgMetersSquared() {
return m_jKgMetersSquared;
return m_jKgMetersSquared;
}
}
/**
/**
* Returns the gearbox for the flywheel.
* Returns the gearbox for the DC motor.
*
*
* @return The flywheel's gearbox.
* @return The DC motor's gearbox.
*/
*/
public DCMotor getGearbox() {
public DCMotor getGearbox() {
return m_gearbox;
return m_gearbox;
}
}
/**
/**
* Returns the flywheel's velocity in Radians Per Second.
* Returns the DC motor's position.
*
*
* @return The flywheel's velocity in Radians Per Second.
* @return The DC motor's position.
*/
*/
public double getAngularVelocityRadPerSec() {
public double getAngularPositionRad() {
return getOutput(0);
return getOutput(0);
}
}
/**
/**
* Returns the flywheel's velocity in RPM.
* Returns the DC motor's position in rotations.
*
*
* @return The flywheel's velocity in RPM.
* @return The DC motor's position in rotations.
*/
public double getAngularPositionRotations() {
return Units.radiansToRotations(getAngularPositionRad());
}
/**
* Returns the DC motor's position.
*
* @return The DC motor's position
*/
public Angle getAngularPosition() {
m_angle.mut_setMagnitude(getAngularPositionRad());
return m_angle;
}
/**
* Returns the DC motor's velocity.
*
* @return The DC motor's velocity.
*/
public double getAngularVelocityRadPerSec() {
return getOutput(1);
}
/**
* Returns the DC motor's velocity in RPM.
*
* @return The DC motor's velocity in RPM.
*/
*/
public double getAngularVelocityRPM() {
public double getAngularVelocityRPM() {
return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
}
}
/**
/**
* Returns the flywheel's velocity.
* Returns the DC motor's velocity.
*
*
* @return The flywheel's velocity
* @return The DC motor's velocity
*/
*/
public AngularVelocity getAngularVelocity() {
public AngularVelocity getAngularVelocity() {
m_angularVelocity.mut_setMagnitude(getAngularVelocityRadPerSec());
m_angularVelocity.mut_setMagnitude(getAngularVelocityRadPerSec());
return m_angularVelocity;
return m_angularVelocity;
}
}
/**
/**
* Returns the flywheel's acceleration in Radians Per Second Squared.
* Returns the DC motor's acceleration in Radians Per Second Squared.
*
*
* @return The flywheel's acceleration in Radians Per Second Squared.
* @return The DC motor's acceleration in Radians Per Second Squared.
*/
*/
public double getAngularAccelerationRadPerSecSq() {
public double getAngularAccelerationRadPerSecSq() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(0, 0);
return acceleration.get(0, 0);
}
}
/**
/**
* Returns the flywheel's acceleration.
* Returns the DC motor's acceleration.
*
*
* @return The flywheel's acceleration.
* @return The DC motor's acceleration.
*/
*/
public AngularAcceleration getAngularAcceleration() {
public AngularAcceleration getAngularAcceleration() {
m_angularAcceleration.mut_setMagnitude(getAngularAccelerationRadPerSecSq());
m_angularAcceleration.mut_setMagnitude(getAngularAccelerationRadPerSecSq());
return m_angularAcceleration;
return m_angularAcceleration;
}
}
/**
/**
* Returns the flywheel's torque in Newton-Meters.
* Returns the DC motor's torque in Newton-Meters.
*
*
* @return The flywheel's torque in Newton-Meters.
* @return The DC motor's torque in Newton-Meters.
*/
*/
public double getTorqueNewtonMeters() {
public double getTorqueNewtonMeters() {
return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared;
return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared;
}
}
/**
/**
* Returns the flywheel's current draw.
* Returns the DC motor's current draw.
*
*
* @return The flywheel's current draw.
* @return The DC motor's current draw.
*/
*/
public double getCurrentDrawAmps() {
public double getCurrentDrawAmps() {
// I = V / R - omega / (Kv * R)
// I = V / R - omega / (Kv * R)
// Reductions are output over input, so a reduction of 2:1 means the motor is spinning
// Reductions are output over input, so a reduction of 2:1 means the motor is spinning
// 2x faster than the flywheel
// 2x faster than the output
return m_gearbox.getCurrent(m_x.get(0, 0) * m_gearing, m_u.get(0, 0))
return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0))
* Math.signum(m_u.get(0, 0));
* Math.signum(m_u.get(0, 0));
}
}
/**
/**
* Gets the input voltage for the flywheel.
* Gets the input voltage for the DC motor.
*
*
* @return The flywheel's input voltage.
* @return The DC motor's input voltage.
*/
*/
public double getInputVoltage() {
public double getInputVoltage() {
return getInput(0);
return getInput(0);
}
}
/**
/**
* Sets the input voltage for the flywheel.
* Sets the input voltage for the DC motor.
*
*
* @param volts The input voltage.
* @param volts The input voltage.
*/
*/
public void setInputVoltage(double volts) {
public void setInputVoltage(double volts) {
setInput(volts);
setInput(volts);
clampInput(RobotController.getBatteryVoltage());
clampInput(RobotController.getBatteryVoltage());
}
}
}
}