712 lines
18 KiB
Python
Executable File
712 lines
18 KiB
Python
Executable File
#!/usr/bin/python3
|
|
# -*- coding: utf-8 -*-
|
|
|
|
########################################
|
|
# HXbot main program #
|
|
# EoF 2016 EoF@itphx.ru #
|
|
########################################
|
|
|
|
import HX
|
|
import REflex
|
|
import HXjoystick
|
|
import HXpower
|
|
import HXserver
|
|
import HXcam
|
|
import HXlcd
|
|
import HXdb
|
|
import time
|
|
import smbus
|
|
import sys
|
|
import threading
|
|
import RPi.GPIO as GPIO
|
|
|
|
|
|
# MAIN CLASS
|
|
class HXbotClass():
|
|
|
|
def __init__(self):
|
|
super(HXbotClass, self).__init__()
|
|
|
|
self.i2c_bus = None
|
|
self.hxpower = None
|
|
self.reflex = None
|
|
self.server = None
|
|
|
|
self.joystick = None
|
|
self.hxcam = None
|
|
self.hxlcd = None
|
|
self.hxdb = None
|
|
self.light = None
|
|
self.sound = None
|
|
self.log = None
|
|
|
|
self.status = HX.STATUS_INIT
|
|
|
|
self.hxpower_lock = threading.Lock()
|
|
self.reflex_lock = threading.Lock()
|
|
self.hxlcd_lock = threading.Lock()
|
|
self.hxdb_lock = threading.Lock()
|
|
|
|
def get_ready(self):
|
|
result = HX.OK_RSP
|
|
|
|
# Init I2C bus
|
|
try:
|
|
self.i2c_bus = smbus.SMBus(HX.I2C_BUS_NUMBER)
|
|
except IOError:
|
|
self.stop()
|
|
return result | HX.I2C_BUS_ERR
|
|
|
|
# Init hxpower
|
|
try:
|
|
self.hxpower = HXpower.HXpowerClass(self.i2c_bus, self.hxpower_lock)
|
|
except IOError:
|
|
self.stop()
|
|
return result | HX.HXPOWER_ERR
|
|
|
|
# Init reflex
|
|
try:
|
|
self.reflex = REflex.REflexClass(self)
|
|
except IOError:
|
|
self.stop()
|
|
return result | HX.REFLEX_ERR
|
|
|
|
# Init server
|
|
try:
|
|
self.server = HXserver.HXserverClass(self)
|
|
except OSError:
|
|
self.stop()
|
|
return result | HX.SERVER_ERR
|
|
|
|
# Init joystick
|
|
try:
|
|
self.joystick = HXjoystick.HXjoystickClass()
|
|
except OSError:
|
|
#self.joystick.stop()
|
|
#self.joystick = None
|
|
result |= HX.JOYSTICK_WRN
|
|
|
|
# Init hxcam
|
|
try:
|
|
self.hxcam = HXcam.HXcamClass()
|
|
except OSError:
|
|
self.hxcam.stop()
|
|
self.hxcam = None
|
|
result |= HX.HXCAMERA_WRN
|
|
|
|
# Init hxlcd
|
|
try:
|
|
self.hxlcd = HXlcd.HXlcdClass(self.i2c_bus, self.hxlcd_lock)
|
|
except OSError:
|
|
self.hxlcd.stop()
|
|
self.hxlcd = None
|
|
result |= HX.HXLCD_WRN
|
|
|
|
# Init hxdb
|
|
try:
|
|
self.hxdb = HXdb.HXdbClass(self)
|
|
except Exception:
|
|
#self.hxdb.stop()
|
|
self.hxdb = None
|
|
result |= HX.HXDB_WRN
|
|
|
|
# Init GPIO
|
|
GPIO.setmode(GPIO.BOARD)
|
|
#GPIO.setup(25, GPIO.OUT)
|
|
|
|
# Set status
|
|
self.status = True
|
|
|
|
return result
|
|
|
|
def start(self):
|
|
if self.status != HX.STATUS_READY:
|
|
return HX.BLK_RSP
|
|
|
|
# Start hxpower
|
|
self.hxpower.start()
|
|
|
|
# Start reflex
|
|
self.reflex.start()
|
|
|
|
# Start server
|
|
self.server.start()
|
|
|
|
# Start joystick
|
|
if self.joystick is not None:
|
|
self.joystick.start()
|
|
|
|
# Start camera
|
|
if self.hxcam is not None:
|
|
self.hxcam.start()
|
|
|
|
# Start lcd
|
|
if self.hxlcd is not None:
|
|
self.hxlcd.start()
|
|
|
|
# Start db
|
|
if self.hxdb is not None:
|
|
self.hxdb.start()
|
|
|
|
self.status = HX.STATUS_RUNNING
|
|
|
|
return HX.OK_RSP
|
|
|
|
def stop(self):
|
|
# Disable MV
|
|
self.hxpower.disable_mv()
|
|
|
|
# Disable camera
|
|
if self.hxcam:
|
|
# Stop process
|
|
if self.hxcam.is_running:
|
|
self.hxcam.stop()
|
|
# Clear property
|
|
self.hxcam = None
|
|
|
|
# Stop reflex
|
|
if self.reflex:
|
|
# Stop process
|
|
if self.reflex.is_running:
|
|
self.reflex.stop()
|
|
# Clear property
|
|
self.reflex = None
|
|
|
|
# Stop joystick
|
|
if self.joystick:
|
|
# Stop process
|
|
if self.joystick.is_running:
|
|
self.joystick.stop()
|
|
# Clear property
|
|
self.joystick = None
|
|
|
|
# Stop server
|
|
if self.server:
|
|
# Stop process
|
|
if self.server.is_running:
|
|
self.server.stop()
|
|
# Clear property
|
|
self.server = None
|
|
|
|
# Stop hxpower
|
|
if self.hxpower:
|
|
# Stop process
|
|
if self.hxpower.is_running:
|
|
self.hxpower.stop()
|
|
# Clear property
|
|
self.hxpower = None
|
|
|
|
# Stop hxlcd
|
|
if self.hxlcd:
|
|
# Stop process
|
|
if self.hxlcd.is_running:
|
|
self.hxlcd.stop()
|
|
# Clear property
|
|
self.hxlcd = None
|
|
|
|
# Stop hxdb
|
|
if self.hxdb:
|
|
# Stop process
|
|
if self.hxdb.is_running:
|
|
self.hxdb.stop()
|
|
# Clear property
|
|
self.hxdb = None
|
|
|
|
# Close I2C bus
|
|
if self.i2c_bus:
|
|
self.i2c_bus.close()
|
|
# Clear property
|
|
self.i2c_bus = None
|
|
|
|
# Set status
|
|
self.status = HX.STATUS_STOP
|
|
|
|
GPIO.cleanup()
|
|
|
|
return HX.OK_RSP
|
|
|
|
def get_temp(self):
|
|
try:
|
|
F = open("/sys/class/thermal/thermal_zone0/temp", "r")
|
|
T = float(F.read())
|
|
except IOError:
|
|
return -999
|
|
finally:
|
|
F.close()
|
|
|
|
return T / 1000
|
|
|
|
|
|
# MAIN PROGRAM
|
|
def main():
|
|
# INIT
|
|
print("Hi! I'am HXbot. Let's fun!'")
|
|
print("Type command or \"help\"")
|
|
|
|
# Create HXbot instance
|
|
hxbot = HXbotClass()
|
|
|
|
# Run
|
|
start(hxbot)
|
|
|
|
# MAIN LOOP
|
|
while True:
|
|
# Set command line view
|
|
if hxbot.status == HX.STATUS_INIT:
|
|
command_view = "INIT>"
|
|
elif hxbot.status == HX.STATUS_READY:
|
|
command_view = "READY>"
|
|
elif hxbot.status == HX.STATUS_RUNNING:
|
|
command_view = "RUNNING>"
|
|
elif hxbot.status == HX.STATUS_STOP:
|
|
command_view = "STOP>"
|
|
elif hxbot.status == HX.STATUS_EXIT:
|
|
command_view = "EXIT>"
|
|
|
|
command = input(command_view)
|
|
|
|
# COMMANDS
|
|
# Help
|
|
if command.lower() == "help":
|
|
print_help()
|
|
|
|
# Exit / Quit
|
|
elif command.lower() == "exit" or \
|
|
command.lower() == "quit":
|
|
stop(hxbot)
|
|
hxbot.status = HX.STATUS_EXIT
|
|
print("Exit...")
|
|
sys.exit()
|
|
|
|
# Status
|
|
elif command.lower() == "status":
|
|
print_status(hxbot)
|
|
# REflex control
|
|
elif command.lower() == "reflex":
|
|
reflex_control(hxbot)
|
|
# HXpower control
|
|
elif command.lower() == "hxpower":
|
|
hxpower_control(hxbot)
|
|
# HXcam control
|
|
elif command.lower() == "hxcam":
|
|
hxcam_control(hxbot)
|
|
|
|
# Get ready
|
|
elif command.lower() == "ready":
|
|
ready(hxbot)
|
|
# Start
|
|
elif command.lower() == "start":
|
|
start(hxbot)
|
|
# Stop
|
|
elif command.lower() == "stop":
|
|
stop(hxbot)
|
|
# Restart
|
|
elif command.lower() == "restart":
|
|
restart(hxbot)
|
|
|
|
# Test
|
|
elif command.lower() == "t":
|
|
t1 = time.clock()
|
|
print("t1:", t1)
|
|
time.sleep(5)
|
|
t2 = time.clock()
|
|
print("t2:", t2)
|
|
dt = t2 - t1
|
|
print("dt:", dt)
|
|
|
|
# WAT?
|
|
else:
|
|
print("WRONG COMMAND")
|
|
print("Try again or type \"help\"")
|
|
|
|
# EXIT
|
|
sys.exit()
|
|
|
|
|
|
# FUNCTIONS
|
|
def ready(hxbot):
|
|
if hxbot.status != HX.STATUS_READY:
|
|
print("Get ready...")
|
|
result = hxbot.get_ready()
|
|
print_result(result)
|
|
else:
|
|
print("HXbot already ready")
|
|
|
|
|
|
def start(hxbot):
|
|
if hxbot.status != HX.STATUS_RUNNING:
|
|
if hxbot.status != HX.STATUS_READY:
|
|
ready(hxbot)
|
|
|
|
print("Start...")
|
|
result = hxbot.start()
|
|
print_result(result)
|
|
else:
|
|
print("HXbot already running")
|
|
|
|
|
|
def stop(hxbot):
|
|
if hxbot.status != HX.STATUS_STOP:
|
|
print("Stop...")
|
|
result = hxbot.stop()
|
|
print_result(result)
|
|
else:
|
|
print("HXbot already stopped")
|
|
|
|
|
|
def restart(hxbot):
|
|
stop(hxbot)
|
|
ready(hxbot)
|
|
start(hxbot)
|
|
|
|
|
|
def print_help():
|
|
print("This is HXbot main program. You can type this commands:")
|
|
print("help - this help")
|
|
print("exit |")
|
|
print("quit - stop HXbot and exit")
|
|
print("status - get status")
|
|
print("start - start HXbot")
|
|
print("stop - stop HXbot")
|
|
print("restart - stop HXbot")
|
|
print("reflex - control REflex")
|
|
print("hxpower - control HXpower")
|
|
|
|
|
|
def hxpower_help():
|
|
print("This is HXpower control program. You can type this commands:")
|
|
print("help - this help")
|
|
print("exit - main menu")
|
|
print("bt - switch BT")
|
|
print("ba - switch BA")
|
|
print("mv - enable/disable moving")
|
|
print("us - enable/disable USB")
|
|
print("0-255 - send raw command")
|
|
|
|
|
|
def hxcam_help():
|
|
print("This is HXcam control program. You can type this commands:")
|
|
print("help - this help")
|
|
print("exit - main menu")
|
|
print("start - start HXcam")
|
|
print("stop - stop HXcam")
|
|
print("restart - restart HXcam")
|
|
|
|
|
|
def reflex_help():
|
|
print("This is REflex control program. You can type this commands:")
|
|
print("help - this help")
|
|
print("exit - main menu")
|
|
print("0-255 - send raw command")
|
|
|
|
|
|
def print_result(result):
|
|
# Universal return codes
|
|
if result == HX.OK_RSP:
|
|
print("OK")
|
|
elif result == HX.NO_RSP:
|
|
print("WARNING: No response")
|
|
elif result == HX.ERR_RSP:
|
|
print("ERROR")
|
|
elif result == HX.BLK_RSP:
|
|
print("ERROR: Blocked")
|
|
elif result == HX.CSE_RSP:
|
|
print("ERROR: Wrong control sum")
|
|
elif result == HX.BLK_RSP:
|
|
print("ERROR: ")
|
|
elif result == HX.IOE_RSP:
|
|
print("ERROR: Input/output error")
|
|
elif result == HX.TMO_RSP:
|
|
print("ERROR: Timeout")
|
|
|
|
# HXbot extended return codes
|
|
elif result == HX.I2C_BUS_ERR:
|
|
print("ERROR: I2C bus not ready")
|
|
elif result == HX.HXPOWER_ERR:
|
|
print("ERROR: HXpower not ready")
|
|
elif result == HX.JOYSTICK_WRN:
|
|
print("WARNING: Joystick not found")
|
|
elif result == HX.REFLEX_ERR:
|
|
print("ERROR: REflex not ready")
|
|
elif result == HX.SERVER_ERR:
|
|
print("ERROR: Server not ready")
|
|
elif result == HX.HXCAMERA_WRN:
|
|
print("WARNING: Camera not ready")
|
|
elif result == HX.HXLCD_WRN:
|
|
print("WARNING: LCD module not ready")
|
|
elif result == HX.HXDB_WRN:
|
|
print("WARNING: DB module not ready")
|
|
|
|
# Unknown return code
|
|
else:
|
|
print("WARNING: Unknown return code received")
|
|
|
|
|
|
def print_status(hxbot):
|
|
if (hxbot.status != HX.STATUS_READY and
|
|
hxbot.status != HX.STATUS_RUNNING):
|
|
print("HXbot is not in ready or running state!")
|
|
return
|
|
|
|
# Print voltages
|
|
print("Vcc =", hxbot.hxpower.vcc)
|
|
print("Vin =", hxbot.hxpower.vin)
|
|
print("Vdc =", hxbot.hxpower.vdc)
|
|
print("Vbt =", hxbot.hxpower.vbt)
|
|
print("Vba =", hxbot.hxpower.vba)
|
|
print("Vpf =", hxbot.hxpower.vpf)
|
|
print("Vzu =", hxbot.hxpower.vzu)
|
|
|
|
# Print currents
|
|
print("Abt =", hxbot.hxpower.abt)
|
|
print("Aba =", hxbot.hxpower.aba)
|
|
print("Ain =", hxbot.hxpower.ain)
|
|
print("Ahx =", hxbot.hxpower.ahx)
|
|
|
|
# Print temperature
|
|
print("CPU Temp:", hxbot.get_temp())
|
|
print("REflex Temp:", hxbot.reflex.get_temp())
|
|
#print("HXPower Temp:", )
|
|
|
|
# Print status
|
|
if hxbot.hxpower.vcc_ok:
|
|
print("Vcc - OK")
|
|
else:
|
|
print("Vcc - FAIL")
|
|
if hxbot.hxpower.vin_ok:
|
|
print("Vin - OK")
|
|
else:
|
|
print("Vin - FAIL")
|
|
if hxbot.hxpower.vdc_ok:
|
|
print("Vdc - OK")
|
|
else:
|
|
print("Vdc - FAIL")
|
|
if hxbot.hxpower.vbt_ok:
|
|
print("Vbt - OK")
|
|
else:
|
|
print("Vbt - FAIL")
|
|
if hxbot.hxpower.vba_ok:
|
|
print("Vba - OK")
|
|
else:
|
|
print("Vba - FAIL")
|
|
if hxbot.hxpower.vpf_ok:
|
|
print("Vpf - OK")
|
|
else:
|
|
print("Vpf - FAIL")
|
|
if hxbot.hxpower.vzu_ok:
|
|
print("Vzu - OK")
|
|
else:
|
|
print("Vzu - FAIL")
|
|
|
|
if hxbot.hxpower.abt_ok:
|
|
print("Abt - OK")
|
|
else:
|
|
print("Abt - FAIL")
|
|
if hxbot.hxpower.aba_ok:
|
|
print("Aba - OK")
|
|
else:
|
|
print("Aba - FAIL")
|
|
if hxbot.hxpower.ain_ok:
|
|
print("Ain - OK")
|
|
else:
|
|
print("Ain - FAIL")
|
|
if hxbot.hxpower.ahx_ok:
|
|
print("Ahx - OK")
|
|
else:
|
|
print("Ahx - FAIL")
|
|
|
|
if hxbot.hxpower.bt_enabled:
|
|
print("BT enabled")
|
|
else:
|
|
print("BT disabled")
|
|
if hxbot.hxpower.ba_enabled:
|
|
print("BA enabled")
|
|
else:
|
|
print("BA disabled")
|
|
if hxbot.hxpower.mv_enabled:
|
|
print("mv enabled")
|
|
else:
|
|
print("PF disabled")
|
|
if hxbot.hxpower.us_enabled:
|
|
print("USB power enabled")
|
|
else:
|
|
print("USB power disabled")
|
|
if hxbot.hxpower.bt_blocked:
|
|
print("BT blocked")
|
|
else:
|
|
print("BT unblocked")
|
|
if hxbot.hxpower.ba_blocked:
|
|
print("BA blocked")
|
|
else:
|
|
print("BA unblocked")
|
|
|
|
if hxbot.hxpower.in_plugged:
|
|
print("IN plugged")
|
|
else:
|
|
print("IN unplugged")
|
|
|
|
if hxbot.hxpower.bt_full:
|
|
print("BT charge complete")
|
|
if hxbot.hxpower.ba_full:
|
|
print("BA charge complete")
|
|
|
|
print()
|
|
|
|
|
|
def reflex_control(hxbot):
|
|
if (hxbot.status != HX.STATUS_READY and
|
|
hxbot.status != HX.STATUS_RUNNING):
|
|
print("HXbot is not in ready or running state!")
|
|
return
|
|
|
|
rawCommand = 0x00
|
|
rawResponse = 0x00
|
|
|
|
print("Reflex control program. Type command or \"help\"")
|
|
print("Type \"exit\" to previous menu")
|
|
command_view = "REflex control>"
|
|
|
|
while True:
|
|
command = input(command_view)
|
|
if command.lower() == "exit":
|
|
break
|
|
elif command.lower() == "help":
|
|
reflex_help()
|
|
else:
|
|
# Try to send RAW command to REflex
|
|
try:
|
|
rawCommand = int(command)
|
|
except:
|
|
rawCommand = 0x00
|
|
|
|
if not rawCommand:
|
|
continue
|
|
|
|
with hxbot.reflex_lock:
|
|
hxbot.i2c_bus.write_byte(HX.REFLEX_ADDRESS, rawCommand)
|
|
print("Sent: ", rawCommand)
|
|
time.sleep(HX.REFLEX_RESPONSE_DELAY)
|
|
rawResponse = hxbot.i2c_bus.read_byte(HX.REFLEX_ADDRESS)
|
|
print("Received: ", rawResponse)
|
|
|
|
|
|
def hxpower_control(hxbot):
|
|
if (hxbot.status != HX.STATUS_READY and
|
|
hxbot.status != HX.STATUS_RUNNING):
|
|
print("HXbot is not in ready or running state!")
|
|
return
|
|
|
|
rawCommand = 0x00
|
|
rawResponse = 0x00
|
|
|
|
print("HXpower control program. Type command or \"help\"")
|
|
print("Type \"exit\" to previous menu")
|
|
command_view = "HXpower control>"
|
|
|
|
while True:
|
|
command = input(command_view)
|
|
if command.lower() == "exit":
|
|
break
|
|
elif command.lower() == "help":
|
|
hxpower_help()
|
|
elif command.lower() == "bt":
|
|
if hxbot.hxpower.bt_enabled:
|
|
print("Disable BT...")
|
|
result = hxbot.hxpower.disable_bt()
|
|
else:
|
|
print("Enable BT...")
|
|
result = hxbot.hxpower.enable_bt()
|
|
print_result(result)
|
|
elif command.lower() == "ba":
|
|
if hxbot.hxpower.ba_enabled:
|
|
print("Disable BA...")
|
|
result = hxbot.hxpower.disable_ba()
|
|
else:
|
|
print("Enable BA...")
|
|
result = hxbot.hxpower.enable_ba()
|
|
print_result(result)
|
|
elif command.lower() == "mv":
|
|
if hxbot.hxpower.mv_enabled:
|
|
print("Disable MV...")
|
|
result = hxbot.hxpower.disable_mv()
|
|
else:
|
|
print("Enable MV...")
|
|
result = hxbot.hxpower.enable_mv()
|
|
print_result(result)
|
|
elif command.lower() == "us":
|
|
if hxbot.hxpower.us_enabled:
|
|
print("Disable USB power...")
|
|
result = hxbot.hxpower.disable_us()
|
|
else:
|
|
print("Enable USB power...")
|
|
result = hxbot.hxpower.enable_us()
|
|
print_result(result)
|
|
elif command.lower() == "reset":
|
|
GPIO.output(25, 0)
|
|
time.sleep(5)
|
|
GPIO.output(25, 1)
|
|
else:
|
|
# Try to send RAW command to HXpower
|
|
try:
|
|
rawCommand = int(command)
|
|
except:
|
|
rawCommand = 0x00
|
|
print("Wrong command!")
|
|
|
|
if not rawCommand:
|
|
continue
|
|
|
|
rawCommand = (rawCommand << 8) | (rawCommand ^ HX.XOR_SEQ)
|
|
try:
|
|
with hxbot.hxpower_lock:
|
|
hxbot.i2c_bus.write_word_data(HX.POWER_ADDRESS,
|
|
HX.EXT_COM, rawCommand)
|
|
print("Sent:", rawCommand, "(", rawCommand, ")")
|
|
time.sleep(HX.POWER_RESPONSE_DELAY)
|
|
rawResponse = hxbot.i2c_bus.read_byte(HX.POWER_ADDRESS)
|
|
print("Received:", rawResponse)
|
|
except IOError as E:
|
|
print(E)
|
|
|
|
|
|
def hxcam_control(hxbot):
|
|
if (hxbot.status != HX.STATUS_READY and
|
|
hxbot.status != HX.STATUS_RUNNING):
|
|
print("HXbot is not in ready or running state!")
|
|
return
|
|
|
|
print("HXcam control program. Type command or \"help\"")
|
|
print("Type \"exit\" to previous menu")
|
|
command_view = "HXcam control>"
|
|
|
|
while True:
|
|
command = input(command_view)
|
|
if command.lower() == "exit":
|
|
break
|
|
elif command.lower() == "help":
|
|
hxcam_help()
|
|
elif command.lower() == "start":
|
|
print("Start HXcam")
|
|
if hxbot.hxcam.is_running:
|
|
print("Allready running")
|
|
else:
|
|
hxbot.hxcam.start()
|
|
elif command.lower() == "stop":
|
|
print("Stop HXcam")
|
|
if not hxbot.hxcam.is_running:
|
|
print("Not running")
|
|
else:
|
|
hxbot.hxcam.stop()
|
|
elif command.lower() == "restart":
|
|
print("Restart HXcam")
|
|
if hxbot.hxcam.is_running:
|
|
print("Stop...")
|
|
hxbot.hxcam.stop()
|
|
if not hxbot.hxcam.is_running:
|
|
print("Start...")
|
|
hxbot.hxcam.start()
|
|
|
|
# MAIN PROGRAM
|
|
if __name__ == "__main__":
|
|
main()
|