# motion update particleFilter.motionUpdate(enc_distL, enc_distR) # measurement update particleFilter.measurementUpdate() # get predict state robotState = particleFilter.getPredictState() #print "From : ", int(robotState[0]), int(robotState[1]), int(robotState[2]*180/pi) #print "To : ", currentPointIndex, int(wayPoints[currentPointIndex][0]), int(wayPoints[currentPointIndex][1]) # set control signal #leftVel, rightVel, action = navigator.navigateToWayPoint(robotState, wayPoints[currentPointIndex]) leftVel, rightVel, action = navigator.navigateToWayPointStateFul(robotState, enc_distL, enc_distR, wayPoints[currentPointIndex]) if action is not lastAction: robot.motors.reset() lastAction = action robot.motors.setVel(leftVel, rightVel, enc_velL, enc_velR) # set waypoint index if(abs(leftVel) < NEAR_ZERO and abs(rightVel) < NEAR_ZERO): currentPointIndex += 1 if(currentPointIndex >= len(wayPoints)): break # draw particle particleFilter.drawParticles()
