Path Planning

This contains instructions on how to incorporate path planning and path tracking into the WSS student code base.

The code includes:

  • Building a grid map containing boundaries and obstacles

  • Astar path planning

  • Spline path planning and tracking (adapted from wpilib)

Trajectory-Imported.git

The map building and path planning was first developed on PC because it is convenient to display the grid and path information on the PC graphic display.

Many of the codes in this project is used in the robot, minus the graphical display.

It is suggested that when preparing for a new arena, change the Layout.java in this project and test it. After visually checking everything is OK, copy the Layout .java to your robot project.

Path planning and following

After testing out the codes in Trajectory-Imported.git, it is necessary to incorporate the codes into your robot code.

The source will be https://github.com/ngbengkiat/WSS2023.git. You may not want to copy the whole project but only some of the files.

Following these steps:

  • Copy directories "util" and "Astar" from WSS2023 into yours

  • Copy file "commands/auto/MoveToB.java" from WSS2023 into yours

  • Modify RobotContainer.java to add in the following codes:

    • Add these into the constructor

m_layout = new Layout();

m_Grid = new Grid(m_layout);

m_Grid.ExpandObstacles(150);


// Create solver

m_Astar = new AStarAlgorithm(m_Grid);


  • Robot start pose (x,y,dir)

    • Layout.java

    • omnidrive.java

      • m_odometry = new OmniDriveOdometry( Layout.Convert_mm_Pose2d(Layout.startPos));