roboroach._set_np(1) print("Initialized") ########################## ####### CALIBRATION ########################## print("Starting calibration... (press <Ctrl-C> to finish)") ref_pos = [] try: while True: pos = polaris_driver.getPositionFromBX("1801") if not 'miss' in pos: ref_pos.append(pos) time.sleep(0.1); except KeyboardInterrupt: pass ref_pos = np.mean(ref_pos[1:], axis=0) print("\nPosition: ["+str(ref_pos[0])+", "+str(ref_pos[1])+", "+str(ref_pos[2])+"]") print("Calibrated") ########################## ####### CONTROL ########################## print("Line follower started! (press <Ctrl-C> to finish)") POS_THRESH_POS = 1.5 POS_THRESH_NEG = -1.5 GAIN_MAX = 250 GAIN_MIN = 150
roboroach._set_np(1) print("Initialized") ########################## ####### CALIBRATION ########################## print("Starting path acquisition... (press <Ctrl-C> to finish)") ref_pos = np.array([]) try: while True: pos = polaris_driver.getPositionFromBX("1801") if not 'miss' in pos: ref_pos = np.vstack([ref_pos, pos]) time.sleep(0.1); except KeyboardInterrupt: pass print("Path acquired!") ########################## ####### CONTROL ########################## print("Path follower started! (press <Ctrl-C> to finish)") pos_vec = np.array([0, 0, 0]) i = 0 try: while i < len(pos)-1 ref_vec = [pos[i], pos[i+1]]