Package yams.mechanisms.swerve

Class SwerveDrive

java.lang.Object
yams.mechanisms.swerve.SwerveDrive
public class SwerveDrive extends Object
Swerve Drive mechanism

  • Constructor Details

  • Method Details

    • drive

      public Command drive(Supplier<ChassisSpeeds> robotRelativeChassisSpeeds)
      Create a RunCommand to drive the swerve drive with robot relative chassis speeds.
      Parameters:
      robotRelativeChassisSpeeds - Supplier<ChassisSpeeds> for the robot relative chassis speeds. Could also use SwerveInputStream
      Returns:
      RunCommand to drive the swerve drive.
      Implementation Note:
      Not compatible with AdvantageKit

    • getGyroAngle

      public Angle getGyroAngle()
      Get the Gyro Angle.
      Returns:
      Gyro angle, or maple sim odometry gyro angle.

    • lockPose

      public void lockPose()
      Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep the current pose.
      Implementation Note:
      Not compatible with AdvantageKit

    • setSwerveModuleStates

      public void setSwerveModuleStates(SwerveModuleState[] states)
      Set the SwerveModuleStates of the swerve drive directly.
      Parameters:
      states - SwerveModuleStates to use, must be the same count as the swerve drive is configured order is Clockwise from FL.
      Implementation Note:
      Not compatible with AdvantageKit if MapleSim is defined.

    • getStateFromRobotRelativeChassisSpeeds

      public SwerveModuleState[] getStateFromRobotRelativeChassisSpeeds(ChassisSpeeds robotRelativeChassisSpeeds)
      Get the SwerveModuleStates of the swerve drive given a robot relative chassis speed..
      Parameters:
      robotRelativeChassisSpeeds - Robot relative ChassisSpeeds.
      Returns:
      SwerveModuleStates of the swerve drive.

    • setRobotRelativeChassisSpeeds

      public void setRobotRelativeChassisSpeeds(ChassisSpeeds robotRelativeChassisSpeeds)
      Set robot relative chassis speeds.
      Parameters:
      robotRelativeChassisSpeeds - Robot relative chassis speeds.

    • setFieldRelativeChassisSpeeds

      public void setFieldRelativeChassisSpeeds(ChassisSpeeds fieldRelativeChassisSpeeds)
      Set field relative chassis speeds.
      Parameters:
      fieldRelativeChassisSpeeds - Field relative chassis speeds.

    • getPose

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

    • getKinematics

      public SwerveDriveKinematics getKinematics()
      Create the SwerveDriveKinematics so you can recreate a new SwerveDrivePoseEstimator.
      Returns:
      SwerveDriveKinematics

    • zeroGyro

      public void zeroGyro()
      Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0 (red alliance station).
      Implementation Note:
      Not compatible with AdvantageKit

    • getName

      public String getName()
      Get the name of the drive.
      Returns:
      Name of the drive.

    • resetOdometry

      public void resetOdometry(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. Field relative, blue-origin where 0deg is facing towards RED alliance.

    • resetAzimuthPID

      public void resetAzimuthPID()
      Resets the azimuth PID controller.

    • resetTranslationPID

      public void resetTranslationPID()
      Resets the translation PID controller.

    • getDistanceFromPose

      public Distance getDistanceFromPose(Pose2d pose)
      Get the Distance from the given pose to the robot.
      Parameters:
      pose - Pose2d to get the distance from.
      Returns:
      Distance from the given pose to the robot.

    • getAngleDifferenceFromPose

      public Angle getAngleDifferenceFromPose(Pose2d pose)
      Get the angle difference between the robot's current pose and the given pose.
      Parameters:
      pose - Pose2d to get the angle difference from.
      Returns:
      Angle difference between the robot's current pose and the given pose.

    • driveToPose

      public Command driveToPose(Pose2d pose)
      Drive the robot to the given pose.
      Parameters:
      pose - Pose2d to drive the robot to. Field relative, blue-origin where 0deg is facing towards RED
      Returns:
      Command to drive the robot to the given pose.
      Implementation Note:
      Not compatible with AdvantageKit

    • addVisionMeasurement

      public void addVisionMeasurement(Pose2d robotPose, double timestamp, Matrix<N3,N1> visionMeasurementStdDevs)
      Add a vision measurement to the SwerveDrivePoseEstimator and update the gyro reading with the given timestamp of the vision measurement.
      Parameters:
      robotPose - Robot Pose2d as measured by vision.
      timestamp - Timestamp the measurement was taken as time since startup, should be taken from Timer.getFPGATimestamp() or similar sources.
      visionMeasurementStdDevs - Vision measurement standard deviation that will be sent to the SwerveDrivePoseEstimator.The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more points and fit a line to it with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get * vision accurate to inches instead of feet.

    • setVisionMeasurementStdDevs

      public void setVisionMeasurementStdDevs(Matrix<N3,N1> visionMeasurementStdDevs)
      Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements after the autonomous period, or to change trust as distance to a vision target increases.
      Parameters:
      visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta], with units in meters and radians.

    • addVisionMeasurement

      public void addVisionMeasurement(Pose2d robotPose, double timestamp)
      Add a vision measurement to the SwerveDrivePoseEstimator and update the gyro reading with the given timestamp of the vision measurement.
      Parameters:
      robotPose - Robot Pose2d as measured by vision.
      timestamp - Timestamp the measurement was taken as time since startup, should be taken from Timer.getFPGATimestamp() or similar sources.

    • updateTelemetry

      public void updateTelemetry()
      Update the telemetry and SwerveDrivePoseEstimator of the drive.

    • simIterate

      public void simIterate()
      Simulate the drive, updating the gyroscope based off of module states.

    • getRobotRelativeSpeed

      public ChassisSpeeds getRobotRelativeSpeed()
      Get the robot relative speed of the drive.
      Returns:
      Robot relative speed of the drive.

    • getFieldRelativeSpeed

      public ChassisSpeeds getFieldRelativeSpeed()
      Get the field relative speed of the drive.
      Returns:
      Field relative speed of the drive.

    • getModulePositions

      public SwerveModulePosition[] getModulePositions()
      Get the SwerveModulePosition of the modules.
      Returns:
      SwerveModulePosition of the modules.

    • getModuleStates

      public SwerveModuleState[] getModuleStates()
      Get the SwerveModuleState of the modules.
      Returns:
      SwerveModuleState of the modules.

    • getConfig

      public SwerveDriveConfig getConfig()
      Get the SwerveDriveConfig of the drive.
      Returns:
      SwerveDriveConfig of the drive.