def __init__(self, pi=None, port=DEF_PORT, debug=False):
""" __init__ """
self._dbg = debug
self._log = get_logger(__class__.__name__, debug)
self._log.debug('pi=%s, port=%s', pi, port)
if type(pi) == pigpio.pi:
self._pi = pi
self._mypi = False
else:
self._pi = pigpio.pi()
self._mypi = True
self._log.debug('mypi = %s', self._mypi)
self._ctrl = OttoPiCtrl(self._pi, debug=self._dbg)
self._ctrl.start()
self._auto = OttoPiAuto(self._ctrl, debug=self._dbg)
self._auto.start()
self._dance = None
self._dance2 = None
time.sleep(1)
self._port = port
try:
super().__init__(('', self._port), ServerHandler)
except Exception as e:
self._log.warning('%s:%s', type(e).__name__, e)
return None
def __init__(self, debug=False):
""" init """
self.dbg = debug
self._log = get_logger(__class__.__name__, debug)
self._log.debug('')
self.pi = pigpio.pi()
self.robot_ctrl = OttoPiCtrl(self.pi, debug=self.dbg)
self.robot_ctrl.start()
self.robot_auto = OttoPiAuto(self.robot_ctrl, debug=self.dbg)
self.robot_auto.start()
self.active = True
def __init__(self, robot_ctrl=None, debug=False):
""" init """
self.dbg = debug
self._log = get_logger(__class__.__name__, self.dbg)
self._log.debug('')
self.cmd_func = {
self.CMD_NULL: self.cmd_null,
self.CMD_ON: self.cmd_on,
self.CMD_OFF: self.cmd_off,
self.CMD_ENABLE: self.cmd_enable,
self.CMD_DISABLE: self.cmd_disable,
self.CMD_END: self.cmd_end
}
self.my_robot_ctrl = False
self.robot_ctrl = robot_ctrl
if self.robot_ctrl is None:
self.my_robot_ctrl = True
self.robot_ctrl = OttoPiCtrl(None, debug=self.dbg)
self.robot_ctrl.start()
self.tof = VL53L0X.VL53L0X()
# self.tof.start_ranging(VL53L0X.VL53L0X_BEST_ACCURACY_MODE)
self.tof.start_ranging(VL53L0X.VL53L0X_BETTER_ACCURACY_MODE)
self.tof_timing = self.tof.get_timing()
self._log.info('tof_timing = %.02f ms', self.tof_timing / 1000)
self.d = 0
self.cmdq = queue.Queue()
self.active = True
self.on = False
self.enable = False
self.prev_stat = self.STAT_NONE
self.stat = self.STAT_NONE
self.prev_rl = ""
self.touch_count = 0
self.ready_count = 0
self.distance = self.D_FAR
super().__init__(daemon=True)
class OttoPiAutoApp:
""" OttoPiAutoApp """
def __init__(self, debug=False):
""" init """
self.dbg = debug
self._log = get_logger(__class__.__name__, debug)
self._log.debug('')
self.pi = pigpio.pi()
self.robot_ctrl = OttoPiCtrl(self.pi, debug=self.dbg)
self.robot_ctrl.start()
self.robot_auto = OttoPiAuto(self.robot_ctrl, debug=self.dbg)
self.robot_auto.start()
self.active = True
def main(self):
""" main """
self._log.debug('')
while self.active:
cmdline = input()
self._log.debug('cmdline = %s', cmdline)
self.robot_auto.send(cmdline)
time.sleep(1)
if not self.robot_auto.is_active():
self.active = False
self._log.debug('done(active=%s)', self.active)
def end(self):
""" end """
self._log.debug('')
self.robot_auto.end()
self._log.info('robot_auto thread: end')
self.robot_ctrl.send(OttoPiCtrl.CMD_STOP)
self.robot_ctrl.end()
self._log.info('robot_ctrl thread: end')
self.pi.stop()
self._log.info('done')
def main(max_sleep_sec, debug):
_log = get_logger(__name__, debug)
_log.info('max_sleep_sec=%d', max_sleep_sec)
try:
pi = pigpio.pi()
robot_ctrl = OttoPiCtrl(pi)
robot_ctrl.start()
obj = Dance(robot_ctrl, max_sleep_sec, debug=debug)
obj.start()
cmdline = input('> ')
print('cmdline=%a' % (cmdline))
obj.end()
while not robot_ctrl.cmdq.empty():
time.sleep(1)
print('.', end='', flush=True)
finally:
_log.info('finally')
robot_ctrl.end()
class OttoPiServer(socketserver.ThreadingTCPServer):
""" server """
DEF_PORT = 12345
CMD_PREFIX = ':' # word command
CMD_PREFIX2 = '.' # interupt off
CMD_AUTO_PREFIX = 'auto_' # auto command
def __init__(self, pi=None, port=DEF_PORT, debug=False):
""" __init__ """
self._dbg = debug
self._log = get_logger(__class__.__name__, debug)
self._log.debug('pi=%s, port=%s', pi, port)
if type(pi) == pigpio.pi:
self._pi = pi
self._mypi = False
else:
self._pi = pigpio.pi()
self._mypi = True
self._log.debug('mypi = %s', self._mypi)
self._ctrl = OttoPiCtrl(self._pi, debug=self._dbg)
self._ctrl.start()
self._auto = OttoPiAuto(self._ctrl, debug=self._dbg)
self._auto.start()
self._dance = None
self._dance2 = None
time.sleep(1)
self._port = port
try:
super().__init__(('', self._port), ServerHandler)
except Exception as e:
self._log.warning('%s:%s', type(e).__name__, e)
return None
def end(self):
""" end """
self._log.debug('')
if self._auto.is_active():
self._auto.end()
self._log.debug('_auto thread: done')
if self._ctrl.is_active():
self._ctrl.end()
self._log.debug('_ctrl thread: done')
if self._mypi:
self._log.debug('clean up pigpio')
self._pi.stop()
self._mypi = False
self._log.debug('done')
def _del_(self):
""" _del_ """
self._log.debug('')
self.end()
def handle(self):
""" handle """
self._log.debug('')
# Telnet Protocol
#
# mode character
# 0xff IAC
# 0xfd D0
# 0x22 LINEMODE
# self.net_write(b'\xff\xfd\x22')
self.net_write('#Ready\r\n'.encode('utf-8'))
net_data = b''
flag_continue = True
while flag_continue:
# データー受信
try:
net_data = self.request.recv(512)
except ConnectionResetError as e:
self._log.warning('%s:%s.', type(e), e)
return
except BaseException as e:
self._log.warning('BaseException:%s:%s.', type(e), e)
self._log.warning('send: OttoPiCtrl.CMD_STOP')
self._ctrl.send(OttoPiCtrl.CMD_STOP)
return
else:
self._log.debug('net_data:%a', net_data)
# デコード(UTF-8)
try:
decoded_data = net_data.decode('utf-8')
except UnicodeDecodeError as e:
self._log.warning('%s:%s .. ignored', type(e), e)
continue
else:
self._log.debug('decoded_data:%a', decoded_data)
self.net_write('\r\n'.encode('utf-8'))
# 文字列抽出(コントロールキャラクター削除)
data = ''
for ch in decoded_data:
if ord(ch) >= 0x20:
data += ch
self._log.debug('data=%a', data)
# 文字数が0の場合、コネクションが切断されたと判断し終了
if len(data) == 0:
msg = 'No data .. disconnect'
self._log.warning(msg)
self.net_write((msg + '\r\n').encode('utf-8'))
break
# 制御スレッドが動いていない場合は(異常終了など?)、再起動
if not self._ctrl.is_active():
self._log.warning('robot control thread is dead !? .. restart')
self._svr._ctrl = OttoPiCtrl(self._svr._pi,
debug=self._svr._dbg)
self._ctrl = self._svr._ctrl
self._ctrl.start()
# 自動運転スレッドが動いていない場合は(異常終了など?)、再起動
if not self._auto.is_active():
self._log.warning('auto control thread is dead !? .. restart')
self._svr._auto = OttoPiAuto(self._ctrl, debug=self._svr._dbg)
self._auto = self._svr._auto
self._auto.start()
# word command
# ex. ":.forward 2", ":happy 1", ":auto_off"
if data[0] == OttoPiServer.CMD_PREFIX:
cmd = data[1:]
interrupt_flag = True
if data[1] == OttoPiServer.CMD_PREFIX2:
cmd = data[2:]
interrupt_flag = False
cmd_name = cmd.split()[0]
self._log.info('cmd=%s, cmd_name=%s, interrupt_flag=%s', cmd,
cmd_name, interrupt_flag)
""" dance """
if cmd_name in ['dance_on', 'dance_true']:
if self._svr._dance2 is not None:
self._svr._dance2.end()
self._svr._dance2 = None
if self._svr._dance is None:
self._svr._dance = Dance(self._svr._ctrl,
max_sleep_sec=3,
debug=True)
self._svr._dance.start()
self.send_reply(data, True, '')
continue
""" dance2 """
if cmd_name in ['dance2_on', 'dance2_true']:
if self._svr._dance is not None:
self._svr._dance.end()
self._svr._dance = None
if self._svr._dance2 is None:
self._svr._dance2 = Dance2(self._svr._ctrl,
max_sleep_sec=3,
debug=True)
self._svr._dance2.start()
self.send_reply(data, True, '')
continue
""" dance and dance2 off """
if cmd_name in [
'dance_off', 'dance_false', 'dance2_off',
'dance2_false'
]:
if self._svr._dance is not None:
self._svr._dance.end()
self._svr._dance = None
if self._svr._dance2 is not None:
self._svr._dance2.end()
self._svr._dance2 = None
self.send_reply(data, True, '')
continue
""" auto """
if cmd.startswith(OttoPiServer.CMD_AUTO_PREFIX):
"""
auto command
注意
----
auto commandは、回数パラメータがないため、
cmd_nameだけをsendする。
"""
auto_cmd = cmd.replace(OttoPiServer.CMD_AUTO_PREFIX, '')
cmd_name = cmd_name.replace(OttoPiServer.CMD_AUTO_PREFIX,
'')
self._log.info('auto_cmd=%s, cmd_name=%s', auto_cmd,
cmd_name)
if cmd_name in self._auto.cmd_func.keys():
d = self._auto.send(cmd_name)
self._log.info('d=%smm', '{:,}'.format(d))
self.send_reply(data, True, {'d': d})
else:
self._log.warning('%s: invalid auto command', auto_cmd)
self.send_reply(data, False, 'invalid auto command')
continue
""" control command """
if cmd_name in self._ctrl.cmd_func.keys():
self._ctrl.send(cmd, interrupt_flag)
self.send_reply(data, True, '')
else:
msg = 'invalid control command'
self._log.warning('%s: %s', cmd, msg)
self.send_reply(data, False, msg)
continue
# one-key command
for ch in data:
self._log.info('ch=%a', ch)
if ch not in self.cmd_key.keys():
self._ctrl.send(OttoPiCtrl.CMD_STOP)
self._log.warning('invalid 1-key command:%a .. stop', ch)
self.send_reply(ch, False, 'invalid 1-key command')
continue
cmd = self.cmd_key[ch]
self._log.info('cmd=%s', cmd)
if cmd.startswith(OttoPiServer.CMD_AUTO_PREFIX):
# auto command
auto_cmd = cmd.replace(OttoPiServer.CMD_AUTO_PREFIX, '')
self._log.info('auto_cmd=%s', auto_cmd)
self._auto.send(auto_cmd)
self.send_reply('%s(%s)' % (ch, cmd), True, '')
else:
# control command
self._ctrl.send(cmd)
self.send_reply('%s(%s)' % (ch, cmd), True, '')
self._log.debug('done')
class OttoPiAuto(threading.Thread):
""" auto pilot """
CMD_NULL = 'null'
CMD_ON = 'on'
CMD_OFF = 'off'
CMD_ENABLE = 'enable'
CMD_DISABLE = 'disable'
CMD_READY = 'ready'
CMD_END = 'end'
DEF_RECV_TIMEOUT = 0.2 # sec
D_TOUCH = 40
D_TOO_NEAR = 180
D_NEAR = 250
D_FAR = 8000
D_READY_MIN = D_TOUCH + 10
D_READY_MAX = 120
STAT_NONE = 'none'
STAT_YELLOW = 'yellow'
STAT_TOO_NEAR = 'too_near'
STAT_NEAR = 'near'
STAT_FAR = 'far'
STAT_READY = 'ready'
TOUCH_COUNT_COMMIT = 3
READY_COUNT_COMMIT = 2
def __init__(self, robot_ctrl=None, debug=False):
""" init """
self.dbg = debug
self._log = get_logger(__class__.__name__, self.dbg)
self._log.debug('')
self.cmd_func = {
self.CMD_NULL: self.cmd_null,
self.CMD_ON: self.cmd_on,
self.CMD_OFF: self.cmd_off,
self.CMD_ENABLE: self.cmd_enable,
self.CMD_DISABLE: self.cmd_disable,
self.CMD_END: self.cmd_end
}
self.my_robot_ctrl = False
self.robot_ctrl = robot_ctrl
if self.robot_ctrl is None:
self.my_robot_ctrl = True
self.robot_ctrl = OttoPiCtrl(None, debug=self.dbg)
self.robot_ctrl.start()
self.tof = VL53L0X.VL53L0X()
# self.tof.start_ranging(VL53L0X.VL53L0X_BEST_ACCURACY_MODE)
self.tof.start_ranging(VL53L0X.VL53L0X_BETTER_ACCURACY_MODE)
self.tof_timing = self.tof.get_timing()
self._log.info('tof_timing = %.02f ms', self.tof_timing / 1000)
self.d = 0
self.cmdq = queue.Queue()
self.active = True
self.on = False
self.enable = False
self.prev_stat = self.STAT_NONE
self.stat = self.STAT_NONE
self.prev_rl = ""
self.touch_count = 0
self.ready_count = 0
self.distance = self.D_FAR
super().__init__(daemon=True)
def __del__(self):
""" del """
self._log.debug('')
def end(self):
""" end """
self._log.debug('')
self.active = False
self.robot_ctrl.send(OttoPiCtrl.CMD_STOP)
if self.my_robot_ctrl:
self.robot_ctrl.end()
self.join()
self.tof.stop_ranging()
self._log.debug('done')
def cmd_null(self):
""" do nothing (to get distance) """
self._log.debug('')
def cmd_on(self):
""" cmd on """
self._log.debug('')
if not self.enable:
self._log.warning('enable=%s .. ignored', self.enable)
return
self.robot_ctrl.send('forward')
self.on = True
self.touch_count = 0
self.stat = self.STAT_NONE
def cmd_off(self):
""" cmd off """
self._log.debug('')
self.robot_ctrl.send('stop')
self.on = False
self.ready_count = 0
self.stat = self.STAT_NONE
def cmd_enable(self):
""" cmd enable """
self._log.debug('')
self.enable = True
self._log.debug('enable=%s', self.enable)
self.ready_count = 0
self.stat = self.STAT_NONE
def cmd_disable(self):
""" cmd disable """
self._log.debug('')
self.cmd_off()
self.enable = False
self._log.debug('enable=%s', self.enable)
self.ready_count = 0
self.stat = self.STAT_NONE
def cmd_end(self):
""" cmd end """
self._log.debug('')
self.cmd_off()
self.active = False
def is_active(self):
""" """
self._log.debug('active=%s', self.active)
return self.active
def send(self, cmd):
""" """
self._log.debug('cmd=\'%s\'', cmd)
self.cmdq.put(cmd)
d = self.get_distance()
self._log.debug('d=%smm', '{:,}'.format(d))
return d
def recv(self, timeout=DEF_RECV_TIMEOUT):
""" """
self._log.debug('timeout=%.1f', timeout)
try:
cmd = self.cmdq.get(timeout=timeout)
except queue.Empty:
cmd = ''
else:
self._log.debug('cmd=\'%s\'', cmd)
return cmd
def get_distance(self):
""" """
self.distance = self.tof.get_distance()
if self.distance == 0:
# ???
self.distance = self.D_FAR
self._log.warning('Crrection: distance = %smm',
'{:,}'.format(self.distance))
return self.distance
def run(self):
""" run """
self._log.debug('')
while self.active:
cmd = self.recv()
if cmd != '':
self._log.debug('cmd=%a', cmd)
cmdline = cmd.split()
cmd_name = cmdline[0]
self._log.info('cmd_name=%a', cmd_name)
if cmd_name in self.cmd_func.keys():
self.cmd_func[cmd_name]()
else:
self._log.error('%s: invalid command .. ignore', cmd)
d = self.get_distance()
# self._log.debug('d = %smm', '{:,}'.format(d))
if d < 0:
continue
if not self.enable:
continue
if not self.on:
if self.ready_count > 0:
self._log.info('ready_count=%d/%d', self.ready_count,
self.READY_COUNT_COMMIT)
if self.ready_count < self.READY_COUNT_COMMIT:
if d >= self.D_READY_MIN and d <= self.D_READY_MAX:
self._log.warn('%dmm <= %dmm <= %dmm',
self.D_READY_MIN, d, self.D_READY_MAX)
self.ready_count += 1
self.robot_ctrl.send('happy')
time.sleep(1)
else:
self.ready_count = 0
else: # self.ready_count >= self.READY_COUNT_COMMIT
self.cmd_on()
continue
self.prev_stat = self.stat
if d <= self.D_TOUCH:
self._log.warn('touched(%dmm <= %dmm)', d, self.D_TOUCH)
self.robot_ctrl.send('suprised')
time.sleep(1)
if self.touch_count < self.TOUCH_COUNT_COMMIT:
self.touch_count += 1
self._log.info('touch_count=%d', self.touch_count)
if self.touch_count >= self.TOUCH_COUNT_COMMIT:
self._log.warn('STOP!')
self.cmd_off()
# self.robot_ctrl.send('suprised')
time.sleep(3)
else:
self.robot_ctrl.send('backward')
continue
else:
self.touch_count = 0
if d <= self.D_TOO_NEAR:
self._log.warn('TOO_NEAR(%dmm <= %dmm)', d, self.D_TOO_NEAR)
self.stat = self.STAT_NEAR
if self.prev_stat != self.STAT_NEAR:
self.robot_ctrl.send('suprised')
time.sleep(1)
else:
self.robot_ctrl.send('backward')
time.sleep(2)
elif d <= self.D_NEAR:
self._log.warn('NEAR(%dmm <= %dmm)', d, self.D_NEAR)
self.stat = self.STAT_NEAR
if self.prev_stat != self.STAT_NEAR:
if random.random() < 0.5:
self.prev_rl = "right"
self.robot_ctrl.send('slide_right')
else:
self.prev_rl = "left"
self.robot_ctrl.send('slide_left')
else:
if self.prev_rl == "right":
self.robot_ctrl.send('turn_right')
else:
self.robot_ctrl.send('turn_left')
time.sleep(1)
time.sleep(1.5)
elif d >= self.D_FAR:
self._log.info('FAR(%dmm >= %dmm)', d, self.D_FAR)
self.stat = self.STAT_FAR
if self.prev_stat in [self.STAT_NEAR, self.STAT_YELLOW]:
self.robot_ctrl.send('forward')
else:
if self.prev_stat == self.STAT_NEAR:
self.stat = self.STAT_NONE
if d <= self.D_NEAR + 50:
self.stat = self.STAT_YELLOW
self._log.info('stat: %s', self.stat)
self.robot_ctrl.send('suriashi_fwd')
else:
self._log.info('stat: %s', self.stat)
self.robot_ctrl.send('forward')
self.touch_count = 0
self._log.debug('stat=%s', self.stat)
self._log.info('done(active=%s)', self.active)