def cmd_tleds(): logging.debug("Running cmd_tleds") oled.write_line(1, "") for x in range(0, 16): oled.write_line(0, ("." * (x // 4 + 1)).ljust(4), 1) set_led_int(x % 16) time.sleep(0.4) set_led_int(0) return False
def cmd_run(): logging.debug("Running cmd_run") if WIIMOTE is None: oled.write_line(0, "No wiimote", 1) oled.write_line(1, "conencted", 1) time.sleep(1.5) return False oled.write_line(0, "Press A", 1) oled.write_line(1, "for options", 1) time.sleep(1.5) but = WIIMOTE.state.get("buttons", 0) pwr = [0, 0] # L, R while not but & cwiid.BTN_HOME: s = WIIMOTE.state but, acc = s.get("buttons", 0), s.get("acc", [0, 0, 0]) roll, pitch, accel = [x - y for (x, y) in zip(acc, ACC_CAL)] if but & cwiid.BTN_2: pwr = [ 1 - (pitch - 2) / 40. if pitch > 2 else 1, 1 + (pitch + 2) / 40. if pitch < -2 else 1 ] elif but & (cwiid.BTN_1 | cwiid.BTN_B): # brake - similar to else but more reduction pwr = [ round(pwr[0] - 0.8 - ((pitch - 2) / 40. if pitch > 2 else 0), 3), round(pwr[1] - 0.8 + ((pitch + 2) / 40. if pitch < -2 else 0), 3) ] else: # reduce each by an amount and round to 3dp pwr = [ round(pwr[0] - 0.3 - ((pitch - 2) / 40. if pitch > 2 else 0), 3), round(pwr[1] - 0.3 + ((pitch + 2) / 40. if pitch < -2 else 0), 3) ] if pwr[0] < 0: pwr[0] = 0 if pwr[1] < 0: pwr[1] = 0 oled.write_lines( "{} ->".format(pitch), "({}, {})".format(*[str(float(x)).ljust(5, "0") for x in pwr])) time.sleep(0.1) run_motors(m0=pwr[0], m1=pwr[0], m2=pwr[1], m3=pwr[1]) run_motors() # stop all time.sleep(0.2) return False
def cmd_wcalibrate(): logging.debug("Running cmd_wcalibrate") global ACC_CAL if WIIMOTE is None: oled.write_line(0, "No wiimote", 1) oled.write_line(1, "conencted", 1) time.sleep(1.5) return False options = ["Default", "+^- ", " +^- ", " +^-"] loc = 0 while True: #btns = get_button_int() s = WIIMOTE.state wmbut, acc = s.get("buttons", 0), s.get("acc", [0, 0, 0]) roll, pitch, accel = [x - y for (x, y) in zip(acc, ACC_CAL)] oled_write_menu("{: 3},{: 3},{: 3}".format(roll, pitch, accel), options[loc], "^v><") if wmbut & (cwiid.BTN_B | cwiid.BTN_UP): time.sleep(0.2) return False elif options == 0 and wmbut & cwiid.BTN_A: ACC_CAL = WIIMOTE.get_acc_cal(cwiid.EXT_NONE)[0] oled.write_line(1, "Cal -> default!") for x in range(10): s = WIIMOTE.state but, acc = s.get("buttons", 0), s.get("acc", [0, 0, 0]) roll, pitch, accel = [x - y for (x, y) in zip(acc, ACC_CAL)] oled.write_line(0, "{: 3},{: 3},{: 3}".format(roll, pitch, accel)) time.sleep(0.1) elif options > 0 and wmbut & cwiid.BTN_PLUS: ACC_CAL[options - 1] += 1 time.sleep(0.2) elif options > 0 and wmbut & cwiid.BTN_MINUS: ACC_CAL[options - 1] -= 1 time.sleep(0.2) elif wmbut & cwiid.BTN_LEFT: loc += 1 time.sleep(0.2) elif wmbut & cwiid.BTN_RIGHT: loc -= 1 time.sleep(0.2) loc %= 4
ips = get_ips() ips = filter(lambda x: re.match("^(eth|wlan)\d+$", x["scopename"]), ips) return list(ips) def get_model(): return run_cmd("cat /proc/device-tree/model").rstrip("\x00") def get_git_revision(): return run_cmd("git rev-parse --short HEAD").rstrip("\n") # ---------------------------- oled.init() try: pfd.init() piface = True except pfd.NoPiFaceDigitalDetectedError: piface = False try: logging.debug("OLED: 'Running in rc.local'") oled.write_line(0, "Running in", 1) oled.write_line(1, "rc.local", 1) if piface: for x in range(3): pfd.digital_write(7, 1) time.sleep(0.5) pfd.digital_write(7, 0)
def cmd_twiimote(): logging.debug("Running cmd_twiimote") if WIIMOTE is None: oled.write_line(0, "No wiimote", 1) oled.write_line(1, "conencted", 1) time.sleep(1.5) else: oled.write_line(0, "LEDs", 1) oled.write_line(1, "") for x in range(0, 16): WIIMOTE.led = x time.sleep(0.2) WIIMOTE.led = 1 time.sleep(0.2) oled.write_line(0, "Rumble", 1) for x in range(0, 4): WIIMOTE.rumble = True time.sleep(0.5) WIIMOTE.rumble = False time.sleep(0.5) oled.write_line(0, "Accel & buttons", 1) oled.write_line(1, "(HOME to stop)", 1) time.sleep(1.5) s = WIIMOTE.state while not s.get("buttons", 0) & cwiid.BTN_HOME: s = WIIMOTE.state acc = s.get("acc", [0, 0, 0]) roll, pitch, accel = [x - y for (x, y) in zip(acc, ACC_CAL)] sstr = "{0:3}, {1:3}, {2:3}".format(roll, pitch, accel) oled.write_line(0, sstr) oled.write_line(1, bin(s.get("buttons", 0))[2:].rjust(13, "0")) time.sleep(0.1) oled.clear_display() time.sleep(0.2) return False
def cmd_aupdate(): # update code through git logging.debug("Running cmd_aupdate") oled.write_line(0, "Running git pull", 1) oled.write_line(1, "....", 1) time.sleep(0.5) rcode = subprocess.call("cd /home/pi/robot && git pull --ff-only", shell=True) # --ff-only stops it from merging if rcode == 0: oled.write_line(1, "Success!", 1) time.sleep(1) git_rev = get_git_revision() oled.write_line(0, "Now on revision", 1) oled.write_line(1, git_rev, 1) time.sleep(2) oled.write_lines("Restart for pull", " to take effect ") time.sleep(2) else: oled.write_line(1, "Failed", 1) time.sleep(1) oled.write_line(0, "Try manually", 1) oled.write_line(1, "pulling/merging", 1) time.sleep(2) git_rev = get_git_revision() oled.write_line(0, "Still on revision", 1) oled.write_line(1, git_rev, 1) time.sleep(2) return False
def cmd_wconnect(): logging.debug("Running cmd_wconnect") global WIIMOTE, ACC_CAL if WIIMOTE is None: oled.write_line(0, "Connecting WM", 1) oled.write_line(1, "Press 1+2", 1) try: WIIMOTE = cwiid.Wiimote() WIIMOTE.led = 1 WIIMOTE.rpt_mode = cwiid.RPT_ACC \ | cwiid.RPT_BTN \ | cwiid.RPT_MOTIONPLUS \ | cwiid.RPT_STATUS ACC_CAL = WIIMOTE.get_acc_cal(cwiid.EXT_NONE)[0] oled.write_line(0, "Connected", 1) oled.write_line(1, "") WIIMOTE.rumble = True time.sleep(0.5) WIIMOTE.rumble = False time.sleep(0.5) oled.write_line(0, "Hold the wiimote", 1) oled.write_line(1, "horizontally", 1) time.sleep(1.5) except RuntimeError: oled.write_line(0, "Failed to", 1) oled.write_line(1, "connect", 1) time.sleep(1.5) else: oled.write_lines(" Disconnect Y", " wiimote? N") btns = get_button_int() wmbut = get_wmbut_int() while btns not in [PFD_BTN_3, PFD_BTN_4] \ and wmbut not in [cwiid.BTN_A, cwiid.BTN_B]: btns = get_button_int() wmbut = get_wmbut_int() if btns == PFD_BTN_3 or wmbut == cwiid.BTN_A: # 'Y' WIIMOTE.led = 0 WIIMOTE.close() WIIMOTE = None time.sleep(0.1) oled.write_line(0, "Disconnected", 1) oled.write_line(1, "") time.sleep(1.5) else: # 'N' time.sleep(0.2) return False
def cmd_pass(): oled.write_line(0, "Continuing...", 1) oled.write_line(1, "") time.sleep(1) return False
def oled_write_menu(name, sel, btns="^v><"): oled.write_line(0, name.ljust(14)[:14] + btns[0] + btns[2]) oled.write_line(1, sel.ljust(14)[:14] + btns[1] + btns[3])
logging.basicConfig(filename="/home/pi/main.log", level=logging.DEBUG, format="%(asctime)s: %(levelname)s: %(message)s", datefmt='%Y-%m-%d %H:%M:%S') logging.info("Started") PFD_BTN_1 = 8 PFD_BTN_2 = 4 PFD_BTN_3 = 2 PFD_BTN_4 = 1 ACC_CAL = (128, 128, 128) oled.init() try: pfd.init() piface = True except pfd.NoPiFaceDigitalDetectedError: logging.error("No PiFace detected.") piface = False WIIMOTE = None EXIT_CMD = None def run_cmd(cmd): p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE) output = p.communicate()[0] return output.decode()