Package frc.robot.subsystems.swervedrive2.swervelib

## Class SwerveDrive

- java.lang.Object
- - frc.robot.subsystems.swervedrive2.swervelib.SwerveDrive

-     public class SwerveDrive
      extends Object

  Swerve Drive class representing and controlling the swerve drive.

- - ### Field Summary

    Fields
    Modifier and Type | Field | Description
    --- | --- | ---
    `private double` | `angle` | The current angle of the robot and last time odometry during simulations.
    `edu.wpi.first.wpilibj.smartdashboard.Field2d` | `field` | Field object.
    `private SwerveIMU` | `imu` | Swerve IMU device for sensing the heading of the robot.
    `SwerveKinematics2` | `kinematics` | Swerve Kinematics object utilizing second order kinematics.
    `private double` | `lastTime` | The current angle of the robot and last time odometry during simulations.
    `SwerveController` | `swerveController` | Swerve controller for controlling heading of the robot.
    `SwerveDriveConfiguration` | `swerveDriveConfiguration` | Swerve drive configuration.
    `edu.wpi.first.math.estimator.SwerveDrivePoseEstimator` | `swerveDrivePoseEstimator` | Swerve odometry.
    `private SwerveModule[]` | `swerveModules` | Swerve modules.
    `private edu.wpi.first.wpilibj.Timer` | `timer` | Time during simulations.

  - ### Constructor Summary

    Constructors
    Constructor | Description
    --- | ---
    `SwerveDrive​(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig)` | Creates a new swerve drivebase subsystem.

  - ### Method Summary

    All Methods Instance Methods Concrete Methods
    Modifier and Type | Method | Description
    --- | --- | ---
    `void` | `drive​(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)` | The primary method for controlling the drivebase.
    `edu.wpi.first.math.kinematics.ChassisSpeeds` | `getFieldVelocity()` | Gets the current field-relative velocity (x, y and omega) of the robot
    `edu.wpi.first.math.kinematics.SwerveModulePosition[]` | `getModulePositions()` | Gets the current module positions (azimuth and wheel position (meters))
    `edu.wpi.first.math.geometry.Rotation2d` | `getPitch()` | Gets the current yaw angle of the robot, as reported by the imu.
    `edu.wpi.first.math.geometry.Pose2d` | `getPose()` | Gets the current pose (position and rotation) of the robot, as reported by odometry.
    `edu.wpi.first.math.kinematics.ChassisSpeeds` | `getRobotVelocity()` | Gets the current robot-relative velocity (x, y and omega) of the robot
    `SwerveModuleState2[]` | `getStates()` | Gets the current module states (azimuth and velocity)
    `edu.wpi.first.math.geometry.Pose2d[]` | `getSwerveModulePoses​(edu.wpi.first.math.geometry.Pose2d robotPose)` | Get the swerve module poses and on the field relative to the robot.
    `edu.wpi.first.math.geometry.Rotation2d` | `getYaw()` | Gets the current yaw angle of the robot, as reported by the imu.
    `void` | `replaceSwerveModuleFeedforward​(edu.wpi.first.math.controller.SimpleMotorFeedforward feedforward)` | Setup the swerve module feedforward.
    `void` | `resetOdometry​(edu.wpi.first.math.geometry.Pose2d pose)` | Resets odometry to the given pose.
    `void` | `setChassisSpeeds​(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)` | Set field-relative chassis speeds with closed-loop velocity control.
    `void` | `setDriveBrake()` | Point all modules toward the robot center, thus making the robot very difficult to move.
    `void` | `setModuleStates​(SwerveModuleState2[] desiredStates, boolean isOpenLoop)` | Set the module states (azimuth and velocity) directly.
    `void` | `setMotorBrake​(boolean brake)` | Sets the drive motors to brake/coast mode.
    `void` | `updateOdometry()` | Update odometry should be run every loop.
    `void` | `zeroGyro()` | Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.

    - ### Methods inherited from class java.lang.Object

      `clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait`

- - ### Field Detail

    - #### kinematics

          public final SwerveKinematics2 kinematics

      Swerve Kinematics object utilizing second order kinematics.

    - #### swerveDriveConfiguration

          public final SwerveDriveConfiguration swerveDriveConfiguration

      Swerve drive configuration.

    - #### swerveDrivePoseEstimator

          public final edu.wpi.first.math.estimator.SwerveDrivePoseEstimator swerveDrivePoseEstimator

      Swerve odometry.

    - #### swerveModules

          private final SwerveModule[] swerveModules

      Swerve modules.

    - #### field

          public edu.wpi.first.wpilibj.smartdashboard.Field2d field

      Field object.

    - #### swerveController

          public SwerveController swerveController

      Swerve controller for controlling heading of the robot.

    - #### imu

          private SwerveIMU imu

      Swerve IMU device for sensing the heading of the robot.

    - #### angle

          private double angle

      The current angle of the robot and last time odometry during simulations.

    - #### lastTime

          private double lastTime

      The current angle of the robot and last time odometry during simulations.

    - #### timer

          private edu.wpi.first.wpilibj.Timer timer

      Time during simulations.

  - ### Constructor Detail

    - #### SwerveDrive

          public SwerveDrive​(SwerveDriveConfiguration config,
                             SwerveControllerConfiguration controllerConfig)

      Creates a new swerve drivebase subsystem. Robot is controlled via the drive() method, or via the setModuleStates() method. The drive() method incorporates kinematics— it takes a translation and rotation, as well as parameters for field-centric and closed-loop velocity control. setModuleStates() takes a list of SwerveModuleStates and directly passes them to the modules. This subsystem also handles odometry.

  - ### Method Detail

    - #### drive

          public void drive​(edu.wpi.first.math.geometry.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.

    - #### setModuleStates

          public void setModuleStates​(SwerveModuleState2[] desiredStates,
                                      boolean isOpenLoop)

      Set the module states (azimuth and velocity) directly. Used primarily for auto pathing.
      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.

    - #### setChassisSpeeds

          public void setChassisSpeeds​(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)

      Set field-relative chassis speeds with closed-loop velocity control.
      Parameters:
        `chassisSpeeds` - Field-relative.

    - #### getPose

          public edu.wpi.first.math.geometry.Pose2d getPose()

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

    - #### getFieldVelocity

          public edu.wpi.first.math.kinematics.ChassisSpeeds getFieldVelocity()

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

    - #### getRobotVelocity

          public edu.wpi.first.math.kinematics.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​(edu.wpi.first.math.geometry.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

    - #### getStates

          public SwerveModuleState2[] getStates()

      Gets the current module states (azimuth and velocity)
      Returns:
        A list of SwerveModuleStates containing the current module states

    - #### getModulePositions

          public edu.wpi.first.math.kinematics.SwerveModulePosition[] getModulePositions()

      Gets the current module positions (azimuth and wheel position (meters))
      Returns:
        A list of SwerveModulePositions containg the current module positions

    - #### zeroGyro

          public void zeroGyro()

      Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.

    - #### getYaw

          public edu.wpi.first.math.geometry.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 edu.wpi.first.math.geometry.Rotation2d getPitch()

      Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped.
      Returns:
        The heading as a `Rotation2d` angle

    - #### setMotorBrake

          public void setMotorBrake​(boolean brake)

      Sets the drive motors to brake/coast mode.
      Parameters:
        `brake` - True to set motors to brake mode, false for coast.

    - #### setDriveBrake

          public void setDriveBrake()

      Point all modules toward the robot center, thus making the robot very difficult to move.

    - #### getSwerveModulePoses

          public edu.wpi.first.math.geometry.Pose2d[] getSwerveModulePoses​(edu.wpi.first.math.geometry.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​(edu.wpi.first.math.controller.SimpleMotorFeedforward feedforward)

      Setup the swerve module feedforward.
      Parameters:
        `feedforward` - Feedforward for the drive motor on swerve modules.

    - #### updateOdometry

          public void updateOdometry()

      Update odometry should be run every loop.