def navigateToWaypoint(x, y):
(currentX, currentY, currentAngle) = particles.estimate_location()
print "Navigating from (", currentX, ", ", currentY, ") -> (", x, ", ", y, ")"
dx = x - currentX
dy = y - currentY
distance = math.sqrt(dx**2 + dy**2)
theta_portion = math.fabs(math.degrees(math.atan(dy / dx)))
print "Current Angle: ", currentAngle
target_theta = 0
if dx > 0 and dy > 0:
#target_theta = 360 - theta_portion
target_theta = theta_portion
elif dx > 0 and dy < 0:
#target_theta = theta_portion
target_theta = 360 - theta_portion
elif dx < 0 and dy > 0:
#target_theta = 180 + theta_portion
target_theta = 180 - theta_portion
elif dx < 0 and dy < 0:
#target_theta = 180 - theta_portion
target_theta = 180 + theta_portion
print "Target Angle:", target_theta
rotation_theta = target_theta - currentAngle
if rotation_theta > 180:
rotation_theta = rotation_theta - 360
elif rotation_theta < -180:
rotation_theta = rotation_theta + 360
print "Rotation Angle:", rotation_theta
rotate(rotation_theta)
forward_amount = min(distance, 20)
fwd_amt(forward_amount)
stop()
time.sleep(0.05)
particles.update_forward(forward_amount)
for i in range(0, 9):
sonar_reading = get_sonar_distance()
if sonar_reading < 255:
particles.update_probability(sonar_reading)
break
else:
time.sleep(0.1)
else:
particles.update_probability(get_sonar_distance())
particles.draw()
def navigateToWaypoint(x, y):
(currentX, currentY, currentAngle) = particles.estimate_location()
print "Navigating from (",currentX,", ",currentY,") -> (",x,", ",y,")"
dx = x - currentX
dy = y - currentY
distance = math.sqrt(dx**2 + dy**2)
theta_portion = math.fabs(math.degrees(math.atan(dy / dx)))
print "Current Angle: ", currentAngle
target_theta = 0
if dx > 0 and dy > 0:
#target_theta = 360 - theta_portion
target_theta = theta_portion
elif dx > 0 and dy < 0:
#target_theta = theta_portion
target_theta = 360 - theta_portion
elif dx < 0 and dy > 0:
#target_theta = 180 + theta_portion
target_theta = 180 - theta_portion
elif dx < 0 and dy < 0:
#target_theta = 180 - theta_portion
target_theta = 180 + theta_portion
print "Target Angle:", target_theta
rotation_theta = target_theta - currentAngle
if rotation_theta > 180:
rotation_theta = rotation_theta - 360
elif rotation_theta < -180:
rotation_theta = rotation_theta + 360
print "Rotation Angle:", rotation_theta
rotate(rotation_theta)
forward_amount = min(distance, 20)
fwd_amt(forward_amount)
stop()
time.sleep(0.05)
particles.update_forward(forward_amount)
for i in range(0,9):
sonar_reading = get_sonar_distance()
if sonar_reading < 255:
particles.update_probability(sonar_reading)
break
else:
time.sleep(0.1)
else:
particles.update_probability(get_sonar_distance())
particles.draw()
def estimate_location():
return particles.estimate_location()
def estimate_location(): return particles.estimate_location()