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 minHeightMeters The min allowable height of the elevator. 040 * @param maxHeightMeters The max allowable height of the elevator. 041 * @param simulateGravity Whether gravity should be simulated or not. 042 * @param startingHeightMeters The starting height of the elevator. 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 minHeightMeters, 051 double maxHeightMeters, 052 boolean simulateGravity, 053 double startingHeightMeters, 054 double... measurementStdDevs) { 055 super(plant, measurementStdDevs); 056 m_gearbox = gearbox; 057 m_minHeight = minHeightMeters; 058 m_maxHeight = maxHeightMeters; 059 m_simulateGravity = simulateGravity; 060 061 setState(startingHeightMeters, 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 minHeightMeters The min allowable height of the elevator. 071 * @param maxHeightMeters The max allowable height of the elevator. 072 * @param simulateGravity Whether gravity should be simulated or not. 073 * @param startingHeightMeters The starting height of the elevator. 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 minHeightMeters, 082 double maxHeightMeters, 083 boolean simulateGravity, 084 double startingHeightMeters, 085 double... measurementStdDevs) { 086 this( 087 LinearSystemId.identifyPositionSystem(kV, kA), 088 gearbox, 089 minHeightMeters, 090 maxHeightMeters, 091 simulateGravity, 092 startingHeightMeters, 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 carriageMassKg The mass of the elevator carriage. 102 * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. 103 * @param minHeightMeters The min allowable height of the elevator. 104 * @param maxHeightMeters The max allowable height of the elevator. 105 * @param simulateGravity Whether gravity should be simulated or not. 106 * @param startingHeightMeters The starting height of the elevator. 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 carriageMassKg, 114 double drumRadiusMeters, 115 double minHeightMeters, 116 double maxHeightMeters, 117 boolean simulateGravity, 118 double startingHeightMeters, 119 double... measurementStdDevs) { 120 this( 121 LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), 122 gearbox, 123 minHeightMeters, 124 maxHeightMeters, 125 simulateGravity, 126 startingHeightMeters, 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 positionMeters The new position in meters. 135 * @param velocityMetersPerSecond New velocity in meters per second. 136 */ 137 public final void setState(double positionMeters, double velocityMetersPerSecond) { 138 setState( 139 VecBuilder.fill( 140 MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond)); 141 } 142 143 /** 144 * Returns whether the elevator would hit the lower limit. 145 * 146 * @param elevatorHeightMeters The elevator height. 147 * @return Whether the elevator would hit the lower limit. 148 */ 149 public boolean wouldHitLowerLimit(double elevatorHeightMeters) { 150 return elevatorHeightMeters <= this.m_minHeight; 151 } 152 153 /** 154 * Returns whether the elevator would hit the upper limit. 155 * 156 * @param elevatorHeightMeters The elevator height. 157 * @return Whether the elevator would hit the upper limit. 158 */ 159 public boolean wouldHitUpperLimit(double elevatorHeightMeters) { 160 return elevatorHeightMeters >= this.m_maxHeight; 161 } 162 163 /** 164 * Returns whether the elevator has hit the lower limit. 165 * 166 * @return Whether the elevator has hit the lower limit. 167 */ 168 public boolean hasHitLowerLimit() { 169 return wouldHitLowerLimit(getPositionMeters()); 170 } 171 172 /** 173 * Returns whether the elevator has hit the upper limit. 174 * 175 * @return Whether the elevator has hit the upper limit. 176 */ 177 public boolean hasHitUpperLimit() { 178 return wouldHitUpperLimit(getPositionMeters()); 179 } 180 181 /** 182 * Returns the position of the elevator. 183 * 184 * @return The position of the elevator. 185 */ 186 public double getPositionMeters() { 187 return getOutput(0); 188 } 189 190 /** 191 * Returns the velocity of the elevator. 192 * 193 * @return The velocity of the elevator. 194 */ 195 public double getVelocityMetersPerSecond() { 196 return getOutput(1); 197 } 198 199 /** 200 * Returns the elevator current draw. 201 * 202 * @return The elevator current draw. 203 */ 204 public double getCurrentDrawAmps() { 205 // I = V / R - omega / (Kv * R) 206 // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is 207 // spinning 10x faster than the output 208 // v = r w, so w = v/r 209 double kA = 1 / m_plant.getB().get(1, 0); 210 double kV = -m_plant.getA().get(1, 1) * kA; 211 double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.KvRadPerSecPerVolt; 212 var appliedVoltage = m_u.get(0, 0); 213 return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) 214 * Math.signum(appliedVoltage); 215 } 216 217 /** 218 * Sets the input voltage for the elevator. 219 * 220 * @param volts The input voltage. 221 */ 222 public void setInputVoltage(double volts) { 223 setInput(volts); 224 clampInput(RobotController.getBatteryVoltage()); 225 } 226 227 /** 228 * Updates the state of the elevator. 229 * 230 * @param currentXhat The current state estimate. 231 * @param u The system inputs (voltage). 232 * @param dtSeconds The time difference between controller updates. 233 */ 234 @Override 235 protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) { 236 // Calculate updated x-hat from Runge-Kutta. 237 var updatedXhat = 238 NumericalIntegration.rkdp( 239 (x, _u) -> { 240 Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); 241 if (m_simulateGravity) { 242 xdot = xdot.plus(VecBuilder.fill(0, -9.8)); 243 } 244 return xdot; 245 }, 246 currentXhat, 247 u, 248 dtSeconds); 249 250 // We check for collisions after updating x-hat. 251 if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { 252 return VecBuilder.fill(m_minHeight, 0); 253 } 254 if (wouldHitUpperLimit(updatedXhat.get(0, 0))) { 255 return VecBuilder.fill(m_maxHeight, 0); 256 } 257 return updatedXhat; 258 } 259}