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

  • 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.

    • 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.

    • 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.