Class Drive

java.lang.Object
org.teamvan.robot.mechanisms.drive.Drive
All Implemented Interfaces:
Mechanism
public class Drive extends Object implements Mechanism
Swerve drivetrain mechanism, ported from the AdvantageKit TalonFX swerve template to the WPILib 2027 Mechanism API.

Commands claim sole ownership of the drivetrain through the command scheduler, so no command can drive the robot while another one already does. Per-loop work is split into updateInputs() (read hardware, sample odometry) and updateOutputs() (publish telemetry), which the robot calls around Scheduler.getDefault().run().

  • Field Details

    • ODOMETRY_FREQUENCY

      public static final double ODOMETRY_FREQUENCY
      The frequency at which the Phoenix odometry thread samples module and gyro signals.

    • DRIVE_BASE_RADIUS

      public static final double DRIVE_BASE_RADIUS

    • odometryLock

      public static final Lock odometryLock

  • Constructor Details

  • Method Details

    • updateInputs

      public void updateInputs()
      Reads the drivetrain hardware and integrates the high-frequency odometry samples. Call this before running the command scheduler so commands act on fresh data.

    • updateOutputs

      public void updateOutputs()
      Publishes the drivetrain's measured state to telemetry. Call this after running the command scheduler so the reported state reflects the outputs commands just wrote.

    • runVelocity

      public void runVelocity(ChassisVelocities velocities)
      Runs the drive at the desired velocity.
      Parameters:
      velocities - Velocities in meters/sec

    • runCharacterization

      public void runCharacterization(double output)
      Runs the drive in a straight line with the specified drive output.

    • joystickDrive

      public Command joystickDrive(Gamepad gamepad)
      Returns a field-relative drive command that reads the gamepad's left stick for linear velocity and its right stick for rotation. Intended to be set as the drivetrain's default command.
      Parameters:
      gamepad - the driver's gamepad
      Returns:
      the joystick drive command

    • joystickDrive

      public Command joystickDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier)
      Returns a field-relative drive command from the given stick suppliers.
      Parameters:
      xSupplier - forward input in [-1, 1], positive forward
      ySupplier - leftward input in [-1, 1], positive left
      omegaSupplier - counter-clockwise input in [-1, 1], positive counter-clockwise
      Returns:
      the joystick drive command

    • stop

      public void stop()
      Stops the drive.

    • stopWithX

      public void stopWithX()
      Stops the drive and turns the modules to an X arrangement to resist movement. The modules will return to their normal orientations the next time a nonzero velocity is requested.

    • getWheelRadiusCharacterizationPositions

      public double[] getWheelRadiusCharacterizationPositions()
      Returns the position of each module in radians.

    • getFFCharacterizationVelocity

      public double getFFCharacterizationVelocity()
      Returns the average velocity of the modules in rotations/sec (Phoenix native units).

    • getPose

      public Pose2d getPose()
      Returns the current odometry pose.

    • getRotation

      public Rotation2d getRotation()
      Returns the current odometry rotation.

    • setPose

      public void setPose(Pose2d pose)
      Resets the current odometry pose.

    • addVisionMeasurement

      public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,N1> visionMeasurementStdDevs)
      Adds a new timestamped vision measurement.

    • getMaxLinearSpeedMetersPerSec

      public double getMaxLinearSpeedMetersPerSec()
      Returns the maximum linear speed in meters per sec.

    • getMaxAngularSpeedRadPerSec

      public double getMaxAngularSpeedRadPerSec()
      Returns the maximum angular speed in radians per sec.

    • getModuleTranslations

      public static Translation2d[] getModuleTranslations()
      Returns an array of module translations.