Package edu.wpi.first.wpilibj.simulation

Class DifferentialDrivetrainSim

java.lang.Object
edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim
public class DifferentialDrivetrainSim extends Object
This class simulates the state of the drivetrain. In simulationPeriodic, users should first set inputs from motors with setInputs(double, double), call update(double) to update the simulation, and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use Field2d to visualize their robot on the Sim GUI's field.

Our state-space system is:

x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]ᵀ in the field coordinate system (dist_* are wheel distances.)

u = [[voltage_l, voltage_r]]ᵀ This is typically the control input of the last timestep from a LTVDiffDriveController.

y = x

  • Constructor Details

    • DifferentialDrivetrainSim

      public DifferentialDrivetrainSim(DCMotor driveMotor, double gearing, double j, double mass, double wheelRadius, double trackwidth, Matrix<N7,N1> measurementStdDevs)
      Creates a simulated differential drivetrain.
      Parameters:
      driveMotor - A DCMotor representing the left side of the drivetrain.
      gearing - The gearing ratio between motor and wheel, as output over input. This must be the same ratio as the ratio used to identify or create the drivetrainPlant.
      j - The moment of inertia of the drivetrain about its center in kg-m².
      mass - The mass of the drivebase in kg.
      wheelRadius - The radius of the wheels on the drivetrain in meters.
      trackwidth - The robot's trackwidth, or distance between left and right wheels, in meters.
      measurementStdDevs - Standard deviations for measurements, in the form [x, y, heading, left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting point.

    • DifferentialDrivetrainSim

      public DifferentialDrivetrainSim(LinearSystem<N2,N2,N2> plant, DCMotor driveMotor, double gearing, double trackwidth, double wheelRadius, Matrix<N7,N1> measurementStdDevs)
      Creates a simulated differential drivetrain.
      Parameters:
      plant - The LinearSystem representing the robot's drivetrain. This system can be created with LinearSystemId.createDrivetrainVelocitySystem(DCMotor, double, double, double, double, double) or LinearSystemId.identifyDrivetrainSystem(double, double, double, double).
      driveMotor - A DCMotor representing the drivetrain.
      gearing - The gearingRatio ratio of the robot, as output over input. This must be the same ratio as the ratio used to identify or create the drivetrainPlant.
      trackwidth - The distance between the two sides of the drivetrain in meters. Can be found with SysId.
      wheelRadius - The radius of the wheels on the drivetrain, in meters.
      measurementStdDevs - Standard deviations for measurements, in the form [x, y, heading, left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting point.

  • Method Details

    • setInputs

      public void setInputs(double leftVoltageVolts, double rightVoltageVolts)
      Sets the applied voltage to the drivetrain. Note that positive voltage must make that side of the drivetrain travel forward (+X).
      Parameters:
      leftVoltageVolts - The left voltage.
      rightVoltageVolts - The right voltage.

    • update

      public void update(double dt)
      Update the drivetrain states with the current time difference.
      Parameters:
      dt - the time difference in seconds

    • getHeading

      public Rotation2d getHeading()
      Returns the direction the robot is pointing.

      Note that this angle is counterclockwise-positive, while most gyros are clockwise positive.

      Returns:
      The direction the robot is pointing.

    • getPose

      public Pose2d getPose()
      Returns the current pose.
      Returns:
      The current pose.

    • getRightPosition

      public double getRightPosition()
      Get the right encoder position.
      Returns:
      The encoder position in meters.

    • getRightVelocity

      public double getRightVelocity()
      Get the right encoder velocity.
      Returns:
      The encoder velocity in meters per second.

    • getLeftPosition

      public double getLeftPosition()
      Get the left encoder position.
      Returns:
      The encoder position in meters.

    • getLeftVelocity

      public double getLeftVelocity()
      Get the left encoder velocity.
      Returns:
      The encoder velocity in meters per second.

    • getLeftCurrentDraw

      public double getLeftCurrentDraw()
      Get the current draw of the left side of the drivetrain.
      Returns:
      the drivetrain's left side current draw, in amps

    • getRightCurrentDraw

      public double getRightCurrentDraw()
      Get the current draw of the right side of the drivetrain.
      Returns:
      the drivetrain's right side current draw, in amps

    • getCurrentDraw

      public double getCurrentDraw()
      Get the current draw of the drivetrain.
      Returns:
      the current draw, in amps

    • getCurrentGearing

      public double getCurrentGearing()
      Get the drivetrain gearing.
      Returns:
      the gearing ration

    • setCurrentGearing

      public void setCurrentGearing(double newGearRatio)
      Sets the gearing reduction on the drivetrain. This is commonly used for shifting drivetrains.
      Parameters:
      newGearRatio - The new gear ratio, as output over input.

    • setState

      public void setState(Matrix<N7,N1> state)
      Sets the system state.
      Parameters:
      state - The state.

    • setPose

      public void setPose(Pose2d pose)
      Sets the system pose.
      Parameters:
      pose - The pose.

    • getDynamics

      protected Matrix<N7,N1> getDynamics(Matrix<N7,N1> x, Matrix<N2,N1> u)
      The differential drive dynamics function.
      Parameters:
      x - The state.
      u - The input.
      Returns:
      The state derivative with respect to time.

    • clampInput

      protected Matrix<N2,N1> clampInput(Matrix<N2,N1> u)
      Clamp the input vector such that no element exceeds the battery voltage. If any does, the relative magnitudes of the input will be maintained.
      Parameters:
      u - The input vector.
      Returns:
      The normalized input.

    • createKitbotSim

      public static DifferentialDrivetrainSim createKitbotSim(DifferentialDrivetrainSim.KitbotMotor motor, DifferentialDrivetrainSim.KitbotGearing gearing, DifferentialDrivetrainSim.KitbotWheelSize wheelSize, Matrix<N7,N1> measurementStdDevs)
      Create a sim for the standard FRC kitbot.
      Parameters:
      motor - The motors installed in the bot.
      gearing - The gearing reduction used.
      wheelSize - The wheel size.
      measurementStdDevs - Standard deviations for measurements, in the form [x, y, heading, left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting point.
      Returns:
      A sim for the standard FRC kitbot.

    • createKitbotSim

      public static DifferentialDrivetrainSim createKitbotSim(DifferentialDrivetrainSim.KitbotMotor motor, DifferentialDrivetrainSim.KitbotGearing gearing, DifferentialDrivetrainSim.KitbotWheelSize wheelSize, double j, Matrix<N7,N1> measurementStdDevs)
      Create a sim for the standard FRC kitbot.
      Parameters:
      motor - The motors installed in the bot.
      gearing - The gearing reduction used.
      wheelSize - The wheel size.
      j - The moment of inertia of the drivebase in kg-m². This can be calculated using SysId.
      measurementStdDevs - Standard deviations for measurements, in the form [x, y, heading, left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting point.
      Returns:
      A sim for the standard FRC kitbot.