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):
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
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
_dir, readings = prb.characterize_location_for_real(_sonnic, _dir)
#print "Scanned " + str(readings)
scannedHisto = ConvertToHisto(readings)
#print "Got histo " + str(scannedHisto)
# match, _dir = prb.recognize_location(signatures, _dir)
match = signatures.read(index)
histo_match = signatures.read_histogram(index)
angle = prb.findAnomaly(
readings, match
) #, histo_match, scannedHisto) # anomaly in absolute angle (degrees)
#print 'Anomalous angle is at dsfsdfsdf: ', angle
#print 'particlesTurnning is ', current_particles[2]
toturn = angle - current_particles[2]
turn.turn(-toturn)
particles = [
pu.updateRotation(particles[i], toturn)
for i in range(numberOfParticles)
]
ob = gostraight.go()
distance, angle = ob.run(80)
particles = [
pu.updateRotation(particles[i], angle)
for i in range(numberOfParticles)
]
particles = [
pu.update(particles[i], distance) for i in range(numberOfParticles)
]
# waypointofobject = getLocationOfObject(pu.getCurrentPosition(particles), angle)
# nwp.navigateToWayPoint(waypointofobject[0], waypointofobject[1], pu.getCurrentPosition(particles), particles)
from place_rec_bits import *
from turn import *
import particleUpdate as pu
from config import *
import navigateToWayPoint as nwp
waypoint1 = (84, 30)
waypointa = (157.5, 30)
waypointb = (126, 120)
waypointc = (42, 80)
waypoints = [waypointa, waypointb, waypointc]
signatures = SignatureContainer(5)
signatures.delete_loc_files()
particles = [initialPosition for i in range(numberOfParticles)]
_dir = 1
for waypoint in waypoints:
particles = nwp.navigateToWayPoint(waypoint[0], waypoint[1],
pu.getCurrentPosition(particles),
particles)
angleAlign = -pu.getCurrentPosition(particles)[2]
turn(angleAlign)
particles = [
pu.updateRotation(particles[i], angleAlign)
for i in range(numberOfParticles)
]
_dir = learn_location(signatures, _dir)
from place_rec_bits import *
from turn import *
import particleUpdate as pu
from config import *
import navigateToWayPoint as nwp
waypoint1 = (84,30)
waypointa = (157.5,30)
waypointb = (126,120)
waypointc = (42,80)
waypoints = [waypointa,waypointb,waypointc]
signatures = SignatureContainer(5)
particles = [initialPosition for i in range(numberOfParticles)]
for waypoint in waypoints:
particles = nwp.navigateToWayPoint(waypoint[0],waypoint[1] , pu.getCurrentPosition(particles), particles)
angleAlign = -pu.getCurrentPosition(particles)[2]
turn(angleAlign)
particles = [pu.updateRotation(particles[i], angleAlign) for i in range(numberOfParticles)]
recognize_location(signatures)