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