Package org.ironmaple.simulation.drivesims

Class SelfControlledSwerveDriveSimulation

java.lang.Object
org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation
public class SelfControlledSwerveDriveSimulation extends Object

An easier way to simulate swerve drive.

Check Online Documentation

This class owns and controls a SwerveDriveSimulation, running closed loops/open loops on the simulated motors.

It works identically to how the real swerve simulation code.

Note: The order for swerve modules is: front-left, front-right, back-left, back-right.

  • Constructor Details

    • SelfControlledSwerveDriveSimulation

      public SelfControlledSwerveDriveSimulation(SwerveDriveSimulation swerveDriveSimulation)

      Default Constructor.

      Constructs a simplified swerve simulation with default standard deviations for odometry & vision pose estimates.

      Odometry is simulated as high-frequency odometry, assuming a robot period of 0.02 seconds and an odometry frequency of 250 Hz.

      Parameters:
      swerveDriveSimulation - the SwerveDriveSimulation to control.

    • SelfControlledSwerveDriveSimulation

      public SelfControlledSwerveDriveSimulation(SwerveDriveSimulation swerveDriveSimulation, Matrix<N3,N1> stateStdDevs, Matrix<N3,N1> visionMeasurementStdDevs)

      Constructs an instance with given odometry standard deviations.

      Constructs a simplified swerve simulation with specified standard deviations for odometry & vision pose estimates.

      Parameters:
      swerveDriveSimulation - the SwerveDriveSimulation to control.
      stateStdDevs - the standard deviations for odometry encoders.
      visionMeasurementStdDevs - the standard deviations for vision pose estimates.

  • Method Details

    • periodic

      public void periodic()

      Periodic Method for Simplified Swerve Sim.

      Call this method in the Subsystem.periodic() of your swerve subsystem.

      Updates the odometry by fetching cached inputs.

    • getLatestModulePositions

      public SwerveModulePosition[] getLatestModulePositions()

      Obtains the LATEST module positions measured by the encoders.

      The order for swerve modules is: front-left, front-right, back-left, back-right.

      Returns:
      the module positions

    • getCachedModulePositions

      public SwerveModulePosition[][] getCachedModulePositions()

      Obtains the CACHED module positions measured by the encoders.

      This simulates high-frequency odometry.

      The module positions, or the value of getLatestModulePositions() are cached in every simulation sub-tick.

      The array is ordered in a [timeStampIndex][moduleIndex] format.

      The order for swerve modules is: front-left, front-right, back-left, back-right.

      Returns:
      the cached module positions

    • getRawGyroAngle

      @Deprecated public Rotation2d getRawGyroAngle()
      Deprecated.
      This rotation is NOT the actual facing of the robot; it is uncalibrated and is only used for features like rotation lock.

      Obtains the raw angle of the gyro.

      Note that the simulated gyro also drifts/skids like real gyros, especially if the robot collides.

      To obtain the facing of the robot retrieved from the odometry, use getOdometryEstimatedPose(); to obtain the actual facing of the robot, use getActualPoseInSimulationWorld().

      Returns:
      the raw (uncalibrated) angle of the simulated gyro.

    • getOdometryEstimatedPose

      public Pose2d getOdometryEstimatedPose()

      Obtains the robot pose measured by the odometry.

      Retrieves the pose estimated by the odometry system (and vision, if applicable).

      This method wraps around PoseEstimator.getEstimatedPosition().

      This represents the estimated position of the robot.

      Note that this estimation includes realistic simulations of measurement errors due to skidding and odometry drift.

      To obtain the ACTUAL pose of the robot, with no measurement errors, use getActualPoseInSimulationWorld().

    • resetOdometry

      public void resetOdometry(Pose2d pose)

      Resets the odometry to a specified position.

      This method wraps around SwerveDrivePoseEstimator#resetPosition(Rotation2d, SwerveModulePosition[], Pose2d).

      It resets the position of the pose estimator to the given pose.

      Parameters:
      pose - The position on the field where the robot is located.

    • addVisionEstimation

      public void addVisionEstimation(Pose2d robotPoseMeters, double timeStampSeconds)

      Adds a vision estimation to the pose estimator.

      This method wraps around PoseEstimator.addVisionMeasurement(Pose2d, double).

      Adds a vision measurement to the Kalman Filter, correcting the odometry pose estimate while accounting for measurement noise.

      Parameters:
      robotPoseMeters - The pose of the robot as measured by the vision camera.
      timeStampSeconds - The timestamp of the vision measurement, in seconds.

    • addVisionEstimation

      public void addVisionEstimation(Pose2d robotPoseMeters, double timeStampSeconds, Matrix<N3,N1> measurementStdDevs)

      Adds a vision estimation to the pose estimator.

      This method wraps around PoseEstimator.addVisionMeasurement(Pose2d, double, Matrix).

      Adds a vision measurement to the Kalman Filter, correcting the odometry pose estimate while accounting for measurement noise.

      Parameters:
      robotPoseMeters - The pose of the robot as measured by the vision camera.
      timeStampSeconds - The timestamp of the vision measurement, in seconds.
      measurementStdDevs - Standard deviations of the vision pose measurement (x position in meters, y position in meters, and heading in radians). Increase these values to reduce the trust in the vision pose measurement.

    • runChassisSpeeds

      public void runChassisSpeeds(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters, boolean fieldCentricDrive, boolean discretizeSpeeds)

      Runs chassis speeds on the simulated swerve drive.

      Runs the specified chassis speeds, either robot-centric or field-centric.

      Parameters:
      chassisSpeeds - The speeds to run, in either robot-centric or field-centric coordinates.
      centerOfRotationMeters - The center of rotation. For example, if you set the center of rotation at one corner of the robot and provide a chassis speed that has only a dtheta component, the robot will rotate around that corner.
      fieldCentricDrive - Whether to execute field-centric drive with the provided speed.
      discretizeSpeeds - Whether to apply ChassisSpeeds.discretize(double) to the provided speed.

    • runSwerveStates

      public void runSwerveStates(SwerveModuleState[] setPoints)

      Runs a raw module states on the chassis.

      Runs the specified module states on the modules.

      Parameters:
      setPoints - an array of SwerveModuleState yielding the requested states

    • getMeasuredStates

      public SwerveModuleState[] getMeasuredStates()

      Obtain the MEASURED swerve states.

      The order of the swerve modules is: front-left, front-right, back-left, back-right.

      Returns:
      The actual measured swerve states of the simulated swerve.

    • getSetPointsOptimized

      public SwerveModuleState[] getSetPointsOptimized()

      Obtain the optimized SETPOINTS of the swerve.

      The setpoints are calculated using SwerveDriveKinematics.toSwerveModuleStates(ChassisSpeeds) in the most recent call to runChassisSpeeds(ChassisSpeeds, Translation2d, boolean, boolean).

      The setpoints are optimized using SwerveModuleState.optimize(SwerveModuleState, Rotation2d).

      The order of the swerve modules is: front-left, front-right, back-left, back-right.

      Returns:
      The optimized setpoints of the swerve, calculated during the last chassis speed run.

    • getMeasuredSpeedsFieldRelative

      public ChassisSpeeds getMeasuredSpeedsFieldRelative(boolean useGyroForAngularVelocity)

      Obtain the field-relative chassis speeds measured from the encoders.

      The speeds are measured from the simulated swerve modules.

      Parameters:
      useGyroForAngularVelocity - Whether to use the gyro for a more accurate angular velocity measurement.
      Returns:
      The measured chassis speeds, field-relative.

    • getMeasuredSpeedsRobotRelative

      public ChassisSpeeds getMeasuredSpeedsRobotRelative(boolean useGyroForAngularVelocity)

      Obtain the robot-relative chassis speeds measured from the encoders.

      The speeds are measured from the simulated swerve modules.

      Parameters:
      useGyroForAngularVelocity - Whether to use the gyro for a more accurate angular velocity measurement.
      Returns:
      The measured chassis speeds, robot-relative.

    • getDriveTrainSimulation

      public SwerveDriveSimulation getDriveTrainSimulation()

      Obtain the SwerveDriveSimulation object controlled by this simplified swerve simulation.

      Returns:
      The swerve drive simulation.

    • getActualPoseInSimulationWorld

      public Pose2d getActualPoseInSimulationWorld()

      Obtain the ACTUAL robot pose.

      Obtains the ACTUAL robot pose with zero measurement error.

      To obtain the pose calculated by odometry (where the robot thinks it is), use getOdometryEstimatedPose().

    • getActualSpeedsFieldRelative

      public ChassisSpeeds getActualSpeedsFieldRelative()

      Get the ACTUAL field-relative chassis speeds of the robot.

      Wraps around AbstractDriveTrainSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative().

      Returns:
      the actual chassis speeds in the simulation world, field-relative

    • getActualSpeedsRobotRelative

      public ChassisSpeeds getActualSpeedsRobotRelative()

      Get the ACTUAL robot-relative chassis speeds of the robot.

      Wraps around AbstractDriveTrainSimulation.getDriveTrainSimulatedChassisSpeedsRobotRelative().

      Returns:
      the actual chassis speeds in the simulation world, robot-relative

    • setSimulationWorldPose

      public void setSimulationWorldPose(Pose2d robotPose)

      Teleport the robot to a specified location on the simulated field.

      This method moves the drivetrain instantly to the specified location on the field, bypassing any obstacles in its path.

      Wraps around AbstractDriveTrainSimulation.setSimulationWorldPose(Pose2d).

      Parameters:
      robotPose - the pose of the robot to teleport to

    • withSteerPID

      public SelfControlledSwerveDriveSimulation withSteerPID(PIDController steerController)

    • withCurrentLimits

      public SelfControlledSwerveDriveSimulation withCurrentLimits(edu.wpi.first.units.measure.Current driveCurrentLimit, edu.wpi.first.units.measure.Current steerCurrentLimit)

    • maxLinearVelocity

      public edu.wpi.first.units.measure.LinearVelocity maxLinearVelocity()
      See Also:

    • maxLinearAcceleration

      public edu.wpi.first.units.measure.LinearAcceleration maxLinearAcceleration()
      See Also:

    • trackWidthY

      public edu.wpi.first.units.measure.Distance trackWidthY()
      See Also:

    • trackLengthX

      public edu.wpi.first.units.measure.Distance trackLengthX()
      See Also:

    • driveBaseRadius

      public edu.wpi.first.units.measure.Distance driveBaseRadius()
      See Also:

    • maxAngularVelocity

      public edu.wpi.first.units.measure.AngularVelocity maxAngularVelocity()
      See Also:

    • maxAngularAcceleration

      public edu.wpi.first.units.measure.AngularAcceleration maxAngularAcceleration()
      See Also: