def moveToBottle_2(currentPosition, bottlePosition, particles):
cx = currentPosition[0]
cy = currentPosition[1]
ctheta = math.radians(currentPosition[2]) # We store the theta in degree
wx = bottlePosition[0]
wy = bottlePosition[1]
angle, distance = calculate_angle_distance(cx, cy, wx, wy, ctheta,
currentPosition)
turn.turn(math.degrees(angle))
particles = [
updateRotation(particles[i], math.degrees(angle))
for i in range(numberOfParticles)
]
preAngle = interface.getMotorAngle(motors[0])[0]
_gostraight = gostraight.go()
distance, angle = _gostraight.run(30)
aftAngle = interface.getMotorAngle(motors[0])[0]
disMoved = abs((aftAngle - preAngle) / (math.pi * 0.124))
particles = [
update(particles[i], disMoved) for i in range(numberOfParticles)
]
particles = [
updateRotation(particles[i], angle) for i in range(numberOfParticles)
]
return particles
def navigateToWayPoint(wx, wy, currentPosition, particles, sonic, range_index):
Drawer = pds.Canvas()
_gostraight = gostraight.go()
bottle_particles = []
cx = currentPosition[0]
cy = currentPosition[1]
ctheta = math.radians(currentPosition[2]) # We store the theta in degree
#Calculate the distance and Angle to turn
angle, distance = calculate_angle_distance(cx, cy, wx, wy, ctheta,
currentPosition)
#Let it rotate towards that position
turn.turn(math.degrees(angle))
particles = [
updateRotation(particles[i], math.degrees(angle))
for i in range(numberOfParticles)
]
everymotion = 20.0
stop_times = int(distance / everymotion)
remain_distance = distance - stop_times * everymotion
bottle_particles = []
for index in range(
stop_times): #Every 20 cm, stop and search for the bottle
preAngle = interface.getMotorAngle(motors[0])[0]
_gostraight.run(everymotion)
aftAngle = interface.getMotorAngle(motors[0])[0]
disMoved = abs((aftAngle - preAngle) / (math.pi * 0.124))
particles = [
update(particles[i], disMoved) for i in range(numberOfParticles)
]
particles = mcl(particles)
Drawer.drawParticles(particles)
if index >= 1:
#bottle_particles.append(detect_bottle_2(sonic,getCurrentPosition(particles),bottle_particles))
bottle_particles = detect_bottle(sonic,
getCurrentPosition(particles),
bottle_particles, range_index)
if bottle_particles:
print 'Bottle_position:', bottle_particles
print 'current_Position:', getCurrentPosition(particles)
#Drawer.drawParticles(bottle_particles)
#sorted_d = fuse_information(bottle_particles,range_index)
# particles = moveToBottle(getCurrentPosition(particles),getBottlePosition_2(sorted_d),particles)
particles = moveToBottle_2(getCurrentPosition(particles),
getBottlePosition(bottle_particles), particles)
return particles
def navigateToWayPoint(wx, wy, currentPosition, particles):
cx = currentPosition[0]
cy = currentPosition[1]
ctheta = math.radians(currentPosition[2]) # We store the theta in degree
distance = math.hypot(wx - cx, wy - cy) # This is the main distance
print "navigating to " + str(wx) + "," + str(wy) + " from " + str(currentPosition)
print "distance is " + str(distance)
angle = (math.atan2(wy - cy, wx - cx)) - ctheta # math.atan2 returns in radians
print "angle before is " + str(angle)
if abs(angle) >= math.pi:
if angle > 0:
angle = -((math.pi * 2) - angle)
else:
angle = -((-math.pi * 2) - angle)
print "angle to turn is " + str(angle)
if abs(math.degrees(angle) - 90) < 8:
turn.turn(90)
particles = [updateRotation(particles[i], 90) for i in range(numberOfParticles)]
elif abs(math.degrees(angle) + 90) < 8:
turn.turn(-90)
particles = [updateRotation(particles[i], -90) for i in range(numberOfParticles)]
else:
turn.turn(math.degrees(angle))
particles = [updateRotation(particles[i], math.degrees(angle)) for i in range(numberOfParticles)]
Angle_before = interface.getMotorAngle(0)[0]
_gostraight = gostraight.go()
distanceMoved = _gostraight.run(distance)
print "dist moved: " + str(distanceMoved)
if distanceMoved[1] is 90:
particles = [update(particles[i], distanceMoved[0]) for i in range(numberOfParticles)]
return particles,True
else:
Angle_after = interface.getMotorAngle(0)[0]
d = (Angle_after - Angle_before) / (-math.pi * 0.124)
d = abs(d)
print 'd:', d
particles = [update(particles[i], d) for i in range(numberOfParticles)]
# print "Particles are now " + str(particles)
# update particles
# particles = mcl(particles)
# print "Current Position is " + str(currentPosition)
return particles,False
def turnToWayPoint(wx, wy, currentPosition, particles):
cx = currentPosition[0]
cy = currentPosition[1]
ctheta = math.radians(currentPosition[2]) # We store the theta in degree
distance = math.hypot(wx - cx, wy - cy) # This is the main distance
print "navigating to " + str(wx) + "," + str(wy) + " from " + str(currentPosition)
print "distance is " + str(distance)
angle = (math.atan2(wy - cy, wx - cx)) - ctheta # math.atan2 returns in radians
if abs(angle) >= math.pi:
if angle > 0:
angle = -((math.pi * 2) - angle)
else:
angle = -((-math.pi * 2) - angle)
print "angle to turn is " + str(angle)
if abs(math.degrees(angle) - 90) < 8:
turn.turn(90)
particles = [updateRotation(particles[i], 90) for i in range(numberOfParticles)]
elif abs(math.degrees(angle) + 90) < 8:
turn.turn(-90)
particles = [updateRotation(particles[i], -90) for i in range(numberOfParticles)]
else:
turn.turn(math.degrees(angle))
particles = [updateRotation(particles[i], math.degrees(angle)) for i in range(numberOfParticles)]
# Angle_before = interface.getMotorAngle(0)[0]
# _gostraight = gostraight.go()
# _gostraight.run(distance)
# Angle_after = interface.getMotorAngle(0)[0]
# d = (Angle_after - Angle_before) / (-math.pi * 0.124)
# print 'd:', d
# particles = [update(particles[i], d) for i in range(numberOfParticles)]
# update particles
particles = mcl(particles)
return particles