Package frc.robot.subsystems

Class Drivetrain

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

  • Constructor Summary

    Constructors
    Constructor
    Description
    Drivetrain()
    Initialize SwerveDrive with the directory provided.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    doVisionUpdates(boolean doVisionUpdates)
     
    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.
    edu.wpi.first.wpilibj2.command.Command
    driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX)
    Command to drive the robot using translative values and heading as angular velocity.
    edu.wpi.first.wpilibj2.command.Command
    driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, DoubleSupplier headingY)
    Command to drive the robot using translative values and heading as a setpoint.
    void
    driveFieldOriented(edu.wpi.first.math.kinematics.ChassisSpeeds velocity)
    Drive the robot given a chassis field oriented velocity.
    edu.wpi.first.wpilibj2.command.Command
    driveToPose(edu.wpi.first.math.geometry.Pose2d pose)
     
    edu.wpi.first.wpilibj2.command.Command
    getAutonomousCommand(String pathName, boolean setOdomToStart)
    Get the path follower with events.
    double
    getDistanceToSpeaker()
     
    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.
    double
    getMaximumVelocity()
     
    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
    edu.wpi.first.math.geometry.Rotation2d
    getRoll()
    Gets the current roll angle of the robot, as reported by the imu.
    swervelib.SwerveController
    getSwerveController()
    Get the SwerveController in the swerve drive.
    swervelib.parser.SwerveDriveConfiguration
    getSwerveDriveConfiguration()
    Get the SwerveDriveConfiguration object.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getTargetSpeeds(double xInput, double yInput, double thetaInput)
     
    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
    resetLastAngeScalar()
     
    void
    resetOdometry(edu.wpi.first.math.geometry.Pose2d initialHolonomicPose)
    Resets odometry to the given pose.
    edu.wpi.first.wpilibj2.command.Command
    resetPose(edu.wpi.first.math.geometry.Pose2d resetPose)
     
    void
    setChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)
    Set chassis speeds with closed-loop velocity control.
    void
    setHeadingCorrection(boolean doHeadingCorrection)
     
    void
    setMotorBrake(boolean brake)
    Sets the drive motors to brake/coast mode.
    void
    setupPathPlanner()
    Setup AutoBuilder for PathPlanner.
    edu.wpi.first.wpilibj2.command.Command
    simDriveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation)
    Command to drive the robot using translative values and heading as a setpoint.
    void
    simulationPeriodic()
     
    edu.wpi.first.wpilibj2.command.Command
    turnToAngleCommand(edu.wpi.first.math.geometry.Rotation2d angle)
     
    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

  • Constructor Details

    • Drivetrain

      public Drivetrain()
      Initialize SwerveDrive with the directory provided.

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

    • driveToPose

      public edu.wpi.first.wpilibj2.command.Command driveToPose(edu.wpi.first.math.geometry.Pose2d pose)

    • driveCommand

      public edu.wpi.first.wpilibj2.command.Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, DoubleSupplier headingY)
      Command to drive the robot using translative values and heading as a setpoint.
      Parameters:
      translationX - Translation in the X direction. Cubed for smoother controls.
      translationY - Translation in the Y direction. Cubed for smoother controls.
      headingX - Heading X to calculate angle of the joystick.
      headingY - Heading Y to calculate angle of the joystick.
      Returns:
      Drive command.

    • simDriveCommand

      public edu.wpi.first.wpilibj2.command.Command simDriveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation)
      Command to drive the robot using translative values and heading as a setpoint.
      Parameters:
      translationX - Translation in the X direction.
      translationY - Translation in the Y direction.
      rotation - Rotation as a value between [-1, 1] converted to radians.
      Returns:
      Drive command.

    • driveCommand

      public edu.wpi.first.wpilibj2.command.Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX)
      Command to drive the robot using translative values and heading as angular velocity.
      Parameters:
      translationX - Translation in the X direction. Cubed for smoother controls.
      translationY - Translation in the Y direction. Cubed for smoother controls.
      angularRotationX - Angular velocity of the robot to set. Cubed for smoother controls.
      Returns:
      Drive command.

    • turnToAngleCommand

      public edu.wpi.first.wpilibj2.command.Command turnToAngleCommand(edu.wpi.first.math.geometry.Rotation2d angle)

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

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

    • zeroGyro

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

    • resetPose

      public edu.wpi.first.wpilibj2.command.Command resetPose(edu.wpi.first.math.geometry.Pose2d resetPose)

    • doVisionUpdates

      public void doVisionUpdates(boolean doVisionUpdates)

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

    • 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

    • setHeadingCorrection

      public void setHeadingCorrection(boolean doHeadingCorrection)

    • getTargetSpeeds

      public edu.wpi.first.math.kinematics.ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double thetaInput)

    • 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. Control the robot at an offset of 90deg.
      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 swervelib.SwerveController getSwerveController()
      Get the SwerveController in the swerve drive.
      Returns:
      SwerveController from the SwerveDrive.

    • getMaximumVelocity

      public double getMaximumVelocity()

    • getSwerveDriveConfiguration

      public swervelib.parser.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

    • resetLastAngeScalar

      public void resetLastAngeScalar()

    • getRoll

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

    • getDistanceToSpeaker

      public double getDistanceToSpeaker()