Package swervelib

Class SwerveDrive

java.lang.Object
swervelib.SwerveDrive
All Implemented Interfaces:
AutoCloseable
public class SwerveDrive extends Object implements AutoCloseable
Swerve Drive class representing and controlling the swerve drive.

  • Field Details

    • kinematics

      public final SwerveDriveKinematics kinematics
      Swerve Kinematics object.

    • swerveDriveConfiguration

      public final SwerveDriveConfiguration swerveDriveConfiguration
      Swerve drive configuration.

    • swerveDrivePoseEstimator

      public final SwerveDrivePoseEstimator swerveDrivePoseEstimator
      Swerve odometry.

    • imuReadingCache

      public final Cache<Rotation3d> imuReadingCache
      IMU reading cache for robot readings.

    • field

      public Field2d field
      Field object.

    • swerveController

      public SwerveController swerveController
      Swerve controller for controlling heading of the robot.

    • chassisVelocityCorrection

      public boolean chassisVelocityCorrection
      Correct chassis velocity in drive(Translation2d, double, boolean, boolean) using 254's correction.

    • autonomousChassisVelocityCorrection

      public boolean autonomousChassisVelocityCorrection
      Correct chassis velocity in setChassisSpeeds(ChassisSpeeds chassisSpeeds) (auto) using 254's correction during auto.

    • angularVelocityCorrection

      public boolean angularVelocityCorrection
      Correct for skew that scales with angular velocity in drive(Translation2d, double, boolean, boolean)

    • autonomousAngularVelocityCorrection

      public boolean autonomousAngularVelocityCorrection
      Correct for skew that scales with angular velocity in setChassisSpeeds(ChassisSpeeds chassisSpeeds) during auto.

    • angularVelocityCoefficient

      public double angularVelocityCoefficient
      Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).

    • headingCorrection

      public boolean headingCorrection
      Whether to correct heading when driving translationally. Set to true to enable.

  • Constructor Details

  • Method Details

    • close

      public void close()
      Specified by:
      close in interface AutoCloseable

    • updateCacheValidityPeriods

      public void updateCacheValidityPeriods(long imu, long driveMotor, long absoluteEncoder)
      Update the cache validity period for the robot.
      Parameters:
      imu - IMU reading cache validity period in milliseconds.
      driveMotor - Drive motor reading cache in milliseconds.
      absoluteEncoder - Absolute encoder reading cache in milliseconds.

    • setOdometryPeriod

      public void setOdometryPeriod(double period)
      Set the odometry update period in seconds.
      Parameters:
      period - period in seconds.

    • stopOdometryThread

      public void stopOdometryThread()
      Stop the odometry thread in favor of manually updating odometry.

    • setAngleMotorConversionFactor

      public void setAngleMotorConversionFactor(double conversionFactor)
      Set the conversion factor for the angle/azimuth motor controller.
      Parameters:
      conversionFactor - Angle motor conversion factor for PID, should be generated from SwerveMath.calculateDegreesPerSteeringRotation(double, double) or calculated.

    • setDriveMotorConversionFactor

      public void setDriveMotorConversionFactor(double conversionFactor)
      Set the conversion factor for the drive motor controller.
      Parameters:
      conversionFactor - Drive motor conversion factor for PID, should be generated from SwerveMath.calculateMetersPerRotation(double, double, double) or calculated.

    • getOdometryHeading

      public Rotation2d getOdometryHeading()
      Fetch the latest odometry heading, should be trusted over getYaw().
      Returns:
      Rotation2d of the robot heading.

    • setHeadingCorrection

      public void setHeadingCorrection(boolean state)
      Set the heading correction capabilities of YAGSL.
      Parameters:
      state - headingCorrection state.

    • setHeadingCorrection

      public void setHeadingCorrection(boolean state, double deadband)
      Set the heading correction capabilities of YAGSL.
      Parameters:
      state - headingCorrection state.
      deadband - HEADING_CORRECTION_DEADBAND deadband.

    • driveFieldOrientedAndRobotOriented

      public void driveFieldOrientedAndRobotOriented(ChassisSpeeds fieldOrientedVelocity, ChassisSpeeds robotOrientedVelocity)
      Tertiary method of controlling the drive base given velocity in both field oriented and robot oriented at the same time. The inputs are added together so this is not intended to be used to give the driver both methods of control.
      Parameters:
      fieldOrientedVelocity - The field oriented velocties to use
      robotOrientedVelocity - The robot oriented velocties to use

    • driveFieldOriented

      public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds)
      Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
      Parameters:
      fieldRelativeSpeeds - Velocity of the robot desired.

    • driveFieldOriented

      public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds, Translation2d centerOfRotationMeters)
      Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
      Parameters:
      fieldRelativeSpeeds - Velocity of the robot desired.
      centerOfRotationMeters - The center of rotation in meters, 0 is the center of the robot.

    • drive

      public void drive(ChassisSpeeds velocity)
      Secondary method for controlling the drivebase. Given a simple ChassisSpeeds set the swerve module states, to achieve the goal.
      Parameters:
      velocity - The desired robot-oriented ChassisSpeeds for the robot to achieve.

    • drive

      public void drive(ChassisSpeeds velocity, Translation2d centerOfRotationMeters)
      Secondary method for controlling the drivebase. Given a simple ChassisSpeeds set the swerve module states, to achieve the goal.
      Parameters:
      velocity - The desired robot-oriented ChassisSpeeds for the robot to achieve.
      centerOfRotationMeters - The center of rotation in meters, 0 is the center of the robot.

    • drive

      public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, Translation2d centerOfRotationMeters)
      The primary method for controlling the drivebase. Takes a Translation2d and a rotation rate, and calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.
      Parameters:
      translation - Translation2d that is the commanded linear velocity of the robot, in meters per second. In robot-relative mode, positive x is torwards the bow (front) and positive y is torwards port (left). In field-relative mode, positive x is away from the alliance wall (field North) and positive y is torwards the left wall when looking through the driver station glass (field West).
      rotation - Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot relativity.
      fieldRelative - Drive mode. True for field-relative, false for robot-relative.
      isOpenLoop - Whether to use closed-loop velocity control. Set to true to disable closed-loop.
      centerOfRotationMeters - The center of rotation in meters, 0 is the center of the robot.

    • drive

      public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)
      The primary method for controlling the drivebase. Takes a Translation2d and a rotation rate, and calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.
      Parameters:
      translation - Translation2d that is the commanded linear velocity of the robot, in meters per second. In robot-relative mode, positive x is torwards the bow (front) and positive y is torwards port (left). In field-relative mode, positive x is away from the alliance wall (field North) and positive y is torwards the left wall when looking through the driver station glass (field West).
      rotation - Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot relativity.
      fieldRelative - Drive mode. True for field-relative, false for robot-relative.
      isOpenLoop - Whether to use closed-loop velocity control. Set to true to disable closed-loop.

    • drive

      public void drive(ChassisSpeeds robotRelativeVelocity, boolean isOpenLoop, Translation2d centerOfRotationMeters)
      The primary method for controlling the drivebase. Takes a ChassisSpeeds, and calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Applies heading correction if enabled and necessary.
      Parameters:
      robotRelativeVelocity - The chassis speeds to set the robot to achieve.
      isOpenLoop - Whether to use closed-loop velocity control. Set to true to disable closed-loop.
      centerOfRotationMeters - The center of rotation in meters, 0 is the center of the robot.

    • setMaximumAttainableSpeeds

      public void setMaximumAttainableSpeeds(double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)
      Set the maximum attainable speeds for desaturation.
      Parameters:
      attainableMaxTranslationalSpeedMetersPerSecond - The absolute max speed that your robot can reach while translating in meters per second.
      attainableMaxRotationalVelocityRadiansPerSecond - The absolute max speed the robot can reach while rotating in radians per second.

    • setMaximumAllowableSpeeds

      public void setMaximumAllowableSpeeds(double maxTranslationalSpeedMetersPerSecond, double maxRotationalVelocityRadiansPerSecond)
      Set the maximum allowable speeds for desaturation.
      Parameters:
      maxTranslationalSpeedMetersPerSecond - The allowable max speed that your robot should reach while translating in meters per second.
      maxRotationalVelocityRadiansPerSecond - The allowable max speed the robot should reach while rotating in radians per second.

    • getMaximumChassisVelocity

      public double getMaximumChassisVelocity()
      Get the maximum velocity from attainableMaxTranslationalSpeedMetersPerSecond or maxChassisSpeedMPS whichever is the lower limit on the robot's speed.
      Returns:
      Minimum speed in meters/second of physically attainable and user allowable limits.

    • getMaximumModuleDriveVelocity

      public double getMaximumModuleDriveVelocity()
      Get the maximum drive velocity of a module as a LinearVelocity.
      Returns:
      LinearVelocity representing the maximum drive speed of a module.

    • getMaximumModuleAngleVelocity

      public AngularVelocity getMaximumModuleAngleVelocity()
      Get the maximum angular velocity of an azimuth/angle motor in the swerve module.
      Returns:
      AngularVelocity of the maximum azimuth/angle motor.

    • getMaximumChassisAngularVelocity

      public double getMaximumChassisAngularVelocity()
      Get the maximum angular velocity, either attainableMaxRotationalVelocityRadiansPerSecond or SwerveControllerConfiguration.maxAngularVelocity, whichever is the lower limit on the robot's speed.
      Returns:
      Minimum angular velocity in radians per second of physically attainable and user allowable limits.

    • setModuleStates

      public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
      Set the module states (azimuth and velocity) directly. Used primarily for auto paths. Does not allow for usage of SwerveDriveKinematics.desaturateWheelSpeeds(SwerveModuleState[] moduleStates, ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)
      Parameters:
      desiredStates - A list of SwerveModuleStates to send to the modules.
      isOpenLoop - Whether to use closed-loop velocity control. Set to true to disable closed-loop.

    • drive

      public void drive(ChassisSpeeds robotRelativeVelocity, SwerveModuleState[] states, Force[] feedforwardForces)
      Drive the robot using the SwerveModuleState, it is recommended to have setCosineCompensator(boolean) set to false for this.
      Parameters:
      robotRelativeVelocity - Robot relative ChassisSpeeds
      states - Corresponding SwerveModuleState to use (not checked against the ).
      feedforwardForces - Feedforward forces generated by set-point generator

    • setChassisSpeeds

      public void setChassisSpeeds(ChassisSpeeds robotRelativeSpeeds)
      Set chassis speeds with closed-loop velocity control.
      Parameters:
      robotRelativeSpeeds - Chassis speeds to set.

    • getPose

      public Pose2d getPose()
      Gets the measured pose (position and rotation) of the robot, as reported by odometry.
      Returns:
      The robot's pose

    • getMapleSimDrive

      public Optional<SwerveDriveSimulation> getMapleSimDrive()
      Gets the maple-sim drivetrain simulation instance This is used to add intake simulation / launch game pieces from the robot
      Returns:
      an optional maple-sim SwerveDriveSimulation object, or Optional.empty() when calling from a real robot

    • getSimulationDriveTrainPose

      public Optional<Pose2d> getSimulationDriveTrainPose()
      Gets the actual pose of the drivetrain during simulation
      Returns:
      an Optional Pose2d, representing the drivetrain pose during simulation, or an empty optional when running on real robot

    • getFieldVelocity

      public ChassisSpeeds getFieldVelocity()
      Gets the measured field-relative robot velocity (x, y and omega)
      Returns:
      A ChassisSpeeds object of the current field-relative velocity

    • getRobotVelocity

      public ChassisSpeeds getRobotVelocity()
      Gets the current robot-relative velocity (x, y and omega) of the robot
      Returns:
      A ChassisSpeeds object of the current robot-relative velocity

    • resetOdometry

      public void resetOdometry(Pose2d pose)
      Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this method. However, if either gyro angle or module position is reset, this must be called in order for odometry to keep working.
      Parameters:
      pose - The pose to set the odometry to

    • postTrajectory

      public void postTrajectory(Trajectory trajectory)
      Post the trajectory to the field
      Parameters:
      trajectory - the trajectory to post.

    • getStates

      public SwerveModuleState[] getStates()
      Gets the current module states (azimuth and velocity)
      Returns:
      A list of SwerveModuleStates containing the current module states

    • getModulePositions

      public SwerveModulePosition[] getModulePositions()
      Gets the current module positions (azimuth and wheel position (meters)).
      Returns:
      A list of SwerveModulePositions containg the current module positions

    • getGyro

      public SwerveIMU getGyro()
      Getter for the SwerveIMU.
      Returns:
      generated SwerveIMU

    • setGyro

      public void setGyro(Rotation3d gyro)
      Set the expected gyroscope angle using a Rotation3d object. To reset gyro, set to a new Rotation3d subtracted from the current gyroscopic readings SwerveIMU.getRotation3d().
      Parameters:
      gyro - expected gyroscope angle as Rotation3d.

    • zeroGyro

      public void zeroGyro()
      Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.

    • getYaw

      public Rotation2d getYaw()
      Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped.
      Returns:
      The yaw as a Rotation2d angle

    • getPitch

      public Rotation2d getPitch()
      Gets the current pitch angle of the robot, as reported by the imu.
      Returns:
      The heading as a Rotation2d angle

    • getRoll

      public Rotation2d getRoll()
      Gets the current roll angle of the robot, as reported by the imu.
      Returns:
      The heading as a Rotation2d angle

    • getGyroRotation3d

      public Rotation3d getGyroRotation3d()
      Gets the current gyro Rotation3d of the robot, as reported by the imu.
      Returns:
      The heading as a Rotation3d angle

    • getAccel

      public Optional<Translation3d> getAccel()
      Gets current acceleration of the robot in m/s/s. If gyro unsupported returns empty.
      Returns:
      acceleration of the robot as a Translation3d

    • setMotorIdleMode

      public void setMotorIdleMode(boolean brake)
      Sets the drive motors to brake/coast mode.
      Parameters:
      brake - True to set motors to brake mode, false for coast.

    • setModuleEncoderAutoSynchronize

      public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband)
      Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
      Parameters:
      enabled - Enable state
      deadband - Deadband in degrees, default is 3 degrees.

    • lockPose

      public void lockPose()
      Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep the current pose.

    • getSwerveModulePoses

      public Pose2d[] getSwerveModulePoses(Pose2d robotPose)
      Get the swerve module poses and on the field relative to the robot.
      Parameters:
      robotPose - Robot pose.
      Returns:
      Swerve module poses.

    • replaceSwerveModuleFeedforward

      public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward driveFeedforward)
      Setup the swerve module feedforward.
      Parameters:
      driveFeedforward - Feedforward for the drive motor on swerve modules.

    • updateOdometry

      public void updateOdometry()
      Update odometry should be run every loop. Synchronizes module absolute encoders with relative encoders periodically. In simulation mode will also post the pose of each module. Updates SmartDashboard with module encoder readings and states.

    • invalidateCache

      public void invalidateCache()
      Invalidate all Cache object used by the SwerveDrive

    • synchronizeModuleEncoders

      public void synchronizeModuleEncoders()
      Synchronize angle motor integrated encoders with data from absolute encoders.

    • setGyroOffset

      public void setGyroOffset(Rotation3d offset)
      Set the gyro scope offset to a desired known rotation. Unlike setGyro(Rotation3d) it DOES NOT take the current rotation into account.
      Parameters:
      offset - Rotation3d known offset of the robot for gyroscope to use.

    • addVisionMeasurement

      public void addVisionMeasurement(Pose2d robotPose, double timestamp, Matrix<N3,N1> visionMeasurementStdDevs)
      Add a vision measurement to the SwerveDrivePoseEstimator and update the SwerveIMU gyro reading with the given timestamp of the vision measurement.
      Parameters:
      robotPose - Robot Pose2d as measured by vision.
      timestamp - Timestamp the measurement was taken as time since startup, should be taken from Timer.getFPGATimestamp() or similar sources.
      visionMeasurementStdDevs - Vision measurement standard deviation that will be sent to the SwerveDrivePoseEstimator.The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more points and fit a line to it with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get * vision accurate to inches instead of feet.

    • setVisionMeasurementStdDevs

      public void setVisionMeasurementStdDevs(Matrix<N3,N1> visionMeasurementStdDevs)
      Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements after the autonomous period, or to change trust as distance to a vision target increases.
      Parameters:
      visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta], with units in meters and radians.

    • addVisionMeasurement

      public void addVisionMeasurement(Pose2d robotPose, double timestamp)
      Add a vision measurement to the SwerveDrivePoseEstimator and update the SwerveIMU gyro reading with the given timestamp of the vision measurement.
      Parameters:
      robotPose - Robot Pose2d as measured by vision.
      timestamp - Timestamp the measurement was taken as time since startup, should be taken from Timer.getFPGATimestamp() or similar sources.

    • getSwerveController

      public SwerveController getSwerveController()
      Helper function to get the swerveController for the SwerveDrive which can be used to generate ChassisSpeeds for the robot to orient it correctly given axis or angles, and apply SlewRateLimiter to given inputs. Important functions to look at are SwerveController.getTargetSpeeds(double, double, double, double, double), SwerveController.addSlewRateLimiters(SlewRateLimiter, SlewRateLimiter, SlewRateLimiter), SwerveController.getRawTargetSpeeds(double, double, double).
      Returns:
      SwerveController for the SwerveDrive.

    • getModules

      public SwerveModule[] getModules()
      Get the SwerveModules associated with the SwerveDrive.
      Returns:
      SwerveModule array specified by configurations.

    • getModuleMap

      public Map<String,SwerveModule> getModuleMap()
      Get the SwerveModule's as a HashMap where the key is the swerve module configuration name.
      Returns:
      HashMap(Module Name, SwerveModule)

    • resetDriveEncoders

      public void resetDriveEncoders()
      Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in autonomous.

    • useExternalFeedbackSensor

      public void useExternalFeedbackSensor()
      Set the motor controller closed loop feedback device to the defined external absolute encoder, with the given offset from the supplied configuration, overwriting any native offset.

    • useInternalFeedbackSensor

      public void useInternalFeedbackSensor()
      Set the motor controller closed loop feedback device to the internal encoder instead of the absolute encoder.

    • pushOffsetsToEncoders

      @Deprecated public void pushOffsetsToEncoders()
      Deprecated.
      Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the internal offsets to prevent double offsetting.

    • restoreInternalOffset

      @Deprecated public void restoreInternalOffset()
      Deprecated.
      Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0

    • setModuleStateOptimization

      public void setModuleStateOptimization(boolean enabled)
      Set module optimization to be utilized or not. Sometimes it is desirable to be enabled for debugging purposes only.
      Parameters:
      enabled - Optimization enabled state.

    • setAutoCenteringModules

      public void setAutoCenteringModules(boolean enabled)
      Enable auto-centering module wheels. This has a side effect of causing some jitter to the robot when a PID is not tuned perfectly. This function is a wrapper for SwerveModule.setAntiJitter(boolean) to perform auto-centering.
      Parameters:
      enabled - Enable auto-centering (disable antiJitter)

    • setCosineCompensator

      public void setCosineCompensator(boolean enabled)
      Enable or disable the SwerveModuleConfiguration.useCosineCompensator for all SwerveModule's in the swerve drive. The cosine compensator will slow down or speed up modules that are close to their desired state in theory.
      Parameters:
      enabled - Usage of the cosine compensator.

    • setChassisDiscretization

      public void setChassisDiscretization(boolean enable, double dtSeconds)
      Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
      Parameters:
      enable - Enable chassis velocity correction, which will use ChassisSpeeds.discretize(ChassisSpeeds, double)} with the following.
      dtSeconds - The duration of the timestep the speeds should be applied for.

    • setChassisDiscretization

      public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
      Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto
      Parameters:
      useInTeleop - Enable chassis velocity correction, which will use ChassisSpeeds.discretize(ChassisSpeeds, double) with the following in teleop.
      useInAuto - Enable chassis velocity correction, which will use ChassisSpeeds.discretize(ChassisSpeeds, double) with the following in auto.
      dtSeconds - The duration of the timestep the speeds should be applied for.

    • setAngularVelocityCompensation

      public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff)
      Enables angular velocity skew correction in teleop and/or autonomous and sets the angular velocity coefficient for both modes
      Parameters:
      useInTeleop - Enables angular velocity correction in teleop.
      useInAuto - Enables angular velocity correction in autonomous.
      angularVelocityCoeff - The angular velocity coefficient. Expected values between -0.15 to 0.15. Start with a value of 0.1, test in teleop. When enabling for the first time if the skew is significantly worse try inverting the value. Tune by moving in a straight line while rotating. Testing is best done with angular velocity controls on the right stick. Change the value until you are visually happy with the skew. Ensure your tune works with different translational and rotational magnitudes. If this reduces skew in teleop, it may improve auto.

    • angularVelocitySkewCorrection

      public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds robotRelativeVelocity)
      Correct for skew that worsens as angular velocity increases
      Parameters:
      robotRelativeVelocity - The chassis speeds to set the robot to achieve.
      Returns:
      ChassisSpeeds of the robot after angular velocity skew correction.

    • toServeModuleStates

      public SwerveModuleState[] toServeModuleStates(ChassisSpeeds robotRelativeVelocity, boolean optimize)
      Convert a ChassisSpeeds to SwerveModuleState for use elsewhere.
      Parameters:
      robotRelativeVelocity - ChassisSpeeds velocity to use.
      optimize - Perform chassis velocity correction or angular velocity correction.
      Returns:
      SwerveModuleState for use elsewhere.