001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.wpilibj.simulation; 006 007import edu.wpi.first.math.VecBuilder; 008import edu.wpi.first.math.numbers.N1; 009import edu.wpi.first.math.numbers.N2; 010import edu.wpi.first.math.system.LinearSystem; 011import edu.wpi.first.math.system.plant.DCMotor; 012import edu.wpi.first.wpilibj.RobotController; 013 014/** Represents a simulated DC motor mechanism. */ 015public class DCMotorSim extends LinearSystemSim<N2, N1, N2> { 016 // Gearbox for the DC motor. 017 private final DCMotor m_gearbox; 018 019 // The gearing from the motors to the output. 020 private final double m_gearing; 021 022 // The moment of inertia for the DC motor mechanism in kg-m². 023 private final double m_j; 024 025 /** 026 * Creates a simulated DC motor mechanism. 027 * 028 * @param plant The linear system representing the DC motor. This system can be created with 029 * {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double, 030 * double)} or {@link 031 * edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}. If 032 * {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)} 033 * is used, the distance unit must be radians. 034 * @param gearbox The type of and number of motors in the DC motor gearbox. 035 * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no 036 * noise is desired. If present must have 2 elements. The first element is for position. The 037 * second element is for velocity. 038 */ 039 public DCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double... measurementStdDevs) { 040 super(plant, measurementStdDevs); 041 m_gearbox = gearbox; 042 043 // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf, 044 // the DC motor state-space model is: 045 // 046 // dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u 047 // A = -G²Kₜ/(KᵥRJ) 048 // B = GKₜ/(RJ) 049 // 050 // Solve for G. 051 // 052 // A/B = -G/Kᵥ 053 // G = -KᵥA/B 054 // 055 // Solve for J. 056 // 057 // B = GKₜ/(RJ) 058 // J = GKₜ/(RB) 059 m_gearing = -gearbox.Kv * plant.getA(1, 1) / plant.getB(1, 0); 060 m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(1, 0)); 061 } 062 063 /** 064 * Sets the state of the DC motor. 065 * 066 * @param angularPosition The new position in radians. 067 * @param angularVelocity The new velocity in radians per second. 068 */ 069 public void setState(double angularPosition, double angularVelocity) { 070 setState(VecBuilder.fill(angularPosition, angularVelocity)); 071 } 072 073 /** 074 * Sets the DC motor's angular position. 075 * 076 * @param angularPosition The new position in radians. 077 */ 078 public void setAngle(double angularPosition) { 079 setState(angularPosition, getAngularVelocity()); 080 } 081 082 /** 083 * Sets the DC motor's angular velocity. 084 * 085 * @param angularVelocity The new velocity in radians per second. 086 */ 087 public void setAngularVelocity(double angularVelocity) { 088 setState(getAngularPosition(), angularVelocity); 089 } 090 091 /** 092 * Returns the gear ratio of the DC motor. 093 * 094 * @return the DC motor's gear ratio. 095 */ 096 public double getGearing() { 097 return m_gearing; 098 } 099 100 /** 101 * Returns the moment of inertia of the DC motor. 102 * 103 * @return The DC motor's moment of inertia in kg-m². 104 */ 105 public double getJ() { 106 return m_j; 107 } 108 109 /** 110 * Returns the gearbox for the DC motor. 111 * 112 * @return The DC motor's gearbox. 113 */ 114 public DCMotor getGearbox() { 115 return m_gearbox; 116 } 117 118 /** 119 * Returns the DC motor's position. 120 * 121 * @return The DC motor's position in radians. 122 */ 123 public double getAngularPosition() { 124 return getOutput(0); 125 } 126 127 /** 128 * Returns the DC motor's velocity. 129 * 130 * @return The DC motor's velocity in radians per second. 131 */ 132 public double getAngularVelocity() { 133 return getOutput(1); 134 } 135 136 /** 137 * Returns the DC motor's acceleration. 138 * 139 * @return The DC motor's acceleration in rad/s². 140 */ 141 public double getAngularAcceleration() { 142 var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u)); 143 return acceleration.get(1, 0); 144 } 145 146 /** 147 * Returns the DC motor's torque in. 148 * 149 * @return The DC motor's torque in Newton-meters. 150 */ 151 public double getTorque() { 152 return getAngularAcceleration() * m_j; 153 } 154 155 /** 156 * Returns the DC motor's current draw. 157 * 158 * @return The DC motor's current draw in amps. 159 */ 160 public double getCurrentDraw() { 161 // I = V / R - omega / (Kv * R) 162 // Reductions are output over input, so a reduction of 2:1 means the motor is spinning 163 // 2x faster than the output 164 return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0)) 165 * Math.signum(m_u.get(0, 0)); 166 } 167 168 /** 169 * Gets the input voltage for the DC motor. 170 * 171 * @return The DC motor's input voltage. 172 */ 173 public double getInputVoltage() { 174 return getInput(0); 175 } 176 177 /** 178 * Sets the input voltage for the DC motor. 179 * 180 * @param volts The input voltage. 181 */ 182 public void setInputVoltage(double volts) { 183 setInput(volts); 184 clampInput(RobotController.getBatteryVoltage()); 185 } 186}