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 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)