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 breakdownAndNavigateToWayPoint(waypoint, particles):
print "renjie sees that particles are" + str(particles)
currentPosition = pu.getCurrentPosition(particles)
#print "Current Position is 1 " + str(currentPosition) #current position is wrong
step = 20.0
distance = math.hypot(waypoint[0] - currentPosition[0],
waypoint[1] - currentPosition[1])
#gradient = (waypoint[1]-currentPosition[1])/(waypoint[0]-currentPosition[0])
#print "distance is " + str(distance)
#print "gradient is " + str(gradient)
ctheta = math.radians(currentPosition[2])
#print "ctheta is " + str(ctheta)
angle = math.atan2(waypoint[1] - currentPosition[1],
waypoint[0] - currentPosition[0])
#print "waypoint[1] is " + str(waypoint[1])
#print "waypoint[0] is " + str(waypoint[0])
#print "need to turn to " + str(angle)
#math.atan2 returns in radians
dy = math.sin(angle) * step
dx = math.cos(angle) * step
#print "dx is " + str(dx)
#print "dy is " + str(dy)
steps = int(distance / step)
remainingDistance = distance % step
for i in range(1, steps + 1):
particles = navigateToWayPoint(currentPosition[0] + dx,
currentPosition[1] + dy,
currentPosition,
particles) # this particles are wrong
#print "rj sees that particles are" + str(particles)
currentPosition = getCurrentPosition(particles)
print "Current Position is:", "iteration:", i, ":" + str(
currentPosition)
canvas.drawParticles(particles)
#dy = math.sin(theta)*remainingDistance
#dx = math.cos(theta)*remainingDistance
particles = navigateToWayPoint(waypoint[0], waypoint[1], currentPosition,
particles)
return particles
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)
s[idx] = freq + 1
break
return s
for index, waypoint in enumerate(waypoints):
print "main 6666 curr position" + str(pu.getCurrentPosition(particles))
particles, bumped = nwp.navigateToWayPoint(
waypoint[0], waypoint[1], pu.getCurrentPosition(particles), particles)
if bumped:
continue
if waypoint[2] is 0:
#if index is 0:
#nwp.wallCheck(_gostraight, _sonnic, _dir)
# include bumped here, then subsequent action
current_particles = pu.getCurrentPosition(particles)
# ls = prb.LocationSignature()
_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)
waypointbc = (126, 112, 1)
waypointc = (42, 80, 0)
waypoints = [waypointa, waypointb, waypointc]
canvas = pds.Canvas()
mymap = pds.Map()
pds.drawWall(mymap, canvas)
particles = [initialPosition for i in range(numberOfParticles)]
canvas.drawParticles(particles)
_sonnic = sonic.Sonic()
_dir = 1
for index, waypoint in enumerate(waypoints):
particles = nwp.navigateToWayPoint(waypoint[0], waypoint[1],
pu.getCurrentPosition(particles),
particles, _sonnic, index)
nwp.navigateToWayPoint(waypoint1[0], waypoint1[1],
pu.getCurrentPosition(particles), particles, _sonnic)
# at each waypoint do a scan
# go to new location with detected object
# bump
# go to next waypoint
# repeat till last object is bumped
# go back to waypoint 1
from config import *
import particleUpdate as pu
from navigateToWayPoint2 import *
waypoint1 = (84, 30)
waypoint2 = (180, 30)
waypoint3 = (180, 54)
waypoint4 = (138, 54)
waypoint5 = (138, 168)
waypoint6 = (114, 168)
waypoint7 = (114, 84)
waypoint8 = (84, 84)
waypoint9 = (84, 30)
waypoints = [
waypoint2, waypoint3, waypoint4, waypoint5, waypoint6, waypoint7,
waypoint8, waypoint9
]
canvas = pds.Canvas()
mymap = pds.Map()
pds.drawWall(mymap, canvas)
particles = [initialPosition for i in range(numberOfParticles)]
canvas.drawParticles(particles)
for waypoint in waypoints:
#particles = breakdownAndNavigateToWayPoint(waypoint,particles)
particles = navigateToWayPoint(waypoint[0], waypoint[1],
pu.getCurrentPosition(particles), particles)
#particles = navigateToWayPoint(waypoint[0],waypoint[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)