lookAheadSwitchDistance = 0.5 maxTurnRate = 1.2 damperLength = 5 #### Intialize robot = Robot() p = Path("Path-around-table-and-back.json") path = p.getPath() pathHandler = PathHandler() navigator = Navigator(maxSpeed, maxTurnRate, damperLength) robot.setMotion(0, 0) position = robot.getPosition() heading = 0 nextPoint = 0 while (navigator.notGoal(position, nextPoint, path)): ### get current status (position, heading) position = robot.getPosition() heading = navigator.convertToDegree(robot.getHeading()) ### get next point nextPoint = pathHandler.getNextPathPoint(position, path, nextPoint, lookAheadDistance, lookAheadSwitchDistance) ### find direction to path directionToPoint = navigator.getDirection(position, path, nextPoint) ### get turn direction +/- maximum turn rate turnDirection = navigator.getTurnDirection(heading, directionToPoint)