Beispiel #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
Beispiel #2
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)