Package trclib.pathdrive

Class TrcGridDrive

java.lang.Object
trclib.pathdrive.TrcGridDrive
public class TrcGridDrive extends Object
This class implements the auto-assist grid drive. It allows the driver to use the DPad to quickly navigate the field maze in grid cell units in a square pattern accurately without the risk of running into obstacles at grid cell intersections. This algorithm assumes we have accurate odometry. If we don't, all bets are off.

  • Field Details

    • moduleName

      private final String moduleName

    • tracer

      public final TrcDbgTrace tracer

    • driveBase

      private final TrcDriveBase driveBase

    • purePursuitDrive

      private final TrcPurePursuitDrive purePursuitDrive

    • gridCellSize

      private final double gridCellSize

    • turnStartAdj

      private final double turnStartAdj

    • turnEndAdj

      private final double turnEndAdj

    • gridDriveTaskObj

      private final TrcTaskMgr.TaskObject gridDriveTaskObj

    • callbackEvent

      private final TrcEvent callbackEvent

    • gridDriveQueue

      private final ArrayList<TrcPose2D> gridDriveQueue

  • Constructor Details

    • TrcGridDrive

      public TrcGridDrive(TrcDriveBase driveBase, TrcPurePursuitDrive purePursuitDrive, double gridCellSize, double turnStartAdj, double turnEndAdj)
      Constructor: Creates an instance of the object.
      Parameters:
      driveBase - specifies the drive base object.
      purePursuitDrive - specifies the pure pursuit drive object.
      gridCellSize - specifies the grid cell size in inches.
      turnStartAdj - specifies the distance adjustment for the previous segment endpoint before the turn.
      turnEndAdj - specifies the distance adjustment for the next segment startpoint after the turn.

    • TrcGridDrive

      public TrcGridDrive(TrcDriveBase driveBase, TrcPurePursuitDrive purePursuitDrive, double gridCellSize)
      Constructor: Creates an instance of the object.
      Parameters:
      driveBase - specifies the drive base object.
      purePursuitDrive - specifies the pure pursuit drive object.
      gridCellSize - specifies the grid cell size in inches.

  • Method Details

    • cancel

      public void cancel()
      This method cancels Grid Drive if one is in progress.

    • isGridDriveActive

      public boolean isGridDriveActive()
      This method checks if Grid Drive is currently in progress.
      Returns:
      true if Grid Drive is active, false otherwise.

    • setTaskEnabled

      private void setTaskEnabled(boolean enabled)
      This method enables/disables the Grid Drive task.
      Parameters:
      enabled - specifies true to enable Grid Drive task, false to disable.

    • adjustToGridCellCenter

      private double adjustToGridCellCenter(double gridCellPos)
      This method adjusts the grid cell position to the grid cell center.
      Parameters:
      gridCellPos - specifies the position in grid cell units.
      Returns:
      adjusted position in grid cell units.

    • adjustGridCellHeading

      private double adjustGridCellHeading(double heading)
      This method adjusts the heading to the nearest 90-degree multiple.
      Parameters:
      heading - specifies the heading to be adjusted to the nearest 90-degree multiple.
      Returns:
      adjusted heading in the range of [0.0, 360.0)

    • poseToGridCell

      public TrcPose2D poseToGridCell(TrcPose2D pose)
      This method translates a real world pose to a grid cell pose.
      Parameters:
      pose - specifies the pose in real world units.
      Returns:
      pose in the grid cell units.

    • gridCellToPose

      public TrcPose2D gridCellToPose(TrcPose2D gridCell)
      This method translates a grid cell pose to a real world pose.
      Parameters:
      gridCell - specifies the grid cell pose in grid cell units.
      Returns:
      pose in real world units.

    • adjustGridCellCenter

      public TrcPose2D adjustGridCellCenter(TrcPose2D gridCell)
      This method takes a grid cell position that may not be at the center of the cell and adjusts it to the center of the grid cell it's on.
      Parameters:
      gridCell - specifies the grid cell to adjust in the unit of cells.
      Returns:
      adjusted gridCell in the unit of cells.

    • adjustPoseToGridCellCenter

      public TrcPose2D adjustPoseToGridCellCenter(TrcPose2D pose)
      This method adjusts the given pose to the nearest grid cell center pose.
      Parameters:
      pose - specifies the pose in real world units to be adjusted.
      Returns:
      adjusted pose in real world units.

    • resetGridCellCenter

      public void resetGridCellCenter()
      This method resets the drive base odometry to the nearest grid cell center. Odometry may get inaccurate for various reasons such as odometry wheel slippage, sensor drifting etc. This situation can be corrected by manually driving the robot to the center of a grid cell and call this method. It assumes the drift is within the current grid cell. Therefore, it resets the odometry back to the nearest grid cell center.

    • getIntermediateGridCell

      private TrcPose2D getIntermediateGridCell(TrcPose2D startCell, TrcPose2D endCell)
      This method returns an intermediate grid cell so that travelling from the start cell to the end cell will go through the intermediate cell such that the robot will only travel in the direction of north/south and east/west but not diagonally and will be centering itself in the grid cells.
      Parameters:
      startCell - specifies the start cell.
      endCell - specifies the end cell.
      Returns:
      intermediate cell if one is needed, null if end cell is already inline with the start cell.

    • setRelativeGridTarget

      private void setRelativeGridTarget(int xGridCells, int yGridCells)
      This method adds a movement segment to the drive queue. Note: it is expected that only one argument will be non-zero and the other one must be zero.
      Parameters:
      xGridCells - specifies the X movement in number of grid cells, positive for East, negative for West.
      yGridCells - specifies the Y movement in number of grid cells, positive for North, negative for South.

    • setRelativeXGridTarget

      public void setRelativeXGridTarget(int gridCells)
      This method adds an X movement segment to the drive queue.
      Parameters:
      gridCells - specifies the X movement in number of grid cells, positive for East, negative for West.

    • setRelativeYGridTarget

      public void setRelativeYGridTarget(int gridCells)
      This method adds an Y movement segment to the drive queue.
      Parameters:
      gridCells - specifies the Y movement in number of grid cells, positive for North, negative for South.

    • driveToEndPoint

      public void driveToEndPoint(TrcPose2D endPoint)
      This method generate a path from the current robot pose to the endpoint that snaps to the grid so it moves only in square pattern, never diagonal.
      Parameters:
      endPoint - specifies the endpoint in real world units.

    • gridDriveTask

      private void gridDriveTask(TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, boolean slowPeriodicLoop)
      This method is called periodically to process the drive queue.
      Parameters:
      taskType - specifies the type of task being run. This may be useful for handling multiple task types.
      runMode - specifies the current competition mode (e.g. Autonomous, TeleOp, Test).
      slowPeriodicLoop - specifies true if it is running the slow periodic loop on the main robot thread, false otherwise.

    • startGridDrive

      private void startGridDrive()
      This method processes the gridDriveQueue to build a path for PurePursuitDrive to follow.

    • driveDone

      private void driveDone(Object context)
      This method is called when the PurePursuitDrive operation is done. It releases the ownership of the drive base.
      Parameters:
      context - not used.

    • willTurn

      private boolean willTurn(TrcPose2D prevSegment, TrcPose2D nextSegment)
      This method determines if the next segment has a compatible heading with the previous segment. If not, the robot will turn.
      Parameters:
      prevSegment - specifies the previous segment position.
      nextSegment - specifies the next segment position.
      Returns:
      true if the robot will turn from the previous segment to the next segment, false otherwise.