#### Intialize variables position = robot.getPosition() speed = 0.25 heading = 0 nextPoint = 0 dropOut = 0 lookAheadDistance = 0.85 while (goal.notGoal(position, nextPoint, path)): ### get current status (position, heading) position = robot.getPosition() heading = converter.convertToDegree(robot.getHeading()) ### get next point nextPoint = pathHandler.getNextPathPoint(position, path, nextPoint, lookAheadDistance) print(nextPoint) ### find direction to path directionToPoint = navigator.getDirection(position, path, nextPoint) turnRate = navigator.getTurnRate(heading, directionToPoint) ### set motion robot.setMotion(speed, turnRate) sleep(0.35) robot.setMotion(0, 0) print('Geeeeeehaaaaaaa')
### 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) ### apply a dampler on the turn rate output turnRate = navigator.dampen(turnRate) ### dynamic speed adjustment according turnRate (currently not in use) #speed = navigator.adjustSpeed( turnRate ) speed = maxSpeed ### set motion robot.setMotion(speed, turnRate) sleep(0.1) robot.setMotion(0, 0) sleep(1)