#### 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)
### determine dynamic turn rate based on interecpt angle
turnRate = navigator.getTurnRate(directionToPoint, turnDirection)
