Package frc.robot.subsystems

Class Drivetrain

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

  • Constructor Details

    • Drivetrain

      public Drivetrain()
      Initialize SwerveDrive with the directory provided.

  • Method Details

    • setupPathPlanner

      public void setupPathPlanner()
      Setup AutoBuilder for PathPlanner.

    • getAutonomousCommand

      public 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 Command driveToPose(Pose2d pose)

    • driveCommand

      public 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 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 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 Command turnToAngleCommand(Rotation2d angle)

    • drive

      public void drive(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(ChassisSpeeds velocity)
      Drive the robot given a chassis field oriented velocity.
      Parameters:
      velocity - Velocity according to the field.

    • drive

      public void drive(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 SwerveDriveKinematics getKinematics()
      Get the swerve drive kinematics object.
      Returns:
      SwerveDriveKinematics of the swerve drive.

    • resetOdometry

      public void resetOdometry(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 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(ChassisSpeeds chassisSpeeds)
      Set chassis speeds with closed-loop velocity control.
      Parameters:
      chassisSpeeds - Chassis Speeds to set.

    • postTrajectory

      public void postTrajectory(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 Command resetPose(Pose2d resetPose)

    • doVisionUpdates

      public void doVisionUpdates(boolean doVisionUpdates)

    • getTargetSpeeds

      public 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 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 ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double thetaInput)

    • getTargetSpeeds

      public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, 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 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 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 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 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()