Package frc.robot.subsystems.swervedrive

Class SwerveSubsystem

java.lang.Object
edu.wpi.first.wpilibj2.command.SubsystemBase
frc.robot.subsystems.swervedrive.SwerveSubsystem
All Implemented Interfaces:
edu.wpi.first.util.sendable.Sendable, edu.wpi.first.wpilibj2.command.Subsystem
public class SwerveSubsystem extends edu.wpi.first.wpilibj2.command.SubsystemBase

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    double
    maximumSpeed
    Maximum speed of the robot in meters per second, used to limit acceleration.

  • Constructor Summary

    Constructors
    Constructor
    Description
    SwerveSubsystem(File directory)
    Initialize SwerveDrive with the directory provided.
    SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg)
    Construct the swerve drive.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addFakeVisionReading()
    Add a fake vision reading for testing purposes.
    void
    drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative)
    The primary method for controlling the drivebase.
    void
    drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity)
    Drive according to the chassis robot oriented velocity.
    void
    driveFieldOriented(edu.wpi.first.math.kinematics.ChassisSpeeds velocity)
    Drive the robot given a chassis field oriented velocity.
    edu.wpi.first.wpilibj2.command.Command
    getAutonomousCommand(String pathName, boolean setOdomToStart)
    Get the path follower with events.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getFieldVelocity()
    Gets the current field-relative velocity (x, y and omega) of the robot
    edu.wpi.first.math.geometry.Rotation2d
    getHeading()
    Gets the current yaw angle of the robot, as reported by the imu.
    edu.wpi.first.math.kinematics.SwerveDriveKinematics
    getKinematics()
    Get the swerve drive kinematics object.
    edu.wpi.first.math.geometry.Rotation2d
    getPitch()
    Gets the current pitch 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 velocity (x, y and omega) of the robot
    SwerveController
    getSwerveController()
    Get the SwerveController in the swerve drive.
    SwerveDriveConfiguration
    getSwerveDriveConfiguration()
    Get the SwerveDriveConfiguration object.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getTargetSpeeds(double xInput, double yInput, double headingX, double headingY)
    Get the chassis speeds based on controller input of 2 joysticks.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getTargetSpeeds(double xInput, double yInput, edu.wpi.first.math.geometry.Rotation2d angle)
    Get the chassis speeds based on controller input of 1 joystick and one angle.
    void
    lock()
    Lock the swerve drive to prevent it from moving.
    void
    periodic()
     
    void
    postTrajectory(edu.wpi.first.math.trajectory.Trajectory trajectory)
    Post the trajectory to the field.
    void
    resetOdometry(edu.wpi.first.math.geometry.Pose2d initialHolonomicPose)
    Resets odometry to the given pose.
    void
    setChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)
    Set chassis speeds with closed-loop velocity control.
    void
    setMotorBrake(boolean brake)
    Sets the drive motors to brake/coast mode.
    void
    setupPathPlanner()
    Setup AutoBuilder for PathPlanner.
    void
    simulationPeriodic()
     
    void
    zeroGyro()
    Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.

    Methods inherited from class edu.wpi.first.wpilibj2.command.SubsystemBase

    addChild, getName, getSubsystem, initSendable, setName, setSubsystem

    Methods inherited from class java.lang.Object

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

    Methods inherited from interface edu.wpi.first.wpilibj2.command.Subsystem

    defer, getCurrentCommand, getDefaultCommand, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, startEnd

  • Field Details

    • maximumSpeed

      public double maximumSpeed
      Maximum speed of the robot in meters per second, used to limit acceleration.

  • Constructor Details

    • SwerveSubsystem

      public SwerveSubsystem(File directory)
      Initialize SwerveDrive with the directory provided.
      Parameters:
      directory - Directory of swerve drive config files.

    • SwerveSubsystem

      public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg)
      Construct the swerve drive.
      Parameters:
      driveCfg - SwerveDriveConfiguration for the swerve.
      controllerCfg - Swerve Controller.

  • Method Details

    • setupPathPlanner

      public void setupPathPlanner()
      Setup AutoBuilder for PathPlanner.

    • getAutonomousCommand

      public edu.wpi.first.wpilibj2.command.Command getAutonomousCommand(String pathName, boolean setOdomToStart)
      Get the path follower with events.
      Parameters:
      pathName - PathPlanner path name.
      setOdomToStart - Set the odometry position to the start of the path.
      Returns:
      AutoBuilder.followPath(PathPlannerPath) path command.

    • drive

      public void drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative)
      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.

    • driveFieldOriented

      public void driveFieldOriented(edu.wpi.first.math.kinematics.ChassisSpeeds velocity)
      Drive the robot given a chassis field oriented velocity.
      Parameters:
      velocity - Velocity according to the field.

    • drive

      public void drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity)
      Drive according to the chassis robot oriented velocity.
      Parameters:
      velocity - Robot oriented ChassisSpeeds

    • periodic

      public void periodic()

    • simulationPeriodic

      public void simulationPeriodic()

    • getKinematics

      public edu.wpi.first.math.kinematics.SwerveDriveKinematics getKinematics()
      Get the swerve drive kinematics object.
      Returns:
      SwerveDriveKinematics of the swerve drive.

    • resetOdometry

      public void resetOdometry(edu.wpi.first.math.geometry.Pose2d initialHolonomicPose)
      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:
      initialHolonomicPose - The pose to set the odometry to

    • 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

    • setChassisSpeeds

      public void setChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)
      Set chassis speeds with closed-loop velocity control.
      Parameters:
      chassisSpeeds - Chassis Speeds to set.

    • postTrajectory

      public void postTrajectory(edu.wpi.first.math.trajectory.Trajectory trajectory)
      Post the trajectory to the field.
      Parameters:
      trajectory - The trajectory to post.

    • zeroGyro

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

    • 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.

    • getHeading

      public edu.wpi.first.math.geometry.Rotation2d getHeading()
      Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped.
      Returns:
      The yaw angle

    • getTargetSpeeds

      public edu.wpi.first.math.kinematics.ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY)
      Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for the angle of the robot.
      Parameters:
      xInput - X joystick input for the robot to move in the X direction.
      yInput - Y joystick input for the robot to move in the Y direction.
      headingX - X joystick which controls the angle of the robot.
      headingY - Y joystick which controls the angle of the robot.
      Returns:
      ChassisSpeeds which can be sent to th Swerve Drive.

    • getTargetSpeeds

      public edu.wpi.first.math.kinematics.ChassisSpeeds getTargetSpeeds(double xInput, double yInput, edu.wpi.first.math.geometry.Rotation2d angle)
      Get the chassis speeds based on controller input of 1 joystick and one angle.
      Parameters:
      xInput - X joystick input for the robot to move in the X direction.
      yInput - Y joystick input for the robot to move in the Y direction.
      angle - The angle in as a Rotation2d.
      Returns:
      ChassisSpeeds which can be sent to th Swerve Drive.

    • 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 velocity (x, y and omega) of the robot
      Returns:
      A ChassisSpeeds object of the current velocity

    • getSwerveController

      public SwerveController getSwerveController()
      Get the SwerveController in the swerve drive.
      Returns:
      SwerveController from the SwerveDrive.

    • getSwerveDriveConfiguration

      public SwerveDriveConfiguration getSwerveDriveConfiguration()
      Get the SwerveDriveConfiguration object.
      Returns:
      The SwerveDriveConfiguration fpr the current drive.

    • lock

      public void lock()
      Lock the swerve drive to prevent it from moving.

    • getPitch

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

    • addFakeVisionReading

      public void addFakeVisionReading()
      Add a fake vision reading for testing purposes.