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
]
# canvas = pds.Canvas()
# mymap = pds.Map()
# pds.drawWall(mymap, canvas)
particles = [initialPosition for i in range(numberOfParticles)]
# canvas.drawParticles(particles)
signatures = prb.SignatureContainer()
# bumper = bumper.Bumper()
# bumperThread = threading.Thread(name='bumper', target=bumper.getTouch)
# bumperThread.start()
_sonnic = sonic.Sonic()
_gostraight = gostraight.go()
_dir = 1
def ConvertToHisto(readings):
step = 10
s = [0] * (200 / step)
for currenttuple in readings:
currentAngle = currenttuple[0]
currentDistance = currenttuple[1]
for idx, freq in enumerate(s):
# print "Comparing current Distance " + str(currentDistance) + " against " + str(step*(idx+1))
if (step * (idx + 1) > currentDistance):
# print str(currentDistance) + " has been allocated to " + str(idx+1)
import navigateToWayPoint as nwp
import turn as turn
import gostraight
import sonic
lengthOfPi = 10
waypoint1 = (84, 30, 0)
waypointa = (157.5, 30, 0)
waypointab = (126, 30, 1)
waypointb = (126, 120, 0)
waypointbc = (126, 112, 1)
waypointc = (42, 80, 0)
waypoints = [waypointa, waypointab, waypointb, waypointbc, waypointc]
ob = gostraight.go()
dir = 1
row = 1
noOfRows = 4
halfDistance = 100
while (1):
distance = ob.run(800)
if (row == noOfRows):
ob.goback(halfDistance - 20)
turn.turn(90 * dir)
ob.run(lengthOfPi)
turn.turn(-90 * dir)
ob.run(20)
turn.turn(180)
dir *= -1