Class ElevatorSim

java.lang.Object
edu.wpi.first.wpilibj.simulation.LinearSystemSim<N2,N1,N2>
edu.wpi.first.wpilibj.simulation.ElevatorSim

public class ElevatorSim extends LinearSystemSim<N2,N1,N2>
Represents a simulated elevator mechanism.
  • Constructor Details

    • ElevatorSim

      public ElevatorSim(LinearSystem<N2,N1,N2> plant, DCMotor gearbox, double minHeight, double maxHeight, boolean simulateGravity, double startingHeight, double... measurementStdDevs)
      Creates a simulated elevator mechanism.
      Parameters:
      plant - The linear system that represents the elevator. This system can be created with LinearSystemId.createElevatorSystem(DCMotor, double, double, double).
      gearbox - The type of and number of motors in the elevator gearbox.
      minHeight - The min allowable height of the elevator in meters.
      maxHeight - The max allowable height of the elevator in meters.
      simulateGravity - Whether gravity should be simulated or not.
      startingHeight - The starting height of the elevator in meters.
      measurementStdDevs - The standard deviations of the measurements. Can be omitted if no noise is desired. If present must have 1 element for position.
    • ElevatorSim

      public ElevatorSim(double kV, double kA, DCMotor gearbox, double minHeight, double maxHeight, boolean simulateGravity, double startingHeight, 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.
      minHeight - The min allowable height of the elevator in meters.
      maxHeight - The max allowable height of the elevator in meters.
      simulateGravity - Whether gravity should be simulated or not.
      startingHeight - The starting height of the elevator in meters.
      measurementStdDevs - The standard deviations of the measurements. Can be omitted if no noise is desired. If present must have 1 element for position.
    • ElevatorSim

      public ElevatorSim(DCMotor gearbox, double gearing, double carriageMass, double drumRadius, double minHeight, double maxHeight, boolean simulateGravity, double startingHeight, 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).
      carriageMass - The mass of the elevator carriage in kg.
      drumRadius - The radius of the drum that the elevator spool is wrapped around in meters.
      minHeight - The min allowable height of the elevator in meters.
      maxHeight - The max allowable height of the elevator in meters.
      simulateGravity - Whether gravity should be simulated or not.
      startingHeight - The starting height of the elevator in meters.
      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 position, double velocity)
      Sets the elevator's state. The new position will be limited between the minimum and maximum allowed heights.
      Parameters:
      position - The new position in meters.
      velocity - New velocity in meters per second.
    • wouldHitLowerLimit

      public boolean wouldHitLowerLimit(double elevatorHeight)
      Returns whether the elevator would hit the lower limit.
      Parameters:
      elevatorHeight - The elevator height in meters.
      Returns:
      Whether the elevator would hit the lower limit.
    • wouldHitUpperLimit

      public boolean wouldHitUpperLimit(double elevatorHeight)
      Returns whether the elevator would hit the upper limit.
      Parameters:
      elevatorHeight - The elevator height in meters.
      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.
    • getPosition

      public double getPosition()
      Returns the position of the elevator.
      Returns:
      The position of the elevator in meters.
    • getVelocity

      public double getVelocity()
      Returns the velocity of the elevator.
      Returns:
      The velocity of the elevator in meters per second.
    • getCurrentDraw

      public double getCurrentDraw()
      Returns the elevator current draw.
      Returns:
      The elevator current draw in amps.
    • setInputVoltage

      public void setInputVoltage(double volts)
      Sets the input voltage for the elevator.
      Parameters:
      volts - The input voltage.
    • updateX

      protected Matrix<N2,N1> updateX(Matrix<N2,N1> currentXhat, Matrix<N1,N1> u, double dt)
      Updates the state of the elevator.
      Overrides:
      updateX in class LinearSystemSim<N2,N1,N2>
      Parameters:
      currentXhat - The current state estimate.
      u - The system inputs (voltage).
      dt - The time difference between controller updates in seconds.
      Returns:
      The new state.