Package com.flarerobotics.lib.sim
Class ElevatorSimTilted
java.lang.Object
edu.wpi.first.wpilibj.simulation.LinearSystemSim<edu.wpi.first.math.numbers.N2,edu.wpi.first.math.numbers.N1,edu.wpi.first.math.numbers.N2>
com.flarerobotics.lib.sim.ElevatorSimTilted
public class ElevatorSimTilted
extends edu.wpi.first.wpilibj.simulation.LinearSystemSim<edu.wpi.first.math.numbers.N2,edu.wpi.first.math.numbers.N1,edu.wpi.first.math.numbers.N2>
Represents a simulated elevator mechanism. Allows for the use of non-90 degree tilted
systems.
-
Field Summary
Fields inherited from class edu.wpi.first.wpilibj.simulation.LinearSystemSim
m_measurementStdDevs, m_plant, m_u, m_x, m_y
-
Constructor Summary
ConstructorsConstructorDescriptionElevatorSimTilted
(double kV, double kA, edu.wpi.first.math.system.plant.DCMotor gearbox, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, double... measurementStdDevs) Creates a simulated elevator mechanism.ElevatorSimTilted
(edu.wpi.first.math.system.LinearSystem<edu.wpi.first.math.numbers.N2, edu.wpi.first.math.numbers.N1, edu.wpi.first.math.numbers.N2> plant, edu.wpi.first.math.system.plant.DCMotor gearbox, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, double... measurementStdDevs) Creates a simulated elevator mechanism.ElevatorSimTilted
(edu.wpi.first.math.system.plant.DCMotor gearbox, double gearing, double carriageMassKg, double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, double... measurementStdDevs) Creates a simulated elevator mechanism. -
Method Summary
Modifier and TypeMethodDescriptiondouble
Returns the elevator current draw.double
Returns the position of the elevator.double
Returns the velocity of the elevator.boolean
Returns whether the elevator has hit the lower limit.boolean
Returns whether the elevator has hit the upper limit.void
setInputVoltage
(double volts) Sets the input voltage for the elevator.final void
setState
(double positionMeters, double velocityMetersPerSecond) Sets the elevator's state.void
setSystemTilt
(double angle) Sets the system's tilt to the given angle, defaults to 0 degrees for upright.protected edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2,
edu.wpi.first.math.numbers.N1> updateX
(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2, edu.wpi.first.math.numbers.N1> currentXhat, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N1, edu.wpi.first.math.numbers.N1> u, double dtSeconds) Updates the state of the elevator.boolean
wouldHitLowerLimit
(double elevatorHeightMeters) Returns whether the elevator would hit the lower limit.boolean
wouldHitUpperLimit
(double elevatorHeightMeters) Returns whether the elevator would hit the upper limit.Methods inherited from class edu.wpi.first.wpilibj.simulation.LinearSystemSim
clampInput, getInput, getInput, getOutput, getOutput, setInput, setInput, setInput, setState, update
-
Constructor Details
-
ElevatorSimTilted
public ElevatorSimTilted(edu.wpi.first.math.system.LinearSystem<edu.wpi.first.math.numbers.N2, edu.wpi.first.math.numbers.N1, edu.wpi.first.math.numbers.N2> plant, edu.wpi.first.math.system.plant.DCMotor gearbox, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, double... measurementStdDevs) Creates a simulated elevator mechanism.- Parameters:
plant
- The linear system that represents the elevator. This system can be created withLinearSystemId.createElevatorSystem(DCMotor, double, double, double)
.gearbox
- The type of and number of motors in the elevator gearbox.minHeightMeters
- The min allowable height of the elevator.maxHeightMeters
- The max allowable height of the elevator.simulateGravity
- Whether gravity should be simulated or not.startingHeightMeters
- The starting height of the elevator.measurementStdDevs
- The standard deviations of the measurements. Can be omitted if no noise is desired. If present must have 1 element for position.
-
ElevatorSimTilted
public ElevatorSimTilted(double kV, double kA, edu.wpi.first.math.system.plant.DCMotor gearbox, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, double... measurementStdDevs) Creates a simulated elevator mechanism.- Parameters:
kV
- The velocity gain.kA
- The acceleration gain.gearbox
- The type of and number of motors in the elevator gearbox.minHeightMeters
- The min allowable height of the elevator.maxHeightMeters
- The max allowable height of the elevator.simulateGravity
- Whether gravity should be simulated or not.startingHeightMeters
- The starting height of the elevator.measurementStdDevs
- The standard deviations of the measurements. Can be omitted if no noise is desired. If present must have 1 element for position.
-
ElevatorSimTilted
public ElevatorSimTilted(edu.wpi.first.math.system.plant.DCMotor gearbox, double gearing, double carriageMassKg, double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, double... measurementStdDevs) Creates a simulated elevator mechanism.- Parameters:
gearbox
- The type of and number of motors in the elevator gearbox.gearing
- The gearing of the elevator (numbers greater than 1 represent reductions).carriageMassKg
- The mass of the elevator carriage.drumRadiusMeters
- The radius of the drum that the elevator spool is wrapped around.minHeightMeters
- The min allowable height of the elevator.maxHeightMeters
- The max allowable height of the elevator.simulateGravity
- Whether gravity should be simulated or not.startingHeightMeters
- The starting height of the elevator.measurementStdDevs
- The standard deviations of the measurements. Can be omitted if no noise is desired. If present must have 1 element for position.
-
-
Method Details
-
setState
public final void setState(double positionMeters, double velocityMetersPerSecond) Sets the elevator's state. The new position will be limited between the minimum and maximum allowed heights.- Parameters:
positionMeters
- The new position in meters.velocityMetersPerSecond
- New velocity in meters per second.
-
setSystemTilt
public void setSystemTilt(double angle) Sets the system's tilt to the given angle, defaults to 0 degrees for upright.- Parameters:
angle
- The tilt angle in degrees.
-
wouldHitLowerLimit
public boolean wouldHitLowerLimit(double elevatorHeightMeters) Returns whether the elevator would hit the lower limit.- Parameters:
elevatorHeightMeters
- The elevator height.- Returns:
- Whether the elevator would hit the lower limit.
-
wouldHitUpperLimit
public boolean wouldHitUpperLimit(double elevatorHeightMeters) Returns whether the elevator would hit the upper limit.- Parameters:
elevatorHeightMeters
- The elevator height.- Returns:
- Whether the elevator would hit the upper limit.
-
hasHitLowerLimit
public boolean hasHitLowerLimit()Returns whether the elevator has hit the lower limit.- Returns:
- Whether the elevator has hit the lower limit.
-
hasHitUpperLimit
public boolean hasHitUpperLimit()Returns whether the elevator has hit the upper limit.- Returns:
- Whether the elevator has hit the upper limit.
-
getPositionMeters
public double getPositionMeters()Returns the position of the elevator.- Returns:
- The position of the elevator.
-
getVelocityMetersPerSecond
public double getVelocityMetersPerSecond()Returns the velocity of the elevator.- Returns:
- The velocity of the elevator.
-
getCurrentDrawAmps
public double getCurrentDrawAmps()Returns the elevator current draw.- Returns:
- The elevator current draw.
-
setInputVoltage
public void setInputVoltage(double volts) Sets the input voltage for the elevator.- Parameters:
volts
- The input voltage.
-
updateX
protected edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2,edu.wpi.first.math.numbers.N1> updateX(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2, edu.wpi.first.math.numbers.N1> currentXhat, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N1, edu.wpi.first.math.numbers.N1> u, double dtSeconds) Updates the state of the elevator.- Overrides:
updateX
in classedu.wpi.first.wpilibj.simulation.LinearSystemSim<edu.wpi.first.math.numbers.N2,
edu.wpi.first.math.numbers.N1, edu.wpi.first.math.numbers.N2> - Parameters:
currentXhat
- The current state estimate.u
- The system inputs (voltage).dtSeconds
- The time difference between controller updates.
-