def navigateToWaypoint(X, Y):
global estimate_x
global estimate_y
global estimate_theta
global p_x
global p_y
global p_theta
global weights
global mu
global sigma_e
global sigma_f
global sigma_g
print "Silly test " + str(estimate_x) + " " + str(estimate_y) + " " + str(
estimate_theta)
x_diff = X - estimate_x
y_diff = Y - estimate_y
dist = (x_diff**2 + y_diff**2)**0.5
angleDest = math.atan2(y_diff,
x_diff) # returns an angle between - pi and pi
angleRotate = angleDest - estimate_theta
L01.left_90(angleRotate / 1.5708)
time.sleep(2.5)
#particles_rot = []
for k in range(100):
error_g = random.gauss(mu, sigma_g)
p_theta[k] += angleRotate + error_g
# particles_rot.append((5*p_x[k], 5*p_y[k], p_theta[k]))
#print "drawParticles:" + str(particles_rot) # should print out the drawParticles
L01.forward(dist)
time.sleep(dist * 0.2)
#particles = []
for k in range(
100
): #this code is only relevant for correction once a move has been logged
error_e = random.gauss(mu, sigma_e)
error_f = random.gauss(mu, sigma_f)
p_x[k] += (dist + error_e) * math.cos(angleDest)
p_y[k] += (dist + error_e) * math.sin(angleDest)
p_theta[k] += error_f
# particles.append((5*p_x[k], 5*p_y[k], p_theta[k]))
#print "drawParticles:" + str(particles) # should print out the drawParticles
# now to update current guess of position
estimate_x = 0.0
estimate_y = 0.0
estimate_theta = 0.0
for i in range(100):
estimate_x += p_x[i] * weights[i]
estimate_y += p_y[i] * weights[i]
estimate_theta += p_theta[i] * weights[i]
def navigateToWaypoint(X, Y): # X,Y are cords of dest
global estimate_x
global estimate_y
global estimate_theta
global p_x
global p_y
global p_theta
global mu
global sigma_e
global sigma_f
global sigma_g
#print "Silly test " + str(estimate_x) +" "+ str(estimate_y) +" "+str(estimate_theta)
x_diff = X - estimate_x
y_diff = Y - estimate_y
dist = (x_diff**2 + y_diff**2)**0.5
angleDest = math.atan2(y_diff,
x_diff) # returns an angle between - pi and pi
angleRotate = angleDest - estimate_theta
L01.left_90(angleRotate / 1.5708)
time.sleep(2.5)
e = sigma_e * math.sqrt(dist / 10)
f = sigma_f * math.sqrt(dist / 10)
g = sigma_g * math.sqrt(dist / 10)
#update all the particles angle
for k in range(100):
error_g = random.gauss(mu, g)
p_theta[k] += angleRotate + error_g
L01.forward(dist)
time.sleep(dist * 0.08)
for k in range(
100
): #this code is only relevant for correction once a move has been logged
error_e = random.gauss(mu, e)
error_f = random.gauss(mu, f)
p_x[k] += (dist + error_e) * math.cos(angleDest)
p_y[k] += (dist + error_e) * math.sin(angleDest)
p_theta[k] += error_f
def navigateToWaypoint(X, Y): # X,Y are cords of dest global estimate_x global estimate_y global estimate_theta global p_x global p_y global p_theta global mu global sigma_e global mu_f global sigma_f global sigma_g global mean_g while(not there_yet(X,Y)): # calc dist and angle to move still x_diff = X-estimate_x y_diff = Y-estimate_y dist = (x_diff**2 + y_diff**2)**0.5 # 20cm bursts dist = min(dist, 20) angleDest = math.atan2(y_diff,x_diff) # returns an angle between - pi and pi angleRotate = angleDest - estimate_theta # rotate robot if angleRotate <= -math.pi: ar = angleRotate + 2*math.pi L01.left_90(ar/(math.pi/2)) elif angleRotate <= 0: ar = -angleRotate L01.right_90(ar/(math.pi/2)) elif angleRotate <= math.pi: L01.left_90(angleRotate/(math.pi/2)) else: # angleRoate <= 2*math.pi ar = 2*math.pi - angleRotate L01.right_90(ar/(math.pi/2)) time.sleep(2) # standard deviations for particle simualtion - scaled for distance s_e = sigma_e * math.sqrt(dist/10) s_f = sigma_f * math.sqrt(dist/10) # update all the particle angles for k in range (100): # scale mean for actual rotate mean_g = (angleRotate / (math.pi/2)) * mean_g error_g = random.gauss(mean_g,sigma_g) p_theta[k] += angleRotate + error_g # move robot forward L01.forward(dist) time.sleep(dist*0.08) # update all the particle positions for k in range(100): error_e = random.gauss(mu,s_e) error_f = random.gauss(mu_f,s_f) p_x[k] += (dist + error_e) * math.cos(angleDest+error_f) p_y[k] += (dist + error_e) * math.sin(angleDest+error_f) p_theta[k] += error_f # read sonar - start of MCL reading = read_sonar() for k in range(100): particles.data.append((p_x[k], p_y[k], p_theta[k], weights[k])) particles.draw() update_particles(reading) normalise() resample() time.sleep(1) # re-estimate position based on particles updatePos() # print particles to server # remove particles form presample particles.data = particles.data[:len(particles.data)-100] for k in range(100): particles.data.append((p_x[k], p_y[k], p_theta[k], weights[k])) particles.draw()