Package trclib.drivebase

Class TrcSwerveDriveBase

java.lang.Object
trclib.drivebase.TrcDriveBase
trclib.drivebase.TrcSimpleDriveBase
trclib.drivebase.TrcSwerveDriveBase
All Implemented Interfaces:
TrcExclusiveSubsystem
public class TrcSwerveDriveBase extends TrcSimpleDriveBase
This class implements a platform independent swerve drive base. A swerve drive base consists of 4 swerve modules each of which consists of a driving motor and a PID controlled steering motor. It extends the TrcSimpleDriveBase class so it inherits all the SimpleDriveBase methods and features

The implementation of swerve algorithm is based on Ether's white paper

  • Field Details

    • moduleName

      private final String moduleName

    • lfModule

      private final TrcSwerveModule lfModule

    • rfModule

      private final TrcSwerveModule rfModule

    • lbModule

      private final TrcSwerveModule lbModule

    • rbModule

      private final TrcSwerveModule rbModule

    • wheelBaseWidth

      private final double wheelBaseWidth

    • wheelBaseLength

      private final double wheelBaseLength

    • wheelBaseDiagonal

      private final double wheelBaseDiagonal

    • driveMotorToModuleMap

      private final TrcHashMap<TrcMotor,TrcSwerveModule> driveMotorToModuleMap

  • Constructor Details

    • TrcSwerveDriveBase

      public TrcSwerveDriveBase(TrcSwerveModule lfModule, TrcSwerveModule lbModule, TrcSwerveModule rfModule, TrcSwerveModule rbModule, TrcGyro gyro, double wheelBaseWidth, double wheelBaseLength)
      Constructor: Create an instance of the 4-wheel swerve drive base.
      Parameters:
      lfModule - specifies the left front swerve module of the drive base.
      lbModule - specifies the left back swerve module of the drive base.
      rfModule - specifies the right front swerve module of the drive base.
      rbModule - specifies the right back swerve module of the drive base.
      gyro - specifies the gyro. If none, it can be set to null.
      wheelBaseWidth - specifies the width of the wheel base in inches.
      wheelBaseLength - specifies the length of the wheel base in inches.

    • TrcSwerveDriveBase

      public TrcSwerveDriveBase(TrcSwerveModule lfModule, TrcSwerveModule lbModule, TrcSwerveModule rfModule, TrcSwerveModule rbModule, double wheelBaseWidth, double wheelBaseLength)
      Constructor: Create an instance of the 4-wheel swerve drive base.
      Parameters:
      lfModule - specifies the left front swerve module of the drive base.
      lbModule - specifies the left back swerve module of the drive base.
      rfModule - specifies the right front swerve module of the drive base.
      rbModule - specifies the right back swerve module of the drive base.
      wheelBaseWidth - specifies the width of the wheel base in inches.
      wheelBaseLength - specifies the length of the wheel base in inches.

  • Method Details

    • zeroCalibrateSteering

      public void zeroCalibrateSteering()
      This method does zero calibration on the steer angle encoders.

    • getWheelBaseWidth

      public double getWheelBaseWidth()
      This method returns the wheel base width.
      Returns:
      wheel base width.

    • getWheelBaseLength

      public double getWheelBaseLength()
      This method returns the wheel base length.
      Returns:
      wheel base length.

    • supportsHolonomicDrive

      public boolean supportsHolonomicDrive()
      This method checks if it supports holonomic drive.
      Overrides:
      supportsHolonomicDrive in class TrcDriveBase
      Returns:
      true if this drive base supports holonomic drive, false otherwise.

    • setOdometryScales

      public void setOdometryScales(double xScale, double yScale, double angleScale)
      This method sets the odometry scales. The raw position from the encoder is in encoder counts. By setting the scale factor, one could make getPosition to return unit in inches, for example.
      Overrides:
      setOdometryScales in class TrcDriveBase
      Parameters:
      xScale - specifies the X position scale.
      yScale - specifies the Y position scale.
      angleScale - specifies the angle scale.

    • setOdometryScales

      public void setOdometryScales(double scale)
      This method sets the odometry scales. The raw position from the encoder is in encoder counts. By setting the scale factor, one could make getPosition to return unit in inches, for example.
      Overrides:
      setOdometryScales in class TrcDriveBase
      Parameters:
      scale - specifies the position scale for each motor.

    • setSteerAngle

      public void setSteerAngle(String owner, double angle, boolean optimize)
      This method sets the steering angle of all four wheels.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      angle - specifies the steering angle to be set.
      optimize - specifies true to optimize steering angle to be no greater than 90 degrees, false otherwise.

    • setSteerAngle

      public void setSteerAngle(double angle, boolean optimize)
      This method sets the steering angle of all four wheels.
      Parameters:
      angle - specifies the steering angle to be set.
      optimize - specifies true to optimize steering angle to be no greater than 90 degrees, false otherwise.

    • setSteerAngle

      public void setSteerAngle(double angle)
      This method sets the steering angle of all four wheels.
      Parameters:
      angle - specifies the steering angle to be set.

    • setModuleVelocities

      public void setModuleVelocities(double[][] velocities)
      Set the velocities of the swerve modules.
      Parameters:
      velocities - 2d array. First dimension is number of modules, in order [lf, rf, lr, rr]. Next dimension is polar vector in (r, theta). Theta is degrees CW, r is in range [-1,1].

    • stop

      public void stop(String owner, boolean resetSteer)
      This method stops the drive base. If steerNeutral is true, it also sets all steering angles to zero.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      resetSteer - specifies true to set steering angle to zero.

    • stop

      public void stop(boolean resetSteer)
      This method stops the drive base. If steerNeutral is true, it also sets all steering angles to zero.
      Parameters:
      resetSteer - specifies true to set steering angle to zero.

    • stop

      public void stop()
      This method stops the drive base and reset the steering angle to zero.
      Overrides:
      stop in class TrcDriveBase

    • tankDrive

      public void tankDrive(String owner, double leftPower, double rightPower, boolean inverted, double driveTime, TrcEvent event)
      This method implements tank drive where leftPower controls the left motors and right power controls the right motors. It will set the steering angle to zero, but note that it will take time for the steering angles to reach zero. Since we do not wait for the steering angle to reach neutral, it is possible the drive base will move diagonally initially. If this is undesirable, the caller should make sure steering angles are already at zero before calling this method.
      Overrides:
      tankDrive in class TrcSimpleDriveBase
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      leftPower - specifies left power value.
      rightPower - specifies right power value.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive

      public void holonomicDrive(String owner, double x, double y, double rotation, boolean inverted, double gyroAngle, double driveTime, TrcEvent event)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.

      The implementation of swerve algorithm is based on Ether's white paper

      Overrides:
      holonomicDrive in class TrcDriveBase
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      gyroAngle - specifies the gyro angle to maintain for field relative drive. DO NOT use this with inverted.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • setXMode

      public void setXMode(String owner)
      This method set all the wheels into an X configuration so that nobody can bump us out of position.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.

    • getOdometryDelta

      protected TrcDriveBase.Odometry getOdometryDelta(TrcOdometrySensor.Odometry[] prevOdometries, TrcOdometrySensor.Odometry[] currOdometries)
      This method is called periodically to calculate the delta between the previous and current motor odometries.
      Overrides:
      getOdometryDelta in class TrcSimpleDriveBase
      Parameters:
      prevOdometries - specifies the previous motor odometries.
      currOdometries - specifies the current motor odometries.
      Returns:
      an Odometry object describing the odometry changes since the last update.