Esempio n. 1
0
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
Esempio n. 2
0
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()
Esempio n. 3
0
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()
Esempio n. 4
0
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()
Esempio n. 5
0
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)
Esempio n. 6
0
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)