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.MathUtil; 008import edu.wpi.first.math.Matrix; 009import edu.wpi.first.math.VecBuilder; 010import edu.wpi.first.math.numbers.N1; 011import edu.wpi.first.math.numbers.N2; 012import edu.wpi.first.math.system.LinearSystem; 013import edu.wpi.first.math.system.NumericalIntegration; 014import edu.wpi.first.math.system.plant.DCMotor; 015import edu.wpi.first.math.system.plant.LinearSystemId; 016import edu.wpi.first.wpilibj.RobotController; 017 018/** Represents a simulated elevator mechanism. */ 019public class ElevatorSim extends LinearSystemSim<N2, N1, N2> { 020 // Gearbox for the elevator. 021 private final DCMotor m_gearbox; 022 023 // The min allowable height for the elevator. 024 private final double m_minHeight; 025 026 // The max allowable height for the elevator. 027 private final double m_maxHeight; 028 029 // Whether the simulator should simulate gravity. 030 private final boolean m_simulateGravity; 031 032 /** 033 * Creates a simulated elevator mechanism. 034 * 035 * @param plant The linear system that represents the elevator. This system can be created with 036 * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, 037 * double, double)}. 038 * @param gearbox The type of and number of motors in the elevator gearbox. 039 * @param minHeight The min allowable height of the elevator in meters. 040 * @param maxHeight The max allowable height of the elevator in meters. 041 * @param simulateGravity Whether gravity should be simulated or not. 042 * @param startingHeight The starting height of the elevator in meters. 043 * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no 044 * noise is desired. If present must have 1 element for position. 045 */ 046 @SuppressWarnings("this-escape") 047 public ElevatorSim( 048 LinearSystem<N2, N1, N2> plant, 049 DCMotor gearbox, 050 double minHeight, 051 double maxHeight, 052 boolean simulateGravity, 053 double startingHeight, 054 double... measurementStdDevs) { 055 super(plant, measurementStdDevs); 056 m_gearbox = gearbox; 057 m_minHeight = minHeight; 058 m_maxHeight = maxHeight; 059 m_simulateGravity = simulateGravity; 060 061 setState(startingHeight, 0); 062 } 063 064 /** 065 * Creates a simulated elevator mechanism. 066 * 067 * @param kV The velocity gain. 068 * @param kA The acceleration gain. 069 * @param gearbox The type of and number of motors in the elevator gearbox. 070 * @param minHeight The min allowable height of the elevator in meters. 071 * @param maxHeight The max allowable height of the elevator in meters. 072 * @param simulateGravity Whether gravity should be simulated or not. 073 * @param startingHeight The starting height of the elevator in meters. 074 * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no 075 * noise is desired. If present must have 1 element for position. 076 */ 077 public ElevatorSim( 078 double kV, 079 double kA, 080 DCMotor gearbox, 081 double minHeight, 082 double maxHeight, 083 boolean simulateGravity, 084 double startingHeight, 085 double... measurementStdDevs) { 086 this( 087 LinearSystemId.identifyPositionSystem(kV, kA), 088 gearbox, 089 minHeight, 090 maxHeight, 091 simulateGravity, 092 startingHeight, 093 measurementStdDevs); 094 } 095 096 /** 097 * Creates a simulated elevator mechanism. 098 * 099 * @param gearbox The type of and number of motors in the elevator gearbox. 100 * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). 101 * @param carriageMass The mass of the elevator carriage in kg. 102 * @param drumRadius The radius of the drum that the elevator spool is wrapped around in meters. 103 * @param minHeight The min allowable height of the elevator in meters. 104 * @param maxHeight The max allowable height of the elevator in meters. 105 * @param simulateGravity Whether gravity should be simulated or not. 106 * @param startingHeight The starting height of the elevator in meters. 107 * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no 108 * noise is desired. If present must have 1 element for position. 109 */ 110 public ElevatorSim( 111 DCMotor gearbox, 112 double gearing, 113 double carriageMass, 114 double drumRadius, 115 double minHeight, 116 double maxHeight, 117 boolean simulateGravity, 118 double startingHeight, 119 double... measurementStdDevs) { 120 this( 121 LinearSystemId.createElevatorSystem(gearbox, carriageMass, drumRadius, gearing), 122 gearbox, 123 minHeight, 124 maxHeight, 125 simulateGravity, 126 startingHeight, 127 measurementStdDevs); 128 } 129 130 /** 131 * Sets the elevator's state. The new position will be limited between the minimum and maximum 132 * allowed heights. 133 * 134 * @param position The new position in meters. 135 * @param velocity New velocity in meters per second. 136 */ 137 public final void setState(double position, double velocity) { 138 setState(VecBuilder.fill(MathUtil.clamp(position, m_minHeight, m_maxHeight), velocity)); 139 } 140 141 /** 142 * Returns whether the elevator would hit the lower limit. 143 * 144 * @param elevatorHeight The elevator height in meters. 145 * @return Whether the elevator would hit the lower limit. 146 */ 147 public boolean wouldHitLowerLimit(double elevatorHeight) { 148 return elevatorHeight <= this.m_minHeight; 149 } 150 151 /** 152 * Returns whether the elevator would hit the upper limit. 153 * 154 * @param elevatorHeight The elevator height in meters. 155 * @return Whether the elevator would hit the upper limit. 156 */ 157 public boolean wouldHitUpperLimit(double elevatorHeight) { 158 return elevatorHeight >= this.m_maxHeight; 159 } 160 161 /** 162 * Returns whether the elevator has hit the lower limit. 163 * 164 * @return Whether the elevator has hit the lower limit. 165 */ 166 public boolean hasHitLowerLimit() { 167 return wouldHitLowerLimit(getPosition()); 168 } 169 170 /** 171 * Returns whether the elevator has hit the upper limit. 172 * 173 * @return Whether the elevator has hit the upper limit. 174 */ 175 public boolean hasHitUpperLimit() { 176 return wouldHitUpperLimit(getPosition()); 177 } 178 179 /** 180 * Returns the position of the elevator. 181 * 182 * @return The position of the elevator in meters. 183 */ 184 public double getPosition() { 185 return getOutput(0); 186 } 187 188 /** 189 * Returns the velocity of the elevator. 190 * 191 * @return The velocity of the elevator in meters per second. 192 */ 193 public double getVelocity() { 194 return getOutput(1); 195 } 196 197 /** 198 * Returns the elevator current draw. 199 * 200 * @return The elevator current draw in amps. 201 */ 202 public double getCurrentDraw() { 203 // I = V / R - omega / (Kv * R) 204 // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is 205 // spinning 10x faster than the output 206 // v = r w, so w = v/r 207 double kA = 1 / m_plant.getB().get(1, 0); 208 double kV = -m_plant.getA().get(1, 1) * kA; 209 double motorVelocity = m_x.get(1, 0) * kV * m_gearbox.Kv; 210 var appliedVoltage = m_u.get(0, 0); 211 return m_gearbox.getCurrent(motorVelocity, appliedVoltage) * Math.signum(appliedVoltage); 212 } 213 214 /** 215 * Sets the input voltage for the elevator. 216 * 217 * @param volts The input voltage. 218 */ 219 public void setInputVoltage(double volts) { 220 setInput(volts); 221 clampInput(RobotController.getBatteryVoltage()); 222 } 223 224 /** 225 * Updates the state of the elevator. 226 * 227 * @param currentXhat The current state estimate. 228 * @param u The system inputs (voltage). 229 * @param dt The time difference between controller updates in seconds. 230 */ 231 @Override 232 protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dt) { 233 // Calculate updated x-hat from Runge-Kutta. 234 var updatedXhat = 235 NumericalIntegration.rkdp( 236 (x, _u) -> { 237 Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); 238 if (m_simulateGravity) { 239 xdot = xdot.plus(VecBuilder.fill(0, -9.8)); 240 } 241 return xdot; 242 }, 243 currentXhat, 244 u, 245 dt); 246 247 // We check for collisions after updating x-hat. 248 if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { 249 return VecBuilder.fill(m_minHeight, 0); 250 } 251 if (wouldHitUpperLimit(updatedXhat.get(0, 0))) { 252 return VecBuilder.fill(m_maxHeight, 0); 253 } 254 return updatedXhat; 255 } 256}