def characterize_location(ls):
global direction
pause = pi/180/rspeed
rm.sonar_rotate_v(interface,rspeed*direction)
for i in range(len(ls.sig)):
ls.sig[i] = rm.sonar_measurement(interface)
time.sleep(pause)
rm.sonar_rotate_v_stop(interface)
if direction == -1:
ls.sig = list(reversed(ls.sig))
direction = -direction
def pseudo_uncertainTurn(alpha): # in degrees
global particles
alpha = alpha % 360
sigma_g = np.sqrt(alpha) * stdev_g
new_particles = []
for p in particles.data:
g = random.gauss(0,sigma_g)
new_particles.append((p[0], p[1], p[2]+alpha+g,p[3]))
particles.data = new_particles
sonar_reading = rm.sonar_measurement(interface)
particles.update(sonar_reading)
particles.draw()
def pseudo_uncertainMove(D):
global particles
sigma_e = np.sqrt(D) * stdev_e
sigma_f = np.sqrt(D) * stdev_f
new_particles = []
for p in particles.data: # update particles
e = random.gauss(0,sigma_e)
f = random.gauss(0,sigma_f)
theta = radians(p[2])
new_particles.append((p[0]+(D+e)*cos(theta), p[1]+(D+e)*sin(theta), p[2]+f, p[3]))
particles.data = new_particles
sonar_reading = rm.sonar_measurement(interface)
particles.update(sonar_reading)
particles.draw()
def uncertainTurn(alpha): # in degrees
global particles
alpha = alpha % 360
if alpha<=180:
rm.turn_left(interface,alpha)
else:
rm.turn_right(interface,360-alpha)
sigma_g = np.sqrt(alpha) * stdev_g
new_particles = []
for p in particles.data:
g = random.gauss(0,sigma_g)
new_particles.append((p[0], p[1], p[2]+alpha+g,p[3]))
particles.data = new_particles
sonar_reading = rm.sonar_measurement(interface)
particles.update(sonar_reading)
particles.draw()
def detect_angle():
global direction
pause = pi/180/rspeed
rm.sonar_rotate_v(interface,rspeed*direction)
measurements = np.zeros(360)
for i in range(360):
measurements[i] = rm.sonar_measurement(interface)
time.sleep(pause)
rm.sonar_rotate_v_stop(interface)
if direction == -1:
measurements = np.array(measurements[::-1])
direction = -direction
print measurements
pos = np.where(measurements>30)[0]
a = len(np.where(pos<20)[0])
b = len(np.where(pos>340)[0])
if a>1 and b>1:
for i in range(len(pos)):
if pos[i] < 80:
pos[i] += 360
return np.mod((270 - int(np.average(pos)))+360,360)
PB = (273,63)
PC = (525,63)
PD = (21,21)
PE = (273,21)
PF = (525,21)
points = [PA,PB,PC]
path = []
theta = detect_angle()
particles = Particles(PA[0],PA[1],theta,numberOfParticles)
navigateToWaypoint(PD)
rm.sonar_rotate(interface,90)
left_reading = rm.sonar_measurement(interface)
rm.sonar_rotate(interface,-180)
right_reading = rm.sonar_measurement(interface)
rm.sonar_rotate(interface,90)
print left_reading, right_reading
if left_reading < 35:
print 'Position C'
move_points_to_C()
elif right_reading < 35:
print 'Position A'
uncertainTurn(90)
rm.sonar_rotate(interface,-90)
rm.wall_following(interface,wf_speed,21,half_t,-1)
rm.sonar_rotate(interface,90)