Verison 1.0.3 Release

Based on Buildbotics 0.4.14

src/py/bbctrl/APIHandler.py

@@ -0,0 +1,88 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import json
+import traceback
+import bbctrl
+
+from tornado.web import HTTPError
+import tornado.httpclient
+
+
+class APIHandler(bbctrl.RequestHandler):
+    def delete(self, *args, **kwargs):
+        self.delete_ok(*args, **kwargs)
+        self.write_json('ok')
+
+
+    def delete_ok(self): raise HTTPError(405)
+
+
+    def put(self, *args, **kwargs):
+        self.put_ok(*args, **kwargs)
+        self.write_json('ok')
+
+
+    def put_ok(self): raise HTTPError(405)
+
+
+    def prepare(self):
+        self.json = {}
+
+        if self.request.body:
+            try:
+                self.json = tornado.escape.json_decode(self.request.body)
+            except ValueError:
+                raise HTTPError(400, 'Unable to parse JSON')
+
+
+    def set_default_headers(self):
+        self.set_header('Content-Type', 'application/json')
+
+
+    def write_error(self, status_code, **kwargs):
+        e = {}
+
+        if 'message' in kwargs: e['message'] = kwargs['message']
+
+        elif 'exc_info' in kwargs:
+            typ, value, tb = kwargs['exc_info']
+            if isinstance(value, HTTPError) and value.log_message:
+                e['message'] = value.log_message % value.args
+            else: e['message'] = str(kwargs['exc_info'][1])
+
+        else: e['message'] = 'Unknown error'
+
+        e['code'] = status_code
+
+        self.write_json(e)
+
+
+    def write_json(self, data, pretty = False):
+        if pretty: data = json.dumps(data, indent = 2, separators = (',', ': '))
+        else: data = json.dumps(data, separators = (',', ':'))
+        self.write(data)

src/py/bbctrl/AVR.py

@@ -0,0 +1,162 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import serial
+import time
+import traceback
+import ctypes
+
+import bbctrl
+import bbctrl.Cmd as Cmd
+
+
+class serial_struct(ctypes.Structure):
+    _fields_ = [
+        ('type',            ctypes.c_int),
+        ('line',            ctypes.c_int),
+        ('port',            ctypes.c_uint),
+        ('irq',             ctypes.c_int),
+        ('flags',           ctypes.c_int),
+        ('xmit_fifo_size',  ctypes.c_int),
+        ('custom_divisor',  ctypes.c_int),
+        ('baud_base',       ctypes.c_int),
+        ('close_delay',     ctypes.c_ushort),
+        ('io_type',         ctypes.c_byte),
+        ('reserved',        ctypes.c_byte),
+        ('hub6',            ctypes.c_int),
+        ('closing_wait',    ctypes.c_ushort),
+        ('closing_wait2',   ctypes.c_ushort),
+        ('iomem_base',      ctypes.c_char_p),
+        ('iomem_reg_shift', ctypes.c_ushort),
+        ('port_high',       ctypes.c_uint),
+        ('iomap_base',      ctypes.c_ulong),
+    ]
+
+
+def serial_set_low_latency(sp):
+    import fcntl
+    import termios
+
+    ASYNCB_LOW_LATENCY = 13
+
+    ss = serial_struct()
+    fcntl.ioctl(sp, termios.TIOCGSERIAL, ss)
+    ss.flags |= 1 << ASYNCB_LOW_LATENCY # pylint: disable=no-member
+    fcntl.ioctl(sp, termios.TIOCSSERIAL, ss)
+
+
+class AVR(object):
+    def __init__(self, ctrl):
+        self.ctrl = ctrl
+        self.log = ctrl.log.get('AVR')
+
+        self.sp = None
+        self.i2c_addr = ctrl.args.avr_addr
+        self.read_cb = None
+        self.write_cb = None
+
+
+    def close(self): pass
+
+
+    def _start(self):
+        try:
+            self.sp = serial.Serial(self.ctrl.args.serial, self.ctrl.args.baud,
+                                    rtscts = 1, timeout = 0, write_timeout = 0)
+            self.sp.nonblocking()
+            #serial_set_low_latency(self.sp)
+
+        except Exception as e:
+            self.sp = None
+            self.log.warning('Failed to open serial port: %s', e)
+
+        if self.sp is not None:
+            self.ctrl.ioloop.add_handler(self.sp, self._serial_handler,
+                                         self.ctrl.ioloop.READ)
+
+
+    def set_handlers(self, read_cb, write_cb):
+        if self.read_cb is not None or self.write_cb is not None:
+            raise Exception('Handler already set')
+
+        self.read_cb = read_cb
+        self.write_cb = write_cb
+        self._start()
+
+
+    def enable_write(self, enable):
+        if self.sp is None: return
+
+        flags = self.ctrl.ioloop.READ
+        if enable: flags |= self.ctrl.ioloop.WRITE
+        self.ctrl.ioloop.update_handler(self.sp, flags)
+
+
+    def _serial_write(self):
+        self.write_cb(lambda data: self.sp.write(data))
+
+
+    def _serial_read(self):
+        try:
+            data = ''
+            data = self.sp.read(self.sp.in_waiting)
+            self.read_cb(data)
+
+        except Exception as e:
+            self.log.warning('%s: %s', e, data)
+
+
+    def _serial_handler(self, fd, events):
+        try:
+            if self.ctrl.ioloop.READ & events: self._serial_read()
+            if self.ctrl.ioloop.WRITE & events: self._serial_write()
+
+        except Exception as e:
+            self.log.warning('Serial handler error: %s', traceback.format_exc())
+
+
+    def i2c_command(self, cmd, byte = None, word = None, block = None):
+        self.log.info('I2C: %s b=%s w=%s d=%s' % (cmd, byte, word, block))
+        retry = 10
+        cmd = ord(cmd[0])
+
+        while True:
+            try:
+                self.ctrl.i2c.write(self.i2c_addr, cmd, byte, word, block)
+                break
+
+            except Exception as e:
+                retry -= 1
+
+                if retry:
+                    self.log.warning('I2C failed, retrying: %s' % e)
+                    time.sleep(0.25)
+                    continue
+
+                else:
+                    self.log.error('I2C failed: %s' % e)
+                    raise

src/py/bbctrl/AVREmu.py

@@ -0,0 +1,207 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import sys
+import traceback
+import signal
+
+import bbctrl
+import bbctrl.Cmd as Cmd
+
+
+class AVREmu(object):
+    def __init__(self, ctrl):
+        self.ctrl = ctrl
+        self.log = ctrl.log.get('AVREmu')
+
+        self.avrOut   = None
+        self.avrIn    = None
+        self.i2cOut   = None
+        self.read_cb  = None
+        self.write_cb = None
+        self.pid      = None
+
+
+    def close(self):
+        # Close pipes
+        def _close(fd, withHandle):
+            if fd is None: return
+            try:
+                if withHandle: self.ctrl.ioloop.remove_handler(fd)
+            except: pass
+            try:
+                os.close(fd)
+            except: pass
+
+        _close(self.avrOut, True)
+        _close(self.avrIn,  True)
+        _close(self.i2cOut, False)
+
+        self.avrOut, self.avrIn, self.i2cOut = None, None, None
+
+        # Kill process and wait for it
+        if self.pid is not None:
+            os.kill(self.pid, signal.SIGKILL)
+            os.waitpid(self.pid, 0)
+            self.pid = None
+
+
+    def _start(self):
+        try:
+            self.close()
+
+            # Create pipes
+            stdinFDs  = os.pipe()
+            stdoutFDs = os.pipe()
+            i2cFDs    = os.pipe()
+
+            self.pid = os.fork()
+
+            if not self.pid:
+                # Dup child ends
+                os.dup2(stdinFDs[0],  0)
+                os.dup2(stdoutFDs[1], 1)
+                os.dup2(i2cFDs[0],    3)
+
+                # Close orig fds
+                os.close(stdinFDs[0])
+                os.close(stdoutFDs[1])
+                os.close(i2cFDs[0])
+
+                # Close parent ends
+                os.close(stdinFDs[1])
+                os.close(stdoutFDs[0])
+                os.close(i2cFDs[1])
+
+                cmd = ['bbemu']
+                if self.ctrl.args.fast_emu: cmd.append('--fast')
+
+                os.execvp(cmd[0], cmd)
+                os._exit(1) # In case of failure
+
+            # Parent, close child ends
+            os.close(stdinFDs[0])
+            os.close(stdoutFDs[1])
+            os.close(i2cFDs[0])
+
+            # Non-blocking IO
+            os.set_blocking(stdinFDs[1],  False)
+            os.set_blocking(stdoutFDs[0], False)
+            os.set_blocking(i2cFDs[1],    False)
+
+            self.avrOut = stdinFDs[1]
+            self.avrIn  = stdoutFDs[0]
+            self.i2cOut = i2cFDs[1]
+
+            ioloop = self.ctrl.ioloop
+            ioloop.add_handler(self.avrOut, self._avr_write_handler,
+                               ioloop.WRITE | ioloop.ERROR)
+            ioloop.add_handler(self.avrIn, self._avr_read_handler,
+                               ioloop.READ | ioloop.ERROR)
+
+            self.write_enabled = True
+
+        except Exception:
+            self.close()
+            self.log.exception('Failed to start bbemu')
+
+
+    def set_handlers(self, read_cb, write_cb):
+        if self.read_cb is not None or self.write_cb is not None:
+            raise Exception('AVR handler already set')
+
+        self.read_cb = read_cb
+        self.write_cb = write_cb
+        self._start()
+
+
+    def enable_write(self, enable):
+        if self.avrOut is None: return
+
+        flags = self.ctrl.ioloop.WRITE if enable else 0
+        self.ctrl.ioloop.update_handler(self.avrOut, flags)
+        self.write_enabled = enable
+
+
+    def _avr_write(self, data):
+        try:
+            length = os.write(self.avrOut, data)
+            self.continue_write = length and length == len(data)
+            return length
+
+        except BlockingIOError: pass
+        except BrokenPipeError: pass
+
+        return 0
+
+
+    def _avr_write_handler(self, fd, events):
+        if self.avrOut is None: return
+
+        if events & self.ctrl.ioloop.ERROR:
+            self._start()
+            return
+
+        try:
+            while True:
+                self.continue_write = False
+                self.write_cb(self._avr_write)
+                if not self.continue_write: break
+
+        except Exception as e:
+            self.log.warning('AVR write handler error: %s',
+                             traceback.format_exc())
+
+
+    def _avr_read_handler(self, fd, events):
+        if self.avrIn is None: return
+
+        if events & self.ctrl.ioloop.ERROR:
+            self._start()
+            return
+
+        try:
+            data = os.read(self.avrIn, 4096)
+            if data is not None: self.read_cb(data)
+
+        except Exception as e:
+            self.log.warning('AVR read handler error: %s %s' %
+                        (data, traceback.format_exc()))
+
+
+    def i2c_command(self, cmd, byte = None, word = None, block = None):
+        if byte is not None: data = chr(byte)
+        elif word is not None: data = word
+        elif block is not None: data = block
+        else: data = ''
+
+        try:
+            if self.i2cOut is not None:
+                os.write(self.i2cOut, bytes(cmd + data + '\n', 'utf-8'))
+
+        except BrokenPipeError: pass

src/py/bbctrl/Camera.py

@@ -0,0 +1,510 @@
+#!/usr/bin/env python3
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/icenses/>.                         #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import fcntl
+import select
+import struct
+import mmap
+import pyudev
+import base64
+import socket
+import ctypes
+from tornado import gen, web, iostream
+import bbctrl
+
+try:
+    import v4l2
+except:
+    import bbctrl.v4l2 as v4l2
+
+
+def array_to_string(a):
+    def until_zero(a):
+        for c in a:
+            if c == 0: return
+            yield c
+
+    return ''.join([chr(i) for i in until_zero(a)])
+
+
+def fourcc_to_string(i):
+    return \
+        chr((i >>  0) & 0xff) + \
+        chr((i >>  8) & 0xff) + \
+        chr((i >> 16) & 0xff) + \
+        chr((i >> 24) & 0xff)
+
+
+def string_to_fourcc(s): return v4l2.v4l2_fourcc(s[0], s[1], s[2], s[3])
+
+
+def format_frame(frame):
+    frame = [b'--', VideoHandler.boundary.encode('utf8'), b'\r\n',
+             b'Content-type: image/jpeg\r\n',
+             b'Content-length: %d\r\n\r\n' % len(frame), frame]
+    return b''.join(frame)
+
+
+def get_image_resource(path):
+    path = bbctrl.get_resource(path)
+
+    with open(path, 'rb') as f:
+        return format_frame(f.read())
+
+
+class VideoDevice(object):
+    def __init__(self, path = '/dev/video0'):
+        self.fd = os.open(path, os.O_RDWR | os.O_NONBLOCK | os.O_CLOEXEC)
+        self.buffers = []
+
+
+    def fileno(self): return self.fd
+
+
+    def get_audio(self):
+        b = v4l2.v4l2_audio()
+        b.index = 0
+
+        l = []
+
+        while True:
+            try:
+                fcntl.ioctl(self, v4l2.VIDIOC_ENUMAUDIO, b)
+                l.append((array_to_string(b.name), b.capability, b.mode))
+                b.index += 1
+
+            except OSError: break
+
+        return l
+
+
+    def get_formats(self):
+        b = v4l2.v4l2_fmtdesc()
+        b.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
+        b.index = 0
+
+        l = []
+
+        while True:
+            try:
+                fcntl.ioctl(self, v4l2.VIDIOC_ENUM_FMT, b)
+
+                l.append((fourcc_to_string(b.pixelformat),
+                          array_to_string(b.description)))
+
+                b.index += 1
+
+            except OSError: break
+
+        return l
+
+
+    def get_frame_sizes(self, fourcc):
+        b = v4l2.v4l2_frmsizeenum()
+        b.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
+        b.pixel_format = fourcc
+
+        sizes = []
+
+        while True:
+            try:
+                fcntl.ioctl(self, v4l2.VIDIOC_ENUM_FRAMESIZES, b)
+
+                if b.type == v4l2.V4L2_FRMSIZE_TYPE_DISCRETE:
+                    sizes.append((b.discrete.width, b.discrete.height))
+
+                else:
+                    sizes.append((b.stepwise.min_width, b.stepwise.max_width,
+                                  b.stepwise.step_width, b.stepwise.min_height,
+                                  b.stepwise.max_height,
+                                  b.stepwise.step_height))
+
+                b.index += 1 # pylint: disable=no-member
+
+            except OSError: break
+
+        return sizes
+
+
+    def set_format(self, width, height, fourcc):
+        fmt = v4l2.v4l2_format()
+        fmt.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
+        fcntl.ioctl(self, v4l2.VIDIOC_G_FMT, fmt)
+
+        fmt.fmt.pix.width = width
+        fmt.fmt.pix.height = height
+        fmt.fmt.pix.pixelformat = fourcc
+
+        fcntl.ioctl(self, v4l2.VIDIOC_S_FMT, fmt)
+
+
+    def create_buffers(self, count):
+        # Create buffers
+        rbuf = v4l2.v4l2_requestbuffers()
+        rbuf.count = count;
+        rbuf.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        rbuf.memory = v4l2.V4L2_MEMORY_MMAP;
+
+        fcntl.ioctl(self, v4l2.VIDIOC_REQBUFS, rbuf)
+
+        for i in range(rbuf.count):
+            # Get buffer
+            buf = v4l2.v4l2_buffer()
+            buf.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
+            buf.memory = v4l2.V4L2_MEMORY_MMAP
+            buf.index = i
+            fcntl.ioctl(self, v4l2.VIDIOC_QUERYBUF, buf)
+
+            # Mem map buffer
+            mm = mmap.mmap(self.fileno(), buf.length, mmap.MAP_SHARED,
+                           mmap.PROT_READ | mmap.PROT_WRITE,
+                           offset = buf.m.offset)
+            self.buffers.append(mm)
+
+            # Queue the buffer for capture
+            fcntl.ioctl(self, v4l2.VIDIOC_QBUF, buf)
+
+
+    def _dqbuf(self):
+        buf = v4l2.v4l2_buffer()
+        buf.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
+        buf.memory = v4l2.V4L2_MEMORY_MMAP
+        fcntl.ioctl(self, v4l2.VIDIOC_DQBUF, buf)
+
+        return buf
+
+
+    def _qbuf(self, buf):
+        fcntl.ioctl(self, v4l2.VIDIOC_QBUF, buf)
+
+
+    def read_frame(self):
+        buf = self._dqbuf()
+        mm = self.buffers[buf.index]
+
+        frame = mm.read(buf.bytesused)
+        mm.seek(0)
+        self._qbuf(buf)
+
+        return frame
+
+
+    def flush_frame(self): self._qbuf(self._dqbuf())
+
+
+    def get_info(self):
+        caps = v4l2.v4l2_capability()
+        fcntl.ioctl(self, v4l2.VIDIOC_QUERYCAP, caps)
+
+        caps._driver   = array_to_string(caps.driver)
+        caps._card     = array_to_string(caps.card)
+        caps._bus_info = array_to_string(caps.bus_info)
+
+        l = []
+        c = caps.capabilities
+        if c & v4l2.V4L2_CAP_VIDEO_CAPTURE: l.append('video_capture')
+        if c & v4l2.V4L2_CAP_VIDEO_OUTPUT: l.append('video_output')
+        if c & v4l2.V4L2_CAP_VIDEO_OVERLAY: l.append('video_overlay')
+        if c & v4l2.V4L2_CAP_VBI_CAPTURE: l.append('vbi_capture')
+        if c & v4l2.V4L2_CAP_VBI_OUTPUT: l.append('vbi_output')
+        if c & v4l2.V4L2_CAP_SLICED_VBI_CAPTURE: l.append('sliced_vbi_capture')
+        if c & v4l2.V4L2_CAP_SLICED_VBI_OUTPUT: l.append('sliced_vbi_output')
+        if c & v4l2.V4L2_CAP_RDS_CAPTURE: l.append('rds_capture')
+        if c & v4l2.V4L2_CAP_VIDEO_OUTPUT_OVERLAY:
+            l.append('video_output_overlay')
+        if c & v4l2.V4L2_CAP_HW_FREQ_SEEK: l.append('hw_freq_seek')
+        if c & v4l2.V4L2_CAP_RDS_OUTPUT: l.append('rds_output')
+        if c & v4l2.V4L2_CAP_TUNER: l.append('tuner')
+        if c & v4l2.V4L2_CAP_AUDIO: l.append('audio')
+        if c & v4l2.V4L2_CAP_RADIO: l.append('radio')
+        if c & v4l2.V4L2_CAP_MODULATOR: l.append('modulator')
+        if c & v4l2.V4L2_CAP_READWRITE: l.append('readwrite')
+        if c & v4l2.V4L2_CAP_ASYNCIO: l.append('asyncio')
+        if c & v4l2.V4L2_CAP_STREAMING: l.append('streaming')
+        caps._caps = l
+
+        return caps
+
+
+    def set_fps(self, fps):
+        setfps = v4l2.v4l2_streamparm()
+        setfps.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        setfps.parm.capture.timeperframe.numerator = 1
+        setfps.parm.capture.timeperframe.denominator = fps
+        fcntl.ioctl(self, v4l2.VIDIOC_S_PARM, setfps)
+
+
+    def start(self):
+        buf_type = v4l2.v4l2_buf_type(v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE)
+        fcntl.ioctl(self, v4l2.VIDIOC_STREAMON, buf_type)
+
+
+    def stop(self):
+        buf_type = v4l2.v4l2_buf_type(v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE)
+        fcntl.ioctl(self, v4l2.VIDIOC_STREAMOFF, buf_type)
+
+
+    def close(self):
+        if self.fd is None: return
+        try:
+            os.close(self.fd)
+        finally: self.fd = None
+
+
+class Camera(object):
+    def __init__(self, ioloop, args, log):
+        self.ioloop = ioloop
+        self.log = log.get('Camera')
+
+        self.width = args.width
+        self.height = args.height
+        self.fps = args.fps
+        self.fourcc = 'MJPG'
+        self.max_clients = args.camera_clients
+
+        self.overtemp = False
+        self.dev = None
+        self.clients = []
+        self.path = None
+        self.have_camera = False
+
+        # Find connected cameras
+        for i in range(4):
+            path = '/dev/video%d' % i
+            if os.path.exists(path):
+                self.have_camera = True
+                self.open(path)
+                break
+
+        # Get notifications of camera (un)plug events
+        self.udevCtx = pyudev.Context()
+        self.udevMon = pyudev.Monitor.from_netlink(self.udevCtx)
+        self.udevMon.filter_by(subsystem = 'video4linux')
+        ioloop.add_handler(self.udevMon, self._udev_handler, ioloop.READ)
+        self.udevMon.start()
+
+
+    def _udev_handler(self, fd, events):
+        action, device = self.udevMon.receive_device()
+        if device is None or self.dev is not None: return
+
+        path = str(device.device_node)
+
+        if action == 'add':
+            self.have_camera = True
+            self.open(path)
+
+        if action == 'remove' and path == self.path:
+            self.have_camera = False
+            self.close()
+
+
+    def _send_frame(self, frame):
+        if not len(self.clients): return
+
+        try:
+            frame = format_frame(frame)
+            for i in range(self.max_clients):
+                if i < len(self.clients):
+                    self.clients[-(i + 1)].write_frame(frame)
+
+        except Exception as e:
+            self.log.warning('Failed to write frame to client: %s' % e)
+
+
+    def _fd_handler(self, fd, events):
+        try:
+            if len(self.clients):
+                frame = self.dev.read_frame()
+                self._send_frame(frame)
+
+            else: self.dev.flush_frame()
+
+        except Exception as e:
+            if isinstance(e, BlockingIOError): return
+
+            self.log.warning('Failed to read from camera.')
+            self.ioloop.remove_handler(fd)
+            self.close()
+
+
+    def _update_client_image(self):
+        if self.have_camera and not self.overtemp: return
+        if self.overtemp and self.have_camera: img = 'overtemp'
+        else: img = 'offline'
+
+        if len(self.clients): self.clients[-1].write_img(img)
+
+
+    def open(self, path):
+        try:
+            self._update_client_image()
+            self.path = path
+            if self.overtemp: return
+            self.dev = VideoDevice(path)
+
+            caps = self.dev.get_info()
+            self.log.info('%s, %s, %s, %s', caps._driver, caps._card,
+                          caps._bus_info, caps._caps)
+
+            if caps.capabilities & v4l2.V4L2_CAP_VIDEO_CAPTURE == 0:
+                raise Exception('Video capture not supported.')
+
+            fourcc  = string_to_fourcc(self.fourcc)
+            formats = self.dev.get_formats()
+            sizes   = self.dev.get_frame_sizes(fourcc)
+
+            self.log.info('Formats: %s', formats)
+            self.log.info('Sizes: %s', sizes)
+            self.log.info('Audio: %s', self.dev.get_audio())
+
+            hasFormat = False
+            for name, description in formats:
+                if name == self.fourcc: hasFormat = True
+
+            if not hasFormat:
+                raise Exception(self.fourcc + ' video format not supported.')
+
+            self.dev.set_format(self.width, self.height, fourcc = fourcc)
+            self.dev.set_fps(self.fps)
+            self.dev.create_buffers(4)
+            self.dev.start()
+
+            self.ioloop.add_handler(self.dev, self._fd_handler,
+                                    self.ioloop.READ)
+
+            self.log.info('Opened camera ' + path)
+
+
+        except Exception as e:
+            self.log.warning('While loading camera: %s' % e)
+            self._close_dev()
+
+
+    def _close_dev(self):
+        if self.dev is None: return
+        try:
+            self.dev.close()
+        except Exception as e: self.log.warning('While closing camera: %s', e)
+
+        self.dev = None
+
+
+    def close(self, overtemp = False):
+        self._update_client_image()
+        if self.dev is None: return
+
+        try:
+            self.ioloop.remove_handler(self.dev)
+            try:
+                self.dev.stop()
+            except: pass
+
+            self._close_dev()
+            self.log.info('Closed camera')
+
+        except: self.log.exception('Exception while closing camera')
+        finally: self.dev = None
+
+
+    def add_client(self, client):
+        self.log.info('Adding camera client: %d' % len(self.clients))
+
+        if self.max_clients <= len(self.clients):
+            self.clients[-self.max_clients].write_img('in-use')
+
+        self.clients.append(client)
+        self._update_client_image()
+
+
+    def remove_client(self, client):
+        self.log.info('Removing camera client')
+        try:
+            self.clients.remove(client)
+        except: pass
+
+
+    def set_overtemp(self, overtemp):
+        if self.overtemp == overtemp: return
+        self.overtemp = overtemp
+
+        if overtemp: self.close(True)
+        elif self.path is not None: self.open(self.path)
+
+
+
+class VideoHandler(web.RequestHandler):
+    boundary = '-f36a3a39e5c955484390e0e3a6b031d1---'
+
+
+    def __init__(self, app, request, **kwargs):
+        super().__init__(app, request, **kwargs)
+        self.camera = app.camera
+
+
+    @web.asynchronous
+    def get(self):
+        self.request.connection.stream.max_write_buffer_size = 10000
+
+        self.set_header('Cache-Control', 'no-store, no-cache, must-revalidate, '
+                        'pre-check=0, post-check=0, max-age=0')
+        self.set_header('Connection', 'close')
+        self.set_header('Content-Type', 'multipart/x-mixed-replace;boundary=' +
+                        self.boundary)
+        self.set_header('Expires', 'Mon, 3 Jan 2000 12:34:56 GMT')
+        self.set_header('Pragma', 'no-cache')
+
+        if self.camera is None: self.write_img('offline')
+        else: self.camera.add_client(self)
+
+
+    def write_img(self, name):
+        self.write_frame_twice(get_image_resource('http/images/%s.jpg' % name))
+
+
+    def write_frame(self, frame):
+        # Don't allow too many frames to queue up
+        min_size = len(frame) * 2
+        if self.request.connection.stream.max_write_buffer_size < min_size:
+            self.request.connection.stream.max_write_buffer_size = min_size
+
+        try:
+            self.write(frame)
+            self.flush()
+
+        except iostream.StreamBufferFullError:
+            pass # Drop frame if buffer is full
+
+
+    def write_frame_twice(self, frame):
+        self.write_frame(frame)
+        self.write_frame(frame)
+
+
+    def on_connection_close(self): self.camera.remove_client(self)

src/py/bbctrl/Cmd.py

@@ -0,0 +1,280 @@
+#!/usr/bin/env python3
+
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import struct
+import base64
+import json
+
+# Keep this in sync with AVR code command.def
+SET          = '$'
+SET_SYNC     = '#'
+MODBUS_READ  = 'm'
+MODBUS_WRITE = 'M'
+SEEK         = 's'
+SET_AXIS     = 'a'
+LINE         = 'l'
+SYNC_SPEED   = '%'
+SPEED        = 'p'
+INPUT        = 'I'
+DWELL        = 'd'
+PAUSE        = 'P'
+STOP         = 'S'
+UNPAUSE      = 'U'
+JOG          = 'j'
+REPORT       = 'r'
+REBOOT       = 'R'
+RESUME       = 'c'
+ESTOP        = 'E'
+SHUTDOWN     = 'X'
+CLEAR        = 'C'
+FLUSH        = 'F'
+DUMP         = 'D'
+HELP         = 'h'
+
+SEEK_ACTIVE = 1 << 0
+SEEK_ERROR  = 1 << 1
+
+
+def encode_float(x):
+    return base64.b64encode(struct.pack('<f', x))[:-2].decode("utf-8")
+
+
+def decode_float(s):
+    return struct.unpack('<f', base64.b64decode(s + '=='))[0]
+
+
+def encode_axes(axes):
+    data = ''
+    for axis in 'xyzabc':
+        if axis in axes:
+            data += axis + encode_float(axes[axis])
+
+        elif axis.upper() in axes:
+            data += axis + encode_float(axes[axis.upper()])
+
+    return data
+
+
+def set_sync(name, value):
+    if isinstance(value, float): return set_float(name, value)
+    else: return SET_SYNC + '%s=%s' % (name, value)
+
+
+def set(name, value):
+    if isinstance(value, float): return set_float(name, value)
+    else: return SET + '%s=%s' % (name, value)
+
+
+def set_float(name, value):
+    return SET_SYNC + '%s=:%s' % (name, encode_float(value))
+
+
+def modbus_read(addr): return MODBUS_READ + '%d' % addr
+def modbus_write(addr, value): return MODBUS_WRITE + '%d=%d' % (addr, value)
+def set_axis(axis, position): return SET_AXIS + axis + encode_float(position)
+
+
+def line(target, exitVel, maxAccel, maxJerk, times, speeds):
+    cmd = LINE
+
+    cmd += encode_float(exitVel)
+    cmd += encode_float(maxAccel)
+    cmd += encode_float(maxJerk)
+    cmd += encode_axes(target)
+
+    # S-Curve time parameters
+    for i in range(7):
+        if times[i]:
+            cmd += str(i) + encode_float(times[i] / 60000) # to mins
+
+    # Speeds
+    for dist, speed in speeds:
+        cmd += '\n' + sync_speed(dist, speed)
+
+    return cmd
+
+
+def speed(value): return SPEED + encode_float(value)
+
+
+def sync_speed(dist, speed):
+    return SYNC_SPEED + encode_float(dist) + encode_float(speed)
+
+
+def input(port, mode, timeout):
+    type, index, m = 'd', 0, 0
+
+    # Analog/digital & port index
+    if port == 'digital-in-0': type, index = 'd', 0
+    if port == 'digital-in-1': type, index = 'd', 1
+    if port == 'digital-in-2': type, index = 'd', 2
+    if port == 'digital-in-3': type, index = 'd', 3
+    if port == 'analog-in-0':  type, index = 'a', 0
+    if port == 'analog-in-1':  type, index = 'a', 1
+    if port == 'analog-in-2':  type, index = 'a', 2
+    if port == 'analog-in-3':  type, index = 'a', 3
+
+    # Mode
+    if mode == 'immediate': m = 0
+    if mode == 'rise':      m = 1
+    if mode == 'fall':      m = 2
+    if mode == 'high':      m = 3
+    if mode == 'low':       m = 4
+
+    return '%s%s%d%d%s' % (INPUT, type, index, m, encode_float(timeout))
+
+
+def output(port, value):
+    if port == 'mist':  return '#1oa=' + ('1' if value else '0')
+    if port == 'flood': return '#2oa=' + ('1' if value else '0')
+    raise Exception('Unsupported output "%s"' % port)
+
+
+def dwell(seconds): return DWELL + encode_float(seconds)
+
+
+def pause(type):
+    if type == 'program': type = 1
+    elif type == 'optional': type = 2
+    elif type == 'pallet-change': type = 1
+    else: raise Exception('Unknown pause type "%s"' % type)
+
+    return '%s%d' % (PAUSE, type)
+
+
+def jog(axes): return JOG + encode_axes(axes)
+
+
+def seek(switch, active, error):
+    # cmd = SEEK + str(switch)
+    cmd = SEEK + '%x' % switch
+
+    flags = 0
+    if active: flags |= SEEK_ACTIVE
+    if error:  flags |= SEEK_ERROR
+    cmd += chr(flags + ord('0'))
+
+    return cmd
+
+
+def decode_command(cmd):
+    if not len(cmd): return
+
+    data = {}
+
+    if cmd[0] == SET or cmd[0] == SET_SYNC:
+        data['type'] = 'set'
+        if cmd[0] == SET_SYNC: data['sync'] = True
+
+        equal = cmd.find('=')
+        data['name'] = cmd[1:equal]
+
+        value = cmd[equal + 1:]
+
+        if value.lower() == 'true': value = True
+        elif value.lower() == 'false': value = False
+        elif value.find('.') == -1: data['value'] = int(value)
+        else: data['value'] = float(value)
+
+    elif cmd[0] == JOG:
+        data['type'] = 'jog'
+
+        cmd = cmd[1:]
+        while len(cmd):
+            name = cmd[0]
+            value = decode_float(cmd[1:7])
+            cmd = cmd[7:]
+
+            if name in 'xyzabcuvw': data[name] = value
+
+
+    elif cmd[0] == SEEK:
+        data['type'] = 'seek'
+
+        data['port'] = int(cmd[2], 16)
+        flags = int(cmd[2], 16)
+
+        data['active'] = bool(flags & SEEK_ACTIVE)
+        data['error'] = bool(flags & SEEK_ERROR)
+
+    elif cmd[0] == LINE:
+        data['type'] = 'line'
+        data['exit-vel']  = decode_float(cmd[1:7])
+        data['max-accel'] = decode_float(cmd[7:13])
+        data['max-jerk']  = decode_float(cmd[13:19])
+
+        data['target'] = {}
+        data['times'] = [0] * 7
+        cmd = cmd[19:]
+
+        while len(cmd):
+            name = cmd[0]
+            value = decode_float(cmd[1:7])
+            cmd = cmd[7:]
+
+            if name in 'xyzabcuvw': data['target'][name] = value
+            else: data['times'][int(name)] = value
+
+    elif cmd[0] == SYNC_SPEED:
+        data['type'] = 'speed'
+        data['offset'] = decode_float(cmd[1:7])
+        data['speed']  = decode_float(cmd[7:13])
+
+    elif cmd[0] == REPORT:   data['type'] = 'report'
+    elif cmd[0] == PAUSE:    data['type'] = 'pause'
+    elif cmd[0] == UNPAUSE:  data['type'] = 'unpause'
+    elif cmd[0] == ESTOP:    data['type'] = 'estop'
+    elif cmd[0] == SHUTDOWN: data['type'] = 'shutdown'
+    elif cmd[0] == CLEAR:    data['type'] = 'clear'
+    elif cmd[0] == FLUSH:    data['type'] = 'flush'
+    elif cmd[0] == RESUME:   data['type'] = 'resume'
+
+    return data
+
+
+def decode(cmd):
+    for line in cmd.split('\n'):
+        yield decode_command(line.strip())
+
+
+def decode_and_print(cmd):
+    for data in decode(cmd):
+        print(json.dumps(data))
+
+
+if __name__ == "__main__":
+    import sys
+
+    if 1 < len(sys.argv):
+        for arg in sys.argv[1:]:
+            decode_and_print(arg)
+
+    else:
+        for line in sys.stdin:
+            decode_and_print(str(line).strip())

src/py/bbctrl/Comm.py

@@ -0,0 +1,254 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import serial
+import json
+import time
+import traceback
+from collections import deque
+
+import bbctrl
+import bbctrl.Cmd as Cmd
+
+
+# Must be kept in sync with drv8711.h
+DRV8711_STATUS_OTS_bm    = 1 << 0
+DRV8711_STATUS_AOCP_bm   = 1 << 1
+DRV8711_STATUS_BOCP_bm   = 1 << 2
+DRV8711_STATUS_APDF_bm   = 1 << 3
+DRV8711_STATUS_BPDF_bm   = 1 << 4
+DRV8711_STATUS_UVLO_bm   = 1 << 5
+DRV8711_STATUS_STD_bm    = 1 << 6
+DRV8711_STATUS_STDLAT_bm = 1 << 7
+DRV8711_COMM_ERROR_bm    = 1 << 8
+
+# Ignoring stall and stall latch flags for now
+DRV8711_MASK = ~(DRV8711_STATUS_STD_bm | DRV8711_STATUS_STDLAT_bm)
+
+
+def _driver_flags_to_string(flags):
+    if DRV8711_STATUS_OTS_bm    & flags: yield 'over temp'
+    if DRV8711_STATUS_AOCP_bm   & flags: yield 'over current a'
+    if DRV8711_STATUS_BOCP_bm   & flags: yield 'over current b'
+    if DRV8711_STATUS_APDF_bm   & flags: yield 'driver fault a'
+    if DRV8711_STATUS_BPDF_bm   & flags: yield 'driver fault b'
+    if DRV8711_STATUS_UVLO_bm   & flags: yield 'undervoltage'
+    if DRV8711_STATUS_STD_bm    & flags: yield 'stall'
+    if DRV8711_STATUS_STDLAT_bm & flags: yield 'stall latch'
+    if DRV8711_COMM_ERROR_bm    & flags: yield 'comm error'
+
+
+def driver_flags_to_string(flags):
+    return ', '.join(_driver_flags_to_string(flags))
+
+
+class Comm(object):
+    def __init__(self, ctrl, avr):
+        self.ctrl = ctrl
+        self.avr = avr
+        self.log = self.ctrl.log.get('Comm')
+        self.queue = deque()
+        self.in_buf = ''
+        self.command = None
+        self.last_motor_flags = [0] * 4
+
+        avr.set_handlers(self._read, self._write)
+        self._poll_cb(False)
+
+
+    def comm_next(self): raise Exception('Not implemented')
+    def comm_error(self): raise Exception('Not implemented')
+
+
+    def is_active(self): return len(self.queue) or self.command is not None
+
+
+    def i2c_command(self, cmd, byte = None, word = None, block = None):
+        self.log.info('I2C: %s b=%s w=%s d=%s' % (cmd, byte, word, block))
+        self.avr.i2c_command(cmd, byte, word, block)
+
+
+    def flush(self): self.avr.enable_write(True)
+
+
+    def _load_next_command(self, cmd):
+        self.log.info('< ' + json.dumps(cmd).strip('"'))
+        self.command = bytes(cmd.strip() + '\n', 'utf-8')
+
+
+    def resume(self): self.queue_command(Cmd.RESUME)
+
+
+    def queue_command(self, cmd):
+        self.queue.append(cmd)
+        self.flush()
+
+
+    def _poll_cb(self, now = True):
+        # Checks periodically for new commands from planner via comm_next()
+        if now: self.flush()
+        self.ctrl.ioloop.call_later(1, self._poll_cb)
+
+
+    def _write(self, write_cb):
+        # Finish writing current command
+        if self.command is not None:
+            try:
+                count = write_cb(self.command)
+
+            except Exception as e:
+                self.command = None
+                raise e
+
+            self.command = self.command[count:]
+            if len(self.command): return # There's more
+            self.command = None
+
+        # Load next command from queue
+        if len(self.queue): self._load_next_command(self.queue.popleft())
+
+        # Load next command from callback
+        else:
+            cmd = self.comm_next() # pylint: disable=assignment-from-no-return
+
+            if cmd is None: self.avr.enable_write(False) # Stop writing
+            else: self._load_next_command(cmd)
+
+
+    def _update_vars(self, msg):
+        try:
+            self.ctrl.state.set_machine_vars(msg['variables'])
+            self.ctrl.configure()
+            self.queue_command(Cmd.DUMP) # Refresh all vars
+
+            # Set axis positions
+            for axis in 'xyzabc':
+                position = self.ctrl.state.get(axis + 'p', 0)
+                self.queue_command(Cmd.set_axis(axis, position))
+
+        except Exception as e:
+            self.log.warning('AVR reload failed: %s', traceback.format_exc())
+            self.ctrl.ioloop.call_later(1, self.connect)
+
+
+    def _log_msg(self, msg):
+        level = msg.get('level', 'info')
+        where = msg.get('where')
+        msg = msg['msg']
+
+        if   level == 'info':    self.log.info(msg,    where = where)
+        elif level == 'debug':   self.log.debug(msg,   where = where)
+        elif level == 'warning': self.log.warning(msg, where = where)
+        elif level == 'error':   self.log.error(msg,   where = where)
+
+        if level == 'error': self.comm_error()
+
+        # Treat machine alarmed warning as an error
+        if level == 'warning' and 'code' in msg and msg['code'] == 11:
+            self.comm_error()
+
+
+    def _log_motor_flags(self, update):
+        for motor in range(3):
+            var = '%ddf' % motor
+
+            if var in update:
+                flags = update[var] & DRV8711_MASK
+
+                if self.last_motor_flags[motor] == flags: continue
+                self.last_motor_flags[motor] = flags
+
+                flags = driver_flags_to_string(flags)
+                self.log.info('Motor %d flags: %s' % (motor, flags))
+
+
+    def _update_state(self, update):
+        self.ctrl.state.update(update)
+
+        if 'xx' in update:        # State change
+            self.ctrl.ready()     # We've received data from AVR
+            self.flush()          # May have more data to send now
+
+        self._log_motor_flags(update)
+
+
+    def _read(self, data):
+        self.in_buf += data.decode('utf-8')
+
+        # Parse incoming serial data into lines
+        while True:
+            i = self.in_buf.find('\n')
+            if i == -1: break
+            line = self.in_buf[0:i].strip()
+            self.in_buf = self.in_buf[i + 1:]
+
+            if line:
+                self.log.info('> ' + line)
+
+                try:
+                    msg = json.loads(line)
+
+                except Exception as e:
+                    self.log.warning('%s, data: %s', e, line)
+                    continue
+
+                if 'variables' in msg: self._update_vars(msg)
+                elif 'msg' in msg: self._log_msg(msg)
+
+                elif 'firmware' in msg:
+                    self.log.info('AVR firmware rebooted')
+                    self.connect()
+
+                else: self._update_state(msg)
+
+
+    def estop(self):
+        if self.ctrl.state.get('xx', '') != 'ESTOPPED':
+            self.i2c_command(Cmd.ESTOP)
+
+
+    def clear(self):
+        if self.ctrl.state.get('xx', '') == 'ESTOPPED':
+            self.i2c_command(Cmd.CLEAR)
+
+
+    def pause(self):
+        self.i2c_command(Cmd.PAUSE, byte = ord('0')) # User pause
+
+
+    def reboot(self): self.queue_command(Cmd.REBOOT)
+
+
+    def connect(self):
+        try:
+            # Resume once current queue of GCode commands has flushed
+            self.queue_command(Cmd.RESUME)
+            self.queue_command(Cmd.HELP) # Load AVR commands and variables
+
+        except Exception as e:
+            self.log.warning('Connect failed: %s', e)
+            self.ctrl.ioloop.call_later(1, self.connect)

src/py/bbctrl/CommandQueue.py

@@ -0,0 +1,84 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import bbctrl
+from collections import deque
+
+
+# 16-bit less with wrap around
+def id_less(a, b): return (1 << 15) < (a - b) & ((1 << 16) - 1)
+
+
+class CommandQueue():
+    def __init__(self, ctrl):
+        self.log = ctrl.log.get('CmdQ')
+        self.log.set_level(bbctrl.log.WARNING)
+
+        self.lastEnqueueID = 0
+        self.releaseID = 0
+        self.q = deque()
+
+
+    def is_active(self): return len(self.q)
+
+
+    def clear(self):
+        self.lastEnqueueID = 0
+        self.releaseID = 0
+        self.q.clear()
+
+
+    def enqueue(self, id, cb, *args, **kwargs):
+        self.log.info('add(#%d) releaseID=%d', id, self.releaseID)
+        self.lastEnqueueID = id
+        self.q.append([id, cb, args, kwargs])
+        self._release()
+
+
+    def _release(self):
+        while len(self.q):
+            id, cb, args, kwargs = self.q[0]
+
+            # Execute commands <= releaseID
+            if id_less(self.releaseID, id): return
+
+            self.log.info('releasing id=%d' % id)
+            self.q.popleft()
+
+            try:
+                if cb is not None: cb(*args, **kwargs)
+            except Exception:
+                self.log.exception('During command queue callback')
+
+
+
+    def release(self, id):
+        if id and not id_less(self.releaseID, id):
+            self.log.debug('id out of order %d <= %d' % (id, self.releaseID))
+        self.releaseID = id
+
+        self._release()

src/py/bbctrl/Config.py

@@ -0,0 +1,244 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import json
+import pkg_resources
+import subprocess
+import copy
+from pkg_resources import Requirement, resource_filename
+
+
+def get_resource(path):
+    return resource_filename(Requirement.parse('bbctrl'), 'bbctrl/' + path)
+
+
+class Config(object):
+    def __init__(self, ctrl):
+        self.ctrl = ctrl
+        self.log = ctrl.log.get('Config')
+
+        self.values = {}
+
+        try:
+            self.version = pkg_resources.require('bbctrl')[0].version
+
+            # Load config template
+            with open(get_resource('http/config-template.json'), 'r',
+                      encoding = 'utf-8') as f:
+                self.template = json.load(f)
+
+        except Exception: self.log.exception()
+
+
+    def get(self, name, default = None):
+        return self.values.get(name, default)
+
+
+    def get_index(self, name, index, default = None):
+        return self.values.get(name, {}).get(str(index), None)
+
+
+    def load(self):
+        path = self.ctrl.get_path('config.json')
+
+        try:
+            if os.path.exists(path):
+                with open(path, 'r') as f: config = json.load(f)
+            else: config = {'version': self.version}
+
+            try:
+                self.upgrade(config)
+            except Exception: self.log.exception()
+
+        except Exception as e:
+            self.log.warning('%s', e)
+            config = {'version': self.version}
+
+        self._defaults(config)
+        return config
+
+
+    def _valid_value(self, template, value):
+        type = template['type']
+
+        try:
+            if type == 'int':   value = int(value)
+            if type == 'float': value = float(value)
+            if type == 'text':  value = str(value)
+            if type == 'bool':  value = bool(value)
+        except:
+            return False
+
+        if 'values' in template and value not in template['values']:
+            return False
+
+        return True
+
+
+    def __defaults(self, config, name, template):
+        if 'type' in template:
+            if (not name in config or
+                not self._valid_value(template, config[name])):
+                config[name] = template['default']
+
+            elif 'max' in template and template['max'] < config[name]:
+                config[name] = template['max']
+
+            elif 'min' in template and config[name] < template['min']:
+                config[name] = template['min']
+
+            if template['type'] == 'list':
+                config = config[name]
+
+                for i in range(len(template['index'])):
+                    if len(config) <= i: config.append({})
+
+                    for name, tmpl in template['template'].items():
+                        self.__defaults(config[i], name, tmpl)
+
+        else:
+            for name, tmpl in template.items():
+                self.__defaults(config, name, tmpl)
+
+
+    def _defaults(self, config):
+        for name, tmpl in self.template.items():
+            if not 'type' in tmpl:
+                if not name in config: config[name] = {}
+                conf = config[name]
+            else: conf = config
+
+            self.__defaults(conf, name, tmpl)
+
+
+    def upgrade(self, config):
+        version = tuple(map(int, config['version'].split('.')))
+
+        if version < (0, 2, 4):
+            for motor in config['motors']:
+                for key in 'max-jerk max-velocity'.split():
+                    if key in motor: motor[key] /= 1000
+
+        if version < (0, 3, 4):
+            for motor in config['motors']:
+                for key in 'max-accel latch-velocity search-velocity'.split():
+                    if key in motor: motor[key] /= 1000
+
+        if version <= (0, 3, 22):
+            if 'tool' in config:
+                if 'spindle-type' in config['tool']:
+                    type = config['tool']['spindle-type']
+                    if type == 'PWM': type = 'PWM Spindle'
+                    if type == 'Huanyang': type = 'Huanyang VFD'
+                    config['tool']['tool-type'] = type
+                    del config['tool']['spindle-type']
+
+                if 'spin-reversed' in config['tool']:
+                    reversed = config['tool']['spin-reversed']
+                    config['tool']['tool-reversed'] = reversed
+                    del config['tool']['spin-reversed']
+
+        if version <= (0, 4, 6):
+            for motor in config['motors']:
+                if 2 < motor.get('idle-current', 0): motor['idle-current'] = 2
+                if 'enabled' not in motor:
+                    motor['enabled'] = motor.get('power-mode', '') != 'disabled'
+
+        config['version'] = self.version
+
+
+    def save(self, config):
+        self.upgrade(config)
+        self._update(config, False)
+
+        with open(self.ctrl.get_path('config.json'), 'w') as f:
+            json.dump(config, f)
+
+        os.sync()
+
+        self.ctrl.preplanner.invalidate_all()
+        self.log.info('Saved')
+
+
+    def reset(self):
+        if os.path.exists('config.json'): os.unlink('config.json')
+        self.reload()
+        self.ctrl.preplanner.invalidate_all()
+
+
+    def _encode(self, name, index, config, tmpl, with_defaults):
+        # Handle category
+        if not 'type' in tmpl:
+            for name, entry in tmpl.items():
+                if 'type' in entry and config is not None:
+                    conf = config.get(name, None)
+                else: conf = config
+                self._encode(name, index, conf, entry, with_defaults)
+            return
+
+        # Handle defaults
+        if config is not None: value = config
+        elif with_defaults: value = tmpl['default']
+        else: return
+
+        # Handle list
+        if tmpl['type'] == 'list':
+            for i in range(len(tmpl['index'])):
+                if config is not None and i < len(config): conf = config[i]
+                else: conf = None
+                self._encode(name, index + tmpl['index'][i], conf,
+                             tmpl['template'], with_defaults)
+            return
+
+        # Update config values
+        if index:
+            if not name in self.values: self.values[name] = {}
+            self.values[name][index] = value
+
+        else: self.values[name] = value
+
+        # Update state variable
+        if not 'code' in tmpl: return
+
+        if tmpl['type'] == 'enum':
+            if value in tmpl['values']: value = tmpl['values'].index(value)
+            else: value = tmpl['default']
+
+        elif tmpl['type'] == 'bool': value = 1 if value else 0
+        elif tmpl['type'] == 'percent': value /= 100.0
+
+        self.ctrl.state.config(index + tmpl['code'], value)
+
+
+    def _update(self, config, with_defaults):
+        for name, tmpl in self.template.items():
+            conf = config.get(name, None)
+            self._encode(name, '', conf, tmpl, with_defaults)
+
+
+    def reload(self): self._update(self.load(), True)

src/py/bbctrl/Ctrl.py

@@ -0,0 +1,115 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import time
+import bbctrl
+
+
+class Ctrl(object):
+    def __init__(self, args, ioloop, id):
+        self.args = args
+        self.ioloop = bbctrl.IOLoop(ioloop)
+        self.id = id
+        self.timeout = None # Used in demo mode
+
+        if id and not os.path.exists(id): os.mkdir(id)
+
+        # Start log
+        if args.demo: log_path = self.get_path(filename = 'bbctrl.log')
+        else: log_path = args.log
+        self.log = bbctrl.log.Log(args, self.ioloop, log_path)
+
+        self.state = bbctrl.State(self)
+        self.config = bbctrl.Config(self)
+
+        self.log.get('Ctrl').info('Starting %s' % self.id)
+
+        try:
+            if args.demo: self.avr = bbctrl.AVREmu(self)
+            else: self.avr = bbctrl.AVR(self)
+
+            self.i2c = bbctrl.I2C(args.i2c_port, args.demo)
+            self.lcd = bbctrl.LCD(self)
+            self.mach = bbctrl.Mach(self, self.avr)
+            self.preplanner = bbctrl.Preplanner(self)
+            if not args.demo: self.jog = bbctrl.Jog(self)
+            self.pwr = bbctrl.Pwr(self)
+
+            self.mach.connect()
+
+            self.lcd.add_new_page(bbctrl.MainLCDPage(self))
+            self.lcd.add_new_page(bbctrl.IPLCDPage(self.lcd))
+
+            os.environ['GCODE_SCRIPT_PATH'] = self.get_upload()
+
+        except Exception: self.log.get('Ctrl').exception()
+
+
+    def __del__(self): print('Ctrl deleted')
+
+
+    def clear_timeout(self):
+        if self.timeout is not None: self.ioloop.remove_timeout(self.timeout)
+        self.timeout = None
+
+
+    def set_timeout(self, cb, *args, **kwargs):
+        self.clear_timeout()
+        t = self.args.client_timeout
+        self.timeout = self.ioloop.call_later(t, cb, *args, **kwargs)
+
+
+    def get_path(self, dir = None, filename = None):
+        path = './' + self.id if self.id else '.'
+        path = path if dir is None else (path + '/' + dir)
+        return path if filename is None else (path + '/' + filename)
+
+
+    def get_upload(self, filename = None):
+        return self.get_path('upload', filename)
+
+
+    def get_plan(self, filename = None):
+        return self.get_path('plans', filename)
+
+
+    def configure(self):
+        # Indirectly configures state via calls to config() and the AVR
+        self.config.reload()
+
+
+    def ready(self):
+        # This is used to synchronize the start of the preplanner
+        self.preplanner.start()
+
+
+    def close(self):
+        self.log.get('Ctrl').info('Closing %s' % self.id)
+        self.ioloop.close()
+        self.avr.close()
+        self.mach.planner.close()

src/py/bbctrl/FileHandler.py

@@ -0,0 +1,85 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import bbctrl
+import glob
+import html
+from tornado import gen
+from tornado.web import HTTPError
+
+
+def safe_remove(path):
+    try:
+        os.unlink(path)
+    except OSError: pass
+
+
+class FileHandler(bbctrl.APIHandler):
+    def prepare(self): pass
+
+
+    def delete_ok(self, filename):
+        if not filename:
+            # Delete everything
+            for path in glob.glob(self.get_upload('*')): safe_remove(path)
+            self.get_ctrl().preplanner.delete_all_plans()
+            self.get_ctrl().state.clear_files()
+
+        else:
+            # Delete a single file
+            filename = os.path.basename(filename)
+            safe_remove(self.get_upload(filename))
+            self.get_ctrl().preplanner.delete_plans(filename)
+            self.get_ctrl().state.remove_file(filename)
+
+
+    def put_ok(self, *args):
+        gcode = self.request.files['gcode'][0]
+        filename = os.path.basename(gcode['filename'].replace('\\', '/'))
+        filename = filename.replace('#', '-').replace('?', '-')
+
+        if not os.path.exists(self.get_upload()): os.mkdir(self.get_upload())
+
+        with open(self.get_upload(filename).encode('utf8'), 'wb') as f:
+            f.write(gcode['body'])
+        os.sync()
+
+        self.get_ctrl().preplanner.invalidate(filename)
+        self.get_ctrl().state.add_file(filename)
+        self.get_log('FileHandler').info('GCode received: ' + filename)
+
+
+    @gen.coroutine
+    def get(self, filename):
+        if not filename: raise HTTPError(400, 'Missing filename')
+        filename = os.path.basename(filename)
+
+        with open(self.get_upload(filename).encode('utf8'), 'r') as f:
+            self.write(f.read())
+
+        self.get_ctrl().state.select_file(filename)

src/py/bbctrl/I2C.py

@@ -0,0 +1,91 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import errno
+
+try:
+    try:
+        import smbus
+    except:
+        import smbus2 as smbus
+except:
+    smbus = None
+
+
+class I2C(object):
+    def __init__(self, port, disabled):
+        self.port = port
+        self.i2c_bus = None
+        self.disabled = disabled or smbus is None
+
+
+    def connect(self):
+        if self.disabled: return
+        if self.i2c_bus is None:
+            try:
+                self.i2c_bus = smbus.SMBus(self.port)
+
+            except OSError as e:
+                self.i2c_bus = None
+                if e.errno == errno.ENOENT: self.disabled = True
+                else: raise type(e)('I2C failed to open device: %s' % e)
+
+
+    def read_word(self, addr):
+        self.connect()
+        if self.disabled: return
+
+        try:
+            return self.i2c_bus.read_word_data(addr, 0)
+
+        except IOError as e:
+            self.i2c_bus.close()
+            self.i2c_bus = None
+            raise type(e)('I2C read word failed: %s' % e)
+
+
+    def write(self, addr, cmd, byte = None, word = None, block = None):
+        self.connect()
+        if self.disabled: return
+
+        try:
+            if byte is not None:
+                self.i2c_bus.write_byte_data(addr, cmd, byte)
+
+            elif word is not None:
+                self.i2c_bus.write_word_data(addr, cmd, word)
+
+            elif block is not None:
+                if isinstance(block, str): block = list(map(ord, block))
+                self.i2c_bus.write_i2c_block_data(addr, cmd, block)
+
+            else: self.i2c_bus.write_byte(addr, cmd)
+
+        except IOError as e:
+            self.i2c_bus.close()
+            self.i2c_bus = None
+            raise type(e)('I2C write failed: %s' % e)

src/py/bbctrl/IOLoop.py

@@ -0,0 +1,93 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import tornado.ioloop
+import bbctrl
+
+
+class CB(object):
+    def __init__(self, ioloop, delay, cb, *args, **kwargs):
+        self.ioloop = ioloop
+        self.cb = cb
+
+        io = ioloop.ioloop
+        self.h = io.call_later(delay, self._cb, *args, **kwargs)
+
+        ioloop.callbacks[self.h] = self
+
+    def _cb(self, *args, **kwarg):
+        del self.ioloop.callbacks[self.h]
+        return self.cb(*args, **kwarg)
+
+
+class IOLoop(object):
+    READ = tornado.ioloop.IOLoop.READ
+    WRITE = tornado.ioloop.IOLoop.WRITE
+    ERROR = tornado.ioloop.IOLoop.ERROR
+
+
+    def __init__(self, ioloop):
+        self.ioloop = ioloop
+        self.fds = set()
+        self.handles = set()
+        self.callbacks = {}
+
+
+    def close(self):
+        for fd in list(self.fds): self.ioloop.remove_handler(fd)
+        for h in list(self.handles): self.ioloop.remove_timeout(h)
+        for h in list(self.callbacks): self.ioloop.remove_timeout(h)
+
+
+    def add_handler(self, fd, handler, events):
+        self.ioloop.add_handler(fd, handler, events)
+        if hasattr(fd, 'fileno'): fd = fd.fileno()
+        self.fds.add(fd)
+
+
+    def remove_handler(self, h):
+        self.ioloop.remove_handler(h)
+        if hasattr(h, 'fileno'): h = h.fileno()
+        self.fds.remove(h)
+
+
+    def update_handler(self, fd, events): self.ioloop.update_handler(fd, events)
+
+
+    def call_later(self, delay, callback, *args, **kwargs):
+        cb = CB(self, delay, callback, *args, **kwargs)
+        return cb.h
+
+
+    def remove_timeout(self, h):
+        self.ioloop.remove_timeout(h)
+        if h in self.handles: self.handles.remove(h)
+        if h in self.callbacks: del self.callbacks[h]
+
+
+    def add_callback(self, cb, *args, **kwargs):
+        self.ioloop.add_callback(cb, *args, **kwargs)

src/py/bbctrl/IPLCDPage.py

@@ -0,0 +1,48 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import subprocess
+
+import bbctrl
+
+
+class IPLCDPage(bbctrl.LCDPage):
+    # From bbctrl.LCDPage
+    def activate(self):
+        p = subprocess.Popen(['hostname', '-I'], stdout = subprocess.PIPE)
+        ips = p.communicate()[0].decode('utf-8').split()
+
+        p = subprocess.Popen(['hostname'], stdout = subprocess.PIPE)
+        hostname = p.communicate()[0].decode('utf-8').strip()
+
+        self.clear()
+
+        self.text('Host: %s' % hostname[0:14], 0, 0)
+
+        for i in range(min(3, len(ips))):
+            if len(ips[i]) <= 16:
+                self.text('IP: %s' % ips[i], 0, i + 1)

src/py/bbctrl/Jog.py

@@ -0,0 +1,93 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import inevent
+from inevent.Constants import *
+
+
+# Listen for input events
+class Jog(inevent.JogHandler):
+    def __init__(self, ctrl):
+        self.ctrl = ctrl
+        self.log = ctrl.log.get('Jog')
+
+        config = {
+            "Logitech Logitech RumblePad 2 USB": {
+                "deadband": 0.1,
+                "axes":     [ABS_X, ABS_Y, ABS_RZ, ABS_Z],
+                "dir":      [1, -1, -1, 1],
+                "arrows":   [ABS_HAT0X, ABS_HAT0Y],
+                "speed":    [0x120, 0x121, 0x122, 0x123],
+                "lock":     [0x124, 0x125],
+            },
+
+            "default": {
+                "deadband": 0.1,
+                "axes":     [ABS_X, ABS_Y, ABS_RY, ABS_RX],
+                "dir":      [1, -1, -1, 1],
+                "arrows":   [ABS_HAT0X, ABS_HAT0Y],
+                "speed":    [0x133, 0x130, 0x131, 0x134],
+                "lock":     [0x136, 0x137],
+            }
+        }
+
+        super().__init__(config)
+
+        self.v = [0.0] * 4
+        self.lastV = self.v
+        self.callback()
+
+        self.processor = inevent.InEvent(ctrl.ioloop, self, types = ['js'])
+
+
+    def up(self): self.ctrl.lcd.page_up()
+    def down(self): self.ctrl.lcd.page_down()
+    def left(self): self.ctrl.lcd.page_left()
+    def right(self): self.ctrl.lcd.page_right()
+
+
+    def callback(self):
+        if self.v != self.lastV:
+            self.lastV = self.v
+            try:
+                axes = {}
+                for i in range(len(self.v)): axes["xyzabc"[i]] = self.v[i]
+                self.ctrl.mach.jog(axes)
+
+            except Exception as e:
+                self.log.warning('Jog: %s', e)
+
+        self.ctrl.ioloop.call_later(0.25, self.callback)
+
+
+    def changed(self):
+        scale = 1.0
+        if self.speed == 1: scale = 1.0 / 128.0
+        if self.speed == 2: scale = 1.0 / 32.0
+        if self.speed == 3: scale = 1.0 / 4.0
+
+        self.v = [x * scale for x in self.axes]

src/py/bbctrl/LCD.py

@@ -0,0 +1,207 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import lcd
+import atexit
+
+
+class LCDPage:
+    def __init__(self, lcd, text = None):
+        self.lcd = lcd
+        self.data = lcd.new_screen()
+
+        if text is not None:
+            self.text(text, (lcd.width - len(text)) // 2, 1)
+
+
+    def activate(self): pass
+    def deactivate(self): pass
+
+
+    def put(self, c, x, y):
+        y += x // self.lcd.width
+        x %= self.lcd.width
+        y %= self.lcd.height
+
+        if self.data[x][y] != c:
+            self.data[x][y] = c
+            if self == self.lcd.page: self.lcd.update()
+
+
+    def text(self, s, x, y):
+        for c in s:
+            self.put(c, x, y)
+            x += 1
+
+
+    def clear(self):
+        self.data = self.lcd.new_screen()
+        self.lcd.redraw = True
+
+
+    def shift_left(self): pass
+    def shift_right(self): pass
+    def shift_up(self): pass
+    def shift_down(self): pass
+
+
+class LCD:
+    def __init__(self, ctrl):
+        self.ctrl = ctrl
+        self.log = ctrl.log.get('LCD')
+
+        self.addrs = self.ctrl.args.lcd_addr
+        self.addr = self.addrs[0]
+        self.addr_num = 0
+
+        self.width = 20
+        self.height = 4
+        self.lcd = None
+        self.timeout = None
+        self.reset = False
+        self.page = None
+        self.pages = []
+        self.current_page = 0
+        self.screen = self.new_screen()
+        self.set_message('Loading...')
+
+        self._redraw(False)
+        if not ctrl.args.demo: atexit.register(self.goodbye)
+
+
+    def set_message(self, msg):
+        try:
+            self.load_page(LCDPage(self, msg))
+            self._update()
+        except IOError as e:
+            self.log.warning('LCD communication failed: %s' % e)
+
+
+    def new_screen(self):
+        return [[' ' for y in range(self.height)] for x in range(self.width)]
+
+
+    def new_page(self): return LCDPage(self)
+    def add_page(self, page): self.pages.append(page)
+
+
+    def add_new_page(self, page = None):
+        if page is None: page = self.new_page()
+        page.id = len(self.pages)
+        self.add_page(page)
+        return page
+
+
+    def load_page(self, page):
+        if self.page != page:
+            if self.page is not None: self.page.deactivate()
+            page.activate()
+            self.page = page
+            self.redraw = True
+            self.update()
+
+
+    def set_current_page(self, current_page):
+        self.current_page = current_page % len(self.pages)
+        self.load_page(self.pages[self.current_page])
+
+
+    def page_up(self): pass
+    def page_down(self): pass
+    def page_right(self): self.set_current_page(self.current_page + 1)
+    def page_left(self): self.set_current_page(self.current_page - 1)
+
+
+    def update(self):
+        if self.timeout is None:
+            self.timeout = self.ctrl.ioloop.call_later(0.25, self._update)
+
+
+    def _redraw(self, now = True):
+        if now:
+            self.redraw = True
+            self.update()
+        self.redraw_timer = self.ctrl.ioloop.call_later(5, self._redraw)
+
+
+    def _update(self):
+        self.timeout = None
+
+        try:
+            if self.lcd is None:
+                self.lcd = lcd.LCD(self.ctrl.i2c, self.addr, self.height,
+                                   self.width)
+
+            if self.reset:
+                self.lcd.reset()
+                self.redraw = True
+                self.reset = False
+
+            cursorX, cursorY = -1, -1
+
+            for y in range(self.height):
+                for x in range(self.width):
+                    c = self.page.data[x][y]
+
+                    if self.redraw or self.screen[x][y] != c:
+                        if cursorX != x or cursorY != y:
+                            self.lcd.goto(x, y)
+                            cursorX, cursorY = x, y
+
+                        self.lcd.put_char(c)
+                        cursorX += 1
+                        self.screen[x][y] = c
+
+            self.redraw = False
+
+        except IOError as e:
+            # Try next address
+            #self.addr_num += 1
+            #if len(self.addrs) <= self.addr_num: self.addr_num = 0
+            #self.addr = self.addrs[self.addr_num]
+            #self.lcd = None
+
+            #self.log.warning('LCD communication failed, ' +
+            #            'retrying on address 0x%02x: %s' % (self.addr, e))
+
+            #self.log.warning('LCD not present.')
+
+            #self.reset = True
+            self.reset = False
+            #self.timeout = self.ctrl.ioloop.call_later(1, self._update)
+
+
+    def goodbye(self, message = ''):
+        if self.timeout:
+            self.ctrl.ioloop.remove_timeout(self.timeout)
+            self.timeout = None
+
+        if self.redraw_timer:
+            self.ctrl.ioloop.remove_timeout(self.redraw_timer)
+            self.redraw_timer = None
+
+        if self.lcd is not None: self.set_message(message)

src/py/bbctrl/Log.py

@@ -0,0 +1,185 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import sys
+import io
+import datetime
+import traceback
+import pkg_resources
+from inspect import getframeinfo, stack
+import bbctrl
+
+
+DEBUG    = 0
+INFO     = 1
+MESSAGE  = 2
+WARNING  = 3
+ERROR    = 4
+
+
+level_names = 'debug info message warning error'.split()
+
+def get_level_name(level): return level_names[level]
+
+
+# Get this file's name
+_srcfile = os.path.normcase(get_level_name.__code__.co_filename)
+
+
+class Logger(object):
+    def __init__(self, log, name, level):
+        self.log = log
+        self.name = name
+        self.level = level
+
+
+    def set_level(self, level): self.level = level
+    def _enabled(self, level): return self.level <= level and level <= ERROR
+
+
+    def _find_caller(self):
+        f = sys._getframe()
+        if f is not None: f = f.f_back
+
+        while hasattr(f, 'f_code'):
+            co = f.f_code
+
+            filename = os.path.normcase(co.co_filename)
+            if filename == _srcfile:
+                f = f.f_back
+                continue
+
+            return co.co_filename, f.f_lineno, co.co_name
+
+        return '(unknown file)', 0, '(unknown function)'
+
+
+    def _log(self, level, msg, *args, **kwargs):
+        if not self._enabled(level): return
+
+        if not 'where' in kwargs:
+            filename, line, func = self._find_caller()
+            kwargs['where'] = '%s:%d' % (os.path.basename(filename), line)
+
+        if len(args): msg %= args
+
+        self.log._log(msg, level = level, prefix = self.name, **kwargs)
+
+
+    def debug  (self, *args, **kwargs): self._log(DEBUG,   *args, **kwargs)
+    def message(self, *args, **kwargs): self._log(MESSAGE, *args, **kwargs)
+    def info   (self, *args, **kwargs): self._log(INFO,    *args, **kwargs)
+    def warning(self, *args, **kwargs): self._log(WARNING, *args, **kwargs)
+    def error  (self, *args, **kwargs): self._log(ERROR,   *args, **kwargs)
+
+
+    def exception(self, *args, **kwargs):
+        msg = traceback.format_exc()
+        if len(args): msg = args[0] % args[1:] + '\n' + msg
+        self._log(ERROR, msg, **kwargs)
+
+
+class Log(object):
+    def __init__(self, args, ioloop, path):
+        self.path = path
+        self.listeners = []
+        self.loggers = {}
+
+        self.level = DEBUG if args.verbose else INFO
+
+        # Open log, rotate if necessary
+        self.f = None
+        self._open()
+
+        # Log header
+        version = pkg_resources.require('bbctrl')[0].version
+        self._log('Log started v%s' % version)
+        self._log_time(ioloop)
+
+
+    def get_path(self): return self.path
+
+    def add_listener(self, listener): self.listeners.append(listener)
+    def remove_listener(self, listener): self.listeners.remove(listener)
+
+
+    def get(self, name, level = None):
+        if not name in self.loggers:
+            self.loggers[name] = Logger(self, name, self.level)
+        return self.loggers[name]
+
+
+    def _log_time(self, ioloop):
+        self._log(datetime.datetime.now().strftime('%Y/%m/%d %H:%M:%S'))
+        ioloop.call_later(60 * 60, self._log_time, ioloop)
+
+
+    def broadcast(self, msg):
+        for listener in self.listeners: listener(msg)
+
+
+    def _log(self, msg, level = INFO, prefix = '', where = None):
+        if not msg: return
+
+        hdr = '%s:%s:' % ('DIMWE'[level], prefix)
+        s = hdr + ('\n' + hdr).join(msg.split('\n'))
+
+        if self.f is not None:
+            if 1e22 <= self.bytes_written + len(s) + 1: self._open()
+            self.f.write(s + '\n')
+            self.f.flush()
+            self.bytes_written += len(s) + 1
+
+        print(s)
+
+        # Broadcast to log listeners
+        if level == INFO: return
+
+        msg = dict(level = get_level_name(level), source = prefix, msg = msg)
+        if where is not None: msg['where'] = where
+
+        self.broadcast(dict(log = msg))
+
+
+    def _open(self):
+        if self.path is None: return
+        if self.f is not None: self.f.close()
+        self._rotate(self.path)
+        self.f = open(self.path, 'w')
+        self.bytes_written = 0
+
+
+    def _rotate(self, path, n = None):
+        fullpath = '%s.%d' % (path, n) if n is not None else path
+        nextN = (0 if n is None else n) + 1
+
+        if os.path.exists(fullpath):
+            if n == 16: os.unlink(fullpath)
+            else: self._rotate(path, nextN)
+
+            os.rename(fullpath, '%s.%d' % (path, nextN))

src/py/bbctrl/Mach.py

@@ -0,0 +1,351 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import bbctrl
+from bbctrl.Comm import Comm
+import bbctrl.Cmd as Cmd
+
+
+# Axis homing procedure:
+#
+#   Mark axis unhomed
+#   Set feed rate to search_vel
+#   Seek closed by search_dist
+#   Set feed rate to latch_vel
+#   Seek open latch_backoff
+#   Seek closed latch_backoff * -1.5
+#   Rapid to zero_backoff
+#   Mark axis homed and set absolute position
+
+axis_homing_procedure = '''
+  G28.2 %(axis)s0 F[#<_%(axis)s_search_velocity>]
+  G38.6 %(axis)s[#<_%(axis)s_home_travel>]
+  G38.8 %(axis)s[#<_%(axis)s_latch_backoff>] F[#<_%(axis)s_latch_velocity>]
+  G38.6 %(axis)s[#<_%(axis)s_latch_backoff> * -8]
+  G91 G0 G53 %(axis)s[#<_%(axis)s_zero_backoff>]
+  G90 G28.3 %(axis)s[#<_%(axis)s_home_position>]
+'''
+
+stall_homing_procedure = '''
+  G28.2 %(axis)s0 F[#<_%(axis)s_search_velocity>]
+  G38.6 %(axis)s[#<_%(axis)s_home_travel>]
+  G91 G1 G53 %(axis)s[#<_%(axis)s_zero_backoff>] F100
+  G90 G28.3 %(axis)s[#<_%(axis)s_home_position>]
+'''
+
+motor_fault_error = '''\
+Motor %d driver fault.  A potentially damaging electrical condition was \
+detected and the motor driver was shutdown.  Please power down the controller \
+and check your motor cabling. See the "Motor Faults" table on the "Indicators" \
+for more information.\
+'''
+
+def overrides(interface_class):
+    def overrider(method):
+        if not method.__name__ in dir(interface_class):
+            raise Exception('%s does not override %s' % (
+                method.__name__, interface_class.__name__))
+
+        return method
+
+    return overrider
+
+
+class Mach(Comm):
+    def __init__(self, ctrl, avr):
+        super().__init__(ctrl, avr)
+
+        self.ctrl = ctrl
+        self.mlog = self.ctrl.log.get('Mach')
+
+        self.planner = bbctrl.Planner(ctrl)
+        self.unpausing = False
+
+        ctrl.state.set('cycle', 'idle')
+
+        ctrl.state.add_listener(self._update)
+
+        super().reboot()
+
+
+    def _get_state(self): return self.ctrl.state.get('xx', '')
+    def _is_estopped(self): return self._get_state() == 'ESTOPPED'
+    def _is_holding(self): return self._get_state() == 'HOLDING'
+    def _is_ready(self): return self._get_state() == 'READY'
+    def _get_pause_reason(self): return self.ctrl.state.get('pr', '')
+    def _get_cycle(self): return self.ctrl.state.get('cycle', 'idle')
+
+
+    def _is_paused(self):
+        if not self._is_holding() or self.unpausing: return False
+        return self._get_pause_reason() in (
+            'User pause', 'Program pause', 'Optional pause')
+
+
+    def _set_cycle(self, cycle): self.ctrl.state.set('cycle', cycle)
+
+
+    def _begin_cycle(self, cycle):
+        current = self._get_cycle()
+        if current == cycle: return # No change
+
+        if current != 'idle':
+            raise Exception('Cannot enter %s cycle while in %s cycle' %
+                            (cycle, current))
+
+        # TODO handle jogging during pause
+        # if current == 'idle' or (cycle == 'jogging' and self._is_paused()):
+        self._set_cycle(cycle)
+
+
+    def _update(self, update):
+        # Detect motor faults
+        for motor in range(4):
+            key = '%ddf' % motor
+            if key in update and update[key] & 0x1f:
+                self.mlog.error(motor_fault_error % motor)
+
+        # Get state
+        state_changed = 'xc' in update
+        state = self._get_state()
+
+        # Handle EStop
+        if state_changed and state == 'ESTOPPED': self.planner.reset(False)
+
+        # Exit cycle if state changed to READY
+        if (state_changed and self._get_cycle() != 'idle' and
+            self._is_ready() and not self.planner.is_busy() and
+            not super().is_active()):
+            self.planner.position_change()
+            self._set_cycle('idle')
+
+        # Unpause sync
+        if state_changed and state != 'HOLDING': self.unpausing = False
+
+        # Entering HOLDING state
+        if state_changed and state == 'HOLDING':
+            # Always flush queue after pause
+            super().i2c_command(Cmd.FLUSH)
+            super().resume()
+
+        # Automatically unpause after seek or stop hold
+        # Must be after holding commands above
+        op = self.ctrl.state.get('optional_pause', False)
+        pr = self._get_pause_reason()
+        if ((state_changed or 'pr' in update) and self._is_holding() and
+            (pr in ('Switch found', 'User stop') or
+             (pr == 'Optional pause' and not op))):
+            self._unpause()
+
+
+    def _unpause(self):
+        pause_reason = self._get_pause_reason()
+        self.mlog.info('Unpause: ' + pause_reason)
+
+        if pause_reason == 'User stop':
+            self.planner.stop()
+            self.ctrl.state.set('line', 0)
+
+        else: self.planner.restart()
+
+        super().i2c_command(Cmd.UNPAUSE)
+        self.unpausing = True
+
+
+    def _reset(self): self.planner.reset()
+
+
+    def _i2c_block(self, block):
+        super().i2c_command(block[0], block = block[1:])
+
+
+    def _i2c_set(self, name, value): self._i2c_block(Cmd.set(name, value))
+
+
+    @overrides(Comm)
+    def comm_next(self):
+        if self.planner.is_running() and not self._is_holding():
+            return self.planner.next()
+
+
+    @overrides(Comm)
+    def comm_error(self): self._reset()
+
+
+    @overrides(Comm)
+    def connect(self):
+        self._reset()
+        super().connect()
+
+
+    def _query_var(self, cmd):
+        equal = cmd.find('=')
+        if equal == -1:
+            self.mlog.info('%s=%s' % (cmd, self.ctrl.state.get(cmd[1:])))
+
+        else:
+            name, value = cmd[1:equal], cmd[equal + 1:]
+
+            if value.lower() == 'true': value = True
+            elif value.lower() == 'false': value = False
+            else:
+                try:
+                    value = float(value)
+                except: pass
+
+            self.ctrl.state.config(name, value)
+
+
+    def mdi(self, cmd, with_limits = True):
+        if not len(cmd): return
+        if   cmd[0] == '$':  self._query_var(cmd)
+        elif cmd[0] == '\\': super().queue_command(cmd[1:])
+        else:
+            self._begin_cycle('mdi')
+            self.planner.mdi(cmd, with_limits)
+            super().resume()
+
+
+    def set(self, code, value):
+        super().queue_command('${}={}'.format(code, value))
+
+
+    def jog(self, axes):
+        self._begin_cycle('jogging')
+        self.planner.position_change()
+        super().queue_command(Cmd.jog(axes))
+
+
+    def home(self, axis, position = None):
+        state = self.ctrl.state
+
+        if axis is None: axes = 'zxyabc' # TODO This should be configurable
+        else: axes = '%c' % axis
+
+        for axis in axes:
+            enabled = state.is_axis_enabled(axis)
+            mode = state.axis_homing_mode(axis)
+
+            # If this is not a request to home a specific axis and the
+            # axis is disabled or in manual homing mode, don't show any
+            # warnings
+            if 1 < len(axes) and (not enabled or mode == 'manual'):
+                continue
+
+            # Error when axes cannot be homed
+            reason = state.axis_home_fail_reason(axis)
+            if reason is not None:
+                self.mlog.error('Cannot home %s axis: %s' % (
+                    axis.upper(), reason))
+                continue
+
+            if mode == 'manual':
+                if position is None: raise Exception('Position not set')
+                self.mdi('G28.3 %c%f' % (axis, position))
+                continue
+
+            # Home axis
+            self.mlog.info('Homing %s axis' % axis)
+            self._begin_cycle('homing')
+            if mode.startswith('stall-'): procedure = stall_homing_procedure
+            else: procedure = axis_homing_procedure
+            self.planner.mdi(procedure % {'axis': axis}, False)
+
+            # self.planner.mdi(axis_homing_procedure % {'axis': axis}, False)
+            super().resume()
+
+
+    def unhome(self, axis): self.mdi('G28.2 %c0' % axis)
+    def estop(self): super().estop()
+
+
+    def clear(self):
+        if self._is_estopped():
+            self._reset()
+            super().clear()
+
+
+    def start(self):
+        filename = self.ctrl.state.get('selected', '')
+        if not filename: return
+        self._begin_cycle('running')
+        self.planner.load(filename)
+        super().resume()
+
+
+    def step(self):
+        raise Exception('NYI') # TODO
+        if self._get_cycle() != 'running': self.start()
+        else: super().i2c_command(Cmd.UNPAUSE)
+
+
+    def stop(self): super().i2c_command(Cmd.STOP)
+    def pause(self): super().pause()
+
+
+    def unpause(self):
+        if self._is_paused():
+            self.ctrl.state.set('optional_pause', False)
+            self._unpause()
+
+
+    def optional_pause(self, enable = True):
+        self.ctrl.state.set('optional_pause', enable)
+
+
+    def set_position(self, axis, position):
+        axis = axis.lower()
+        state = self.ctrl.state
+
+        if state.is_axis_homed(axis):
+            # If homed, change the offset rather than the absolute position
+            self.mdi('G92%s%f' % (axis, position))
+
+        elif state.is_axis_enabled(axis):
+            if self._get_cycle() != 'idle' and not self._is_paused():
+                raise Exception('Cannot set position during ' +
+                                self._get_cycle())
+
+            # Set the absolute position both locally and via the AVR
+            target = position + state.get('offset_' + axis)
+            state.set(axis + 'p', target)
+            super().queue_command(Cmd.set_axis(axis, target))
+
+
+    def override_feed(self, override):
+        self._i2c_set('fo', int(1000 * override))
+
+
+    def override_speed(self, override):
+        self._i2c_set('so', int(1000 * override))
+
+
+    def modbus_read(self, addr): self._i2c_block(Cmd.modbus_read(addr))
+
+
+    def modbus_write(self, addr, value):
+        self._i2c_block(Cmd.modbus_write(addr, value))

src/py/bbctrl/MainLCDPage.py

@@ -0,0 +1,76 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import bbctrl
+
+
+class MainLCDPage(bbctrl.LCDPage):
+    def __init__(self, ctrl):
+        bbctrl.LCDPage.__init__(self, ctrl.lcd)
+
+        self.ctrl = ctrl
+        self.install = True
+
+        ctrl.state.add_listener(self.update)
+
+
+    def update(self, update):
+        state = self.ctrl.state
+
+        # Must be after machine vars have loaded
+        if self.install and hasattr(self, 'id'):
+            self.install = False
+            self.ctrl.lcd.set_current_page(self.id)
+
+        self.text('%-9s' % state.get('xx', ''), 0, 0)
+
+        metric = not state.get('imperial', False)
+        scale = 1 if metric else 25.4
+
+        # Show enabled axes
+        row = 0
+        for axis in 'xyzabc':
+            if state.is_axis_faulted(axis):
+                self.text('    FAULT %s' % axis.upper(), 9, row)
+                row += 1
+
+            elif state.is_axis_enabled(axis):
+                position = state.get(axis + 'p', 0)
+                position += state.get('offset_' + axis, 0)
+                position /= scale
+                self.text('% 10.3f%s' % (position, axis.upper()), 9, row)
+                row += 1
+
+        while row < 4:
+            self.text(' ' * 11, 9, row)
+            row += 1
+
+        # Show tool, units, feed and speed
+        self.text('%2uT' % state.get('tool',  0),           6, 1)
+        self.text('%-6s' % 'MM' if metric else 'INCH',      0, 1)
+        self.text('%8uF' % (state.get('feed',  0) / scale), 0, 2)
+        self.text('%8dS' % state.get('speed', 0),           0, 3)

src/py/bbctrl/MonitorTemp.py

@@ -0,0 +1,104 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import time
+
+
+def read_temp():
+    with open('/sys/class/thermal/thermal_zone0/temp', 'r') as f:
+        return round(int(f.read()) / 1000)
+
+
+def set_max_freq(freq):
+    filename = '/sys/devices/system/cpu/cpu0/cpufreq/scaling_max_freq'
+    with open(filename, 'w') as f: f.write('%d\n' % freq)
+
+
+class MonitorTemp(object):
+    def __init__(self, app):
+        self.app = app
+
+        self.ctrl = app.get_ctrl()
+        self.log = self.ctrl.log.get('Mon')
+        self.ioloop = self.ctrl.ioloop
+
+        self.last_temp_warn = 0
+        self.temp_thresh = 80
+        self.min_temp = 60
+        self.max_temp = 80
+        self.min_freq = 600000
+        self.max_freq = 1200000
+        self.low_camera_temp = 75
+        self.high_camera_temp = 80
+
+        self.callback()
+
+
+    # Scale max CPU based on temperature
+    def scale_cpu(self, temp):
+        if temp < self.min_temp: cpu_freq = self.max_freq
+        elif self.max_temp < temp: cpu_freq = self.min_freq
+        else:
+            r = 1 - float(temp - self.min_temp) / \
+                (self.max_temp - self.min_temp)
+            cpu_freq = self.min_freq + (self.max_freq - self.min_freq) * r
+
+        set_max_freq(cpu_freq)
+
+
+    def update_camera(self, temp):
+        if self.app.camera is None: return
+
+        # Disable camera if temp too high
+        if temp < self.low_camera_temp: self.app.camera.set_overtemp(False)
+        elif self.high_camera_temp < temp:
+            self.app.camera.set_overtemp(True)
+
+
+    def log_warnings(self, temp):
+        # Reset temperature warning threshold after timeout
+        if time.time() < self.last_temp_warn + 60: self.temp_thresh = 80
+
+        if self.temp_thresh < temp:
+            self.last_temp_warn = time.time()
+            self.temp_thresh = temp
+
+            self.log.info('Hot RaspberryPi at %d°C' % temp)
+
+
+    def callback(self):
+        try:
+            temp = read_temp()
+
+            self.ctrl.state.set('rpi_temp', temp)
+            self.scale_cpu(temp)
+            self.update_camera(temp)
+            self.log_warnings(temp)
+
+        except: self.log.exception()
+
+        self.ioloop.call_later(5, self.callback)

src/py/bbctrl/Planner.py

@@ -0,0 +1,388 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import json
+import math
+import re
+import time
+from collections import deque
+import camotics.gplan as gplan # pylint: disable=no-name-in-module,import-error
+import bbctrl.Cmd as Cmd
+from bbctrl.CommandQueue import CommandQueue
+
+
+reLogLine = re.compile(
+    r'^(?P<level>[A-Z])[0-9 ]:'
+    r'((?P<file>[^:]+):)?'
+    r'((?P<line>\d+):)?'
+    r'((?P<column>\d+):)?'
+    r'(?P<msg>.*)$')
+
+
+def log_floats(o):
+    if isinstance(o, float): return round(o, 2)
+    if isinstance(o, dict): return {k: log_floats(v) for k, v in o.items()}
+    if isinstance(o, (list, tuple)): return [log_floats(x) for x in o]
+    return o
+
+
+def log_json(o): return json.dumps(log_floats(o))
+
+
+class Planner():
+    def __init__(self, ctrl):
+        self.ctrl = ctrl
+        self.log = ctrl.log.get('Planner')
+        self.cmdq = CommandQueue(ctrl)
+        self.planner = None
+        self._position_dirty = False
+        self.where = ''
+
+        ctrl.state.add_listener(self._update)
+
+        self.reset(False)
+        self._report_time()
+
+
+    def is_busy(self): return self.is_running() or self.cmdq.is_active()
+    def is_running(self): return self.planner.is_running()
+    def position_change(self): self._position_dirty = True
+
+
+    def _sync_position(self, force = False):
+        if not force and not self._position_dirty: return
+        self._position_dirty = False
+        self.planner.set_position(self.ctrl.state.get_position())
+
+
+    def get_config(self, mdi, with_limits):
+        state = self.ctrl.state
+        config = self.ctrl.config
+
+        cfg = {
+            # NOTE Must get current units not configured default units
+            'default-units': 'METRIC' if state.get('metric') else 'IMPERIAL',
+            'max-vel':   state.get_axis_vector('vm', 1000),
+            'max-accel': state.get_axis_vector('am', 1000000),
+            'max-jerk':  state.get_axis_vector('jm', 1000000),
+            'rapid-auto-off':  config.get('rapid-auto-off'),
+            'max-blend-error': config.get('max-deviation'),
+            'max-merge-error': config.get('max-deviation'),
+            'junction-accel':  config.get('junction-accel'),
+            }
+
+        if with_limits:
+            minLimit = state.get_soft_limit_vector('tn', -math.inf)
+            maxLimit = state.get_soft_limit_vector('tm', math.inf)
+
+            # If max <= min then no limit
+            for axis in 'xyzabc':
+                if maxLimit[axis] <= minLimit[axis]:
+                    minLimit[axis], maxLimit[axis] = -math.inf, math.inf
+
+            cfg['min-soft-limit'] = minLimit
+            cfg['max-soft-limit'] = maxLimit
+
+        if not mdi:
+            program_start = config.get('program-start')
+            if program_start: cfg['program-start'] = program_start
+
+        overrides = {}
+
+        tool_change = config.get('tool-change')
+        if tool_change: overrides['M6'] = tool_change
+
+        program_end = config.get('program-end')
+        if program_end: overrides['M2'] = program_end
+
+        if overrides: cfg['overrides'] = overrides
+
+        self.log.info('Config:' + log_json(cfg))
+
+        return cfg
+
+
+    def _update(self, update):
+        if 'id' in update:
+            id = update['id']
+            self.planner.set_active(id) # Release planner commands
+            self.cmdq.release(id)       # Synchronize planner variables
+
+
+    def _get_var_cb(self, name, units):
+        value = 0
+
+        if len(name) and name[0] == '_':
+            value = self.ctrl.state.get(name[1:], 0)
+            try:
+                float(value)
+                if units == 'IMPERIAL': value /= 25.4 # Assume metric
+            except ValueError: value = 0
+
+        self.log.info('Get: %s=%s (units=%s)' % (name, value, units))
+
+        return value
+
+
+    def _log_cb(self, line):
+        line = line.strip()
+        m = reLogLine.match(line)
+        if not m: return
+
+        level    = m.group('level')
+        msg      = m.group('msg')
+        filename = m.group('file')
+        line     = m.group('line')
+        column   = m.group('column')
+
+        where = ':'.join(filter(None.__ne__, [filename, line, column]))
+
+        if line is not None: line = int(line)
+        if column is not None: column = int(column)
+
+        if   level == 'I': self.log.info    (msg, where = where)
+        elif level == 'D': self.log.debug   (msg, where = where)
+        elif level == 'W': self.log.warning (msg, where = where)
+        elif level == 'E': self.log.error   (msg, where = where)
+        else: self.log.error('Could not parse planner log line: ' + line)
+
+
+    def _add_message(self, text):
+        self.ctrl.state.add_message(text)
+
+        line = self.ctrl.state.get('line', 0)
+        if 0 <= line: where = '%s:%d' % (self.where, line)
+        else: where = self.where
+
+        self.log.message(text, where = where)
+
+
+    def _enqueue_set_cmd(self, id, name, value):
+        self.log.info('set(#%d, %s, %s)', id, name, value)
+        self.cmdq.enqueue(id, self.ctrl.state.set, name, value)
+
+
+    def _report_time(self):
+        state = self.ctrl.state.get('xx', '')
+
+        if state in ('STOPPING', 'RUNNING') and self.move_start:
+            delta = time.time() - self.move_start
+            if self.move_time < delta: delta = self.move_time
+            plan_time = self.current_plan_time + delta
+
+            self.ctrl.state.set('plan_time', round(plan_time))
+
+        elif state != 'HOLDING': self.ctrl.state.set('plan_time', 0)
+
+        self.ctrl.ioloop.call_later(1, self._report_time)
+
+
+    def _plan_time_restart(self):
+        self.plan_time = self.ctrl.state.get('plan_time', 0)
+
+
+    def _update_time(self, plan_time, move_time):
+        self.current_plan_time = plan_time
+        self.move_time = move_time
+        self.move_start = time.time()
+
+
+    def _enqueue_line_time(self, block):
+        if block.get('first', False) or block.get('seeking', False): return
+
+        # Sum move times
+        move_time = sum(block['times']) / 1000 # To seconds
+
+        self.cmdq.enqueue(block['id'], self._update_time, self.plan_time,
+                          move_time)
+
+        self.plan_time += move_time
+
+
+    def _enqueue_dwell_time(self, block):
+        self.cmdq.enqueue(block['id'], self._update_time, self.plan_time,
+                          block['seconds'])
+        self.plan_time += block['seconds']
+
+
+    def __encode(self, block):
+        type, id = block['type'], block['id']
+
+        if type != 'set': self.log.info('Cmd:' + log_json(block))
+
+        if type == 'line':
+            self._enqueue_line_time(block)
+            return Cmd.line(block['target'], block['exit-vel'],
+                            block['max-accel'], block['max-jerk'],
+                            block['times'], block.get('speeds', []))
+
+        if type == 'set':
+            name, value = block['name'], block['value']
+
+            if name == 'message':
+                self.cmdq.enqueue(id, self._add_message, value)
+
+            if name in ['line', 'tool']: self._enqueue_set_cmd(id, name, value)
+
+            if name == 'speed':
+                self._enqueue_set_cmd(id, name, value)
+                return Cmd.speed(value)
+
+            if len(name) and name[0] == '_':
+                # Don't queue axis positions, can be triggered by new position
+                if len(name) != 2 or name[1] not in 'xyzabc':
+                    self._enqueue_set_cmd(id, name[1:], value)
+
+            if name == '_feed': # Must come after _enqueue_set_cmd() above
+                return Cmd.set_sync('if', 1 / value if value else 0)
+
+            if name[0:1] == '_' and name[1:2] in 'xyzabc':
+                if name[2:] == '_home': return Cmd.set_axis(name[1], value)
+
+                if name[2:] == '_homed':
+                    motor = self.ctrl.state.find_motor(name[1])
+                    if motor is not None:
+                        return Cmd.set_sync('%dh' % motor, value)
+
+            return
+
+        if type == 'input':
+            # TODO handle timeout
+            self.planner.synchronize(0) # TODO Fix this
+            return Cmd.input(block['port'], block['mode'], block['timeout'])
+
+        if type == 'output':
+            return Cmd.output(block['port'], int(float(block['value'])))
+
+        if type == 'dwell':
+            self._enqueue_dwell_time(block)
+            return Cmd.dwell(block['seconds'])
+
+        if type == 'pause': return Cmd.pause(block['pause-type'])
+
+        if type == 'seek':
+            sw = self.ctrl.state.get_switch_id(block['switch'])
+            return Cmd.seek(sw, block['active'], block['error'])
+
+        if type == 'end': return '' # Sends id
+
+        raise Exception('Unknown planner command "%s"' % type)
+
+
+    def _encode(self, block):
+        cmd = self.__encode(block)
+
+        if cmd is not None:
+            self.cmdq.enqueue(block['id'], None)
+            return Cmd.set_sync('id', block['id']) + '\n' + cmd
+
+
+    def reset_times(self):
+        self.move_start = 0
+        self.move_time = 0
+        self.plan_time = 0
+        self.current_plan_time = 0
+
+
+    def close(self):
+        # Release planner callbacks
+        if self.planner is not None:
+            self.planner.set_resolver(None)
+            self.planner.set_logger(None)
+
+
+    def reset(self, stop = True):
+        if stop: self.ctrl.mach.stop()
+        self.planner = gplan.Planner()
+        self.planner.set_resolver(self._get_var_cb)
+        # TODO logger is global and will not work correctly in demo mode
+        self.planner.set_logger(self._log_cb, 1, 'LinePlanner:3')
+        self._position_dirty = True
+        self.cmdq.clear()
+        self.reset_times()
+        self.ctrl.state.reset()
+
+
+    def mdi(self, cmd, with_limits = True):
+        self.where = '<mdi>'
+        self.log.info('MDI:' + cmd)
+        self._sync_position()
+        self.planner.load_string(cmd, self.get_config(True, with_limits))
+        self.reset_times()
+
+
+    def load(self, path):
+        self.where = path
+        path = self.ctrl.get_path('upload', path)
+        self.log.info('GCode:' + path)
+        self._sync_position()
+        self.planner.load(path, self.get_config(False, True))
+        self.reset_times()
+
+
+    def stop(self):
+        try:
+            self.planner.stop()
+            self.cmdq.clear()
+
+        except:
+            self.log.exception()
+            self.reset()
+
+
+    def restart(self):
+        try:
+            id = self.ctrl.state.get('id')
+            position = self.ctrl.state.get_position()
+
+            self.log.info('Planner restart: %d %s' % (id, log_json(position)))
+
+            self.cmdq.clear()
+            self.cmdq.release(id)
+            self._plan_time_restart()
+            self.planner.restart(id, position)
+
+        except:
+            self.log.exception()
+            self.stop()
+
+
+    def next(self):
+        try:
+            while self.planner.has_more():
+                cmd = self.planner.next()
+                cmd = self._encode(cmd)
+                if cmd is not None: return cmd
+
+        except RuntimeError as e:
+            # Pass on the planner message
+            self.log.error(str(e));
+            self.stop()
+
+        except:
+            self.log.exception()
+            self.stop()

src/py/bbctrl/Preplanner.py

@@ -0,0 +1,271 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import time
+import json
+import hashlib
+import glob
+import tempfile
+import signal
+from concurrent.futures import Future
+from tornado import gen, process, iostream
+import bbctrl
+
+
+def hash_dump(o):
+    s = json.dumps(o, separators = (',', ':'), sort_keys = True)
+    return s.encode('utf8')
+
+
+def plan_hash(path, config):
+    h = hashlib.sha256()
+    h.update('v4'.encode('utf8'))
+    h.update(hash_dump(config))
+
+    with open(path, 'rb') as f:
+        while True:
+            buf = f.read(1024 * 1024)
+            if not buf: break
+            h.update(buf)
+
+    return h.hexdigest()
+
+
+def safe_remove(path):
+    try:
+        os.unlink(path)
+    except: pass
+
+
+class Plan(object):
+    def __init__(self, preplanner, ctrl, filename):
+        self.preplanner = preplanner
+
+        # Copy planner state
+        self.state = ctrl.state.snapshot()
+        self.config = ctrl.mach.planner.get_config(False, False)
+        del self.config['default-units']
+
+        self.progress = 0
+        self.cancel = False
+        self.pid = None
+
+        root = ctrl.get_path()
+        self.gcode = '%s/upload/%s' % (root, filename)
+        self.base = '%s/plans/%s' % (root, filename)
+        self.hid = plan_hash(self.gcode, self.config)
+        fbase = '%s.%s.' % (self.base, self.hid)
+        self.files = [
+            fbase + 'json',
+            fbase + 'positions.gz',
+            fbase + 'speeds.gz']
+
+        self.future = Future()
+        ctrl.ioloop.add_callback(self._load)
+
+
+    def terminate(self):
+        if self.cancel: return
+        self.cancel = True
+        if self.pid is not None:
+            try:
+                os.kill(self.pid, signal.SIGKILL)
+            except: pass
+
+
+    def delete(self):
+        files = glob.glob(self.base + '.*')
+        for path in files: safe_remove(path)
+
+
+    def clean(self, max = 2):
+        plans = glob.glob(self.base + '.*.json')
+        if len(plans) <= max: return
+
+        # Delete oldest plans
+        plans = [(os.path.getmtime(path), path) for path in plans]
+        plans.sort()
+
+        for mtime, path in plans[:len(plans) - max]:
+            safe_remove(path)
+            safe_remove(path[:-4] + 'positions.gz')
+            safe_remove(path[:-4] + 'speeds.gz')
+
+
+    def _exists(self):
+        for path in self.files:
+            if not os.path.exists(path): return False
+        return True
+
+
+    def _read(self):
+        if self.cancel: return
+
+        try:
+            with open(self.files[0], 'r')  as f: meta = json.load(f)
+            with open(self.files[1], 'rb') as f: positions = f.read()
+            with open(self.files[2], 'rb') as f: speeds = f.read()
+
+            return meta, positions, speeds
+
+        except:
+            self.preplanner.log.exception()
+
+            # Clean
+            for path in self.files:
+                if os.path.exists(path):
+                    os.remove(path)
+
+
+    @gen.coroutine
+    def _exec(self):
+        self.clean() # Clean up old plans
+
+        with tempfile.TemporaryDirectory() as tmpdir:
+            cmd = (
+                '/usr/bin/env', 'python3',
+                bbctrl.get_resource('plan.py'),
+                os.path.abspath(self.gcode), json.dumps(self.state),
+                json.dumps(self.config),
+                '--max-time=%s' % self.preplanner.max_plan_time,
+                '--max-loop=%s' % self.preplanner.max_loop_time
+            )
+
+            self.preplanner.log.info('Running: %s', cmd)
+
+            proc = process.Subprocess(cmd, stdout = process.Subprocess.STREAM,
+                                      stderr = process.Subprocess.STREAM,
+                                      cwd = tmpdir)
+            errs = ''
+            self.pid = proc.proc.pid
+
+            try:
+                try:
+                    while True:
+                        line = yield proc.stdout.read_until(b'\n')
+                        self.progress = float(line.strip())
+                        if self.cancel: return
+                except iostream.StreamClosedError: pass
+
+                self.progress = 1
+
+                ret = yield proc.wait_for_exit(False)
+                if ret:
+                    errs = yield proc.stderr.read_until_close()
+                    raise Exception('Plan failed: ' + errs.decode('utf8'))
+
+            finally:
+                proc.stderr.close()
+                proc.stdout.close()
+
+            if not self.cancel:
+                os.rename(tmpdir + '/meta.json',    self.files[0])
+                os.rename(tmpdir + '/positions.gz', self.files[1])
+                os.rename(tmpdir + '/speeds.gz',    self.files[2])
+                os.sync()
+
+
+    @gen.coroutine
+    def _load(self):
+        try:
+            if self._exists():
+                data = self._read()
+                if data is not None:
+                    self.future.set_result(data)
+                    return
+
+            if not self._exists(): yield self._exec()
+            self.future.set_result(self._read())
+
+        except:
+            self.preplanner.log.exception()
+
+
+
+class Preplanner(object):
+    def __init__(self, ctrl, max_plan_time = 60 * 60 * 24, max_loop_time = 300):
+        self.ctrl = ctrl
+        self.log = ctrl.log.get('Preplanner')
+
+        self.max_plan_time = max_plan_time
+        self.max_loop_time = max_loop_time
+
+        path = self.ctrl.get_plan()
+        if not os.path.exists(path): os.mkdir(path)
+
+        self.started = Future()
+        self.plans = {}
+
+
+    def start(self):
+        if not self.started.done():
+            self.log.info('Preplanner started')
+            self.started.set_result(True)
+
+
+    def invalidate(self, filename):
+        if filename in self.plans:
+            self.plans[filename].terminate()
+            del self.plans[filename]
+
+
+    def invalidate_all(self):
+        for filename, plan in self.plans.items():
+            plan.terminate()
+        self.plans = {}
+
+
+    def delete_all_plans(self):
+        files = glob.glob(self.ctrl.get_plan('*'))
+        for path in files: safe_remove(path)
+        self.invalidate_all()
+
+
+    def delete_plans(self, filename):
+        if filename in self.plans:
+            self.plans[filename].delete()
+            self.invalidate(filename)
+
+    @gen.coroutine
+    def get_plan(self, filename):
+        if filename is None: raise Exception('Filename cannot be None')
+
+        # Wait until state is fully initialized
+        yield self.started
+
+        if filename in self.plans: plan = self.plans[filename]
+        else:
+            plan = Plan(self, self.ctrl, filename)
+            self.plans[filename] = plan
+
+        data = yield plan.future
+        return data
+
+
+    def get_plan_progress(self, filename):
+        return self.plans[filename].progress if filename in self.plans else 0

src/py/bbctrl/Pwr.py

@@ -0,0 +1,194 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import bbctrl
+import bbctrl.Cmd as Cmd
+
+
+# Must match regs in pwr firmware
+TEMP_REG        = 0
+VIN_REG         = 1
+VOUT_REG        = 2
+MOTOR_REG       = 3
+LOAD1_REG       = 4
+LOAD2_REG       = 5
+VDD_REG         = 6
+FLAGS_REG       = 7
+VERSION_REG     = 8
+
+# Must be kept in sync with pwr firmware
+UNDER_VOLTAGE_FLAG             = 1 << 0
+OVER_VOLTAGE_FLAG              = 1 << 1
+OVER_CURRENT_FLAG              = 1 << 2
+SENSE_ERROR_FLAG               = 1 << 3
+SHUNT_OVERLOAD_FLAG            = 1 << 4
+MOTOR_OVERLOAD_FLAG            = 1 << 5
+LOAD1_SHUTDOWN_FLAG            = 1 << 6
+LOAD2_SHUTDOWN_FLAG            = 1 << 7
+MOTOR_UNDER_VOLTAGE_FLAG       = 1 << 8
+MOTOR_VOLTAGE_SENSE_ERROR_FLAG = 1 << 9
+MOTOR_CURRENT_SENSE_ERROR_FLAG = 1 << 10
+LOAD1_SENSE_ERROR_FLAG         = 1 << 11
+LOAD2_SENSE_ERROR_FLAG         = 1 << 12
+VDD_CURRENT_SENSE_ERROR_FLAG   = 1 << 13
+POWER_SHUTDOWN_FLAG            = 1 << 14
+SHUNT_ERROR_FLAG               = 1 << 15
+
+reg_names = 'temp vin vout motor load1 load2 vdd pwr_flags pwr_version'.split()
+
+
+class Pwr():
+    def __init__(self, ctrl):
+        self.ctrl = ctrl
+        self.log = ctrl.log.get('Pwr')
+
+        self.i2c_addr = ctrl.args.pwr_addr
+        self.regs = [-1] * 9
+        self.lcd_page = ctrl.lcd.add_new_page()
+        self.failures = 0
+
+        self._update_cb(False)
+
+
+    def check_fault(self, var, status):
+        status = bool(status)
+
+        if not self.ctrl.state.has(var) or status != self.ctrl.state.get(var):
+            self.ctrl.state.set(var, status)
+            if status: return True
+
+        return False
+
+
+    def check_faults(self):
+        flags = self.regs[FLAGS_REG]
+
+        if self.check_fault('under_voltage', flags & UNDER_VOLTAGE_FLAG):
+            self.log.warning('Device under voltage')
+
+        if self.check_fault('over_voltage', flags & OVER_VOLTAGE_FLAG):
+            self.log.warning('Device over voltage')
+
+        if self.check_fault('over_current', flags & OVER_CURRENT_FLAG):
+            self.log.warning('Device total current limit exceeded')
+
+        if self.check_fault('sense_error', flags & SENSE_ERROR_FLAG):
+            self.log.warning('Power sense error')
+
+        if self.check_fault('shunt_overload', flags & SHUNT_OVERLOAD_FLAG):
+            self.log.warning('Power shunt overload')
+
+        if self.check_fault('motor_overload', flags & MOTOR_OVERLOAD_FLAG):
+            self.log.warning('Motor power overload')
+
+        if self.check_fault('load1_shutdown', flags & LOAD1_SHUTDOWN_FLAG):
+            self.log.warning('Load 1 over temperature shutdown')
+
+        if self.check_fault('load2_shutdown', flags & LOAD2_SHUTDOWN_FLAG):
+            self.log.warning('Load 2 over temperature shutdown')
+
+        if self.check_fault('motor_under_voltage',
+                            flags & MOTOR_UNDER_VOLTAGE_FLAG):
+            self.log.warning('Motor under voltage')
+
+        if self.check_fault('motor_voltage_sense_error',
+                            flags & MOTOR_VOLTAGE_SENSE_ERROR_FLAG):
+            self.log.warning('Motor voltage sense error')
+
+        if self.check_fault('motor_current_sense_error',
+                            flags & MOTOR_CURRENT_SENSE_ERROR_FLAG):
+            self.log.warning('Motor current sense error')
+
+        if self.check_fault('load1_sense_error',
+                            flags & LOAD1_SENSE_ERROR_FLAG):
+            self.log.warning('Load1 sense error')
+
+        if self.check_fault('load2_sense_error',
+                            flags & LOAD2_SENSE_ERROR_FLAG):
+            self.log.warning('Load2 sense error')
+
+        if self.check_fault('vdd_current_sense_error',
+                            flags & VDD_CURRENT_SENSE_ERROR_FLAG):
+            self.log.warning('Vdd current sense error')
+
+        if self.check_fault('power_shutdown', flags & POWER_SHUTDOWN_FLAG):
+            self.log.warning('Power shutdown')
+            self.ctrl.mach.i2c_command(Cmd.SHUTDOWN)
+
+        if self.check_fault('shunt_error', flags & SHUNT_ERROR_FLAG):
+            self.log.warning('Shunt error')
+
+
+    def _update_cb(self, now = True):
+        if now: self._update()
+        self.ctrl.ioloop.call_later(1, self._update_cb)
+
+
+    def _update(self):
+        update = {}
+
+        try:
+            for i in range(len(self.regs)):
+                value = self.ctrl.i2c.read_word(self.i2c_addr + i)
+                if value is None: return # Handle lack of i2c port
+
+                if i == TEMP_REG: value -= 273
+                elif i == FLAGS_REG or i == VERSION_REG: pass
+                else: value /= 100.0
+
+                key = reg_names[i]
+                self.ctrl.state.set(key, value)
+
+                if self.regs[i] != value:
+                    update[key] = value
+                    self.regs[i] = value
+
+                if i == FLAGS_REG: self.check_faults()
+
+        except Exception as e:
+            if i < 6: # Older pwr firmware does not have regs > 5
+                self.failures += 1
+                msg = 'Pwr communication failed at reg %d: %s' % (i, e)
+                if self.failures != 5: self.log.info(msg)
+                else:
+                    self.log.warning(msg)
+                    self.failures = 0
+                return
+
+        self.lcd_page.text('%3dC   Tmp' % self.regs[TEMP_REG],  0, 0)
+        self.lcd_page.text('%5.1fV  In' % self.regs[VIN_REG],   0, 1)
+        self.lcd_page.text('%5.1fV Out' % self.regs[VOUT_REG],  0, 2)
+        self.lcd_page.text(' %04x  Flg' % self.regs[FLAGS_REG], 0, 3)
+
+        self.lcd_page.text('%5.1fA Mot' % self.regs[MOTOR_REG], 10, 0)
+        self.lcd_page.text('%5.1fA Ld1' % self.regs[LOAD1_REG], 10, 1)
+        self.lcd_page.text('%5.1fA Ld2' % self.regs[LOAD2_REG], 10, 2)
+        self.lcd_page.text('%5.1fA Vdd' % self.regs[VDD_REG],   10, 3)
+
+        if len(update): self.ctrl.state.update(update)
+
+        self.failures = 0

src/py/bbctrl/RequestHandler.py

@@ -0,0 +1,63 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import traceback
+import bbctrl
+
+from tornado.web import HTTPError
+import tornado.web
+
+
+class RequestHandler(tornado.web.RequestHandler):
+    def __init__(self, app, request, **kwargs):
+        super().__init__(app, request, **kwargs)
+        self.app = app
+
+
+    def get_ctrl(self): return self.app.get_ctrl(self.get_cookie('client-id'))
+    def get_log(self, name = 'API'): return self.get_ctrl().log.get(name)
+
+
+    def get_path(self, path = None, filename = None):
+        return self.get_ctrl().get_path(path, filename)
+
+
+    def get_upload(self, filename = None):
+        return self.get_ctrl().get_upload(filename)
+
+
+    # Override exception logging
+    def log_exception(self, typ, value, tb):
+        if (isinstance(value, HTTPError) and
+            400 <= value.status_code and value.status_code < 500): return
+
+        log = self.get_log()
+        log.set_level(bbctrl.log.DEBUG)
+
+        log.error(str(value))
+        trace = ''.join(traceback.format_exception(typ, value, tb))
+        log.debug(trace)

src/py/bbctrl/State.py

@@ -0,0 +1,450 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import traceback
+import copy
+import uuid
+import os
+import bbctrl
+
+
+class State(object):
+    def __init__(self, ctrl):
+        self.ctrl = ctrl
+        self.log = ctrl.log.get('State')
+
+        self.callbacks = {}
+        self.changes = {}
+        self.listeners = []
+        self.timeout = None
+        self.machine_var_set = set()
+        self.message_id = 0
+
+        # Defaults
+        self.vars = {
+            'line': -1,
+            'messages': [],
+            'tool': 0,
+            'feed': 0,
+            'speed': 0,
+            'sid': str(uuid.uuid4()),
+            'demo': ctrl.args.demo,
+        }
+
+        # Add computed variable callbacks for each motor.
+        #
+        # NOTE, variable callbacks must return metric values only because
+        # the planner will scale returned values when in imperial mode.
+        for i in range(4):
+            self.set_callback(str(i) + 'home_position',
+                              lambda name, i = i: self.motor_home_position(i))
+            self.set_callback(str(i) + 'home_travel',
+                              lambda name, i = i: self.motor_home_travel(i))
+            self.set_callback(str(i) + 'latch_backoff',
+                              lambda name, i = i: self.motor_latch_backoff(i))
+            self.set_callback(str(i) + 'zero_backoff',
+                              lambda name, i = i: self.motor_zero_backoff(i))
+            self.set_callback(str(i) + 'search_velocity',
+                              lambda name, i = i: self.motor_search_velocity(i))
+            self.set_callback(str(i) + 'latch_velocity',
+                              lambda name, i = i: self.motor_latch_velocity(i))
+
+        self.set_callback('metric', lambda name: 1 if self.is_metric() else 0)
+        self.set_callback('imperial', lambda name: 0 if self.is_metric() else 1)
+
+        self.reset()
+        self.load_files()
+
+
+    def is_metric(self): return self.get('units', 'METRIC') == 'METRIC'
+
+
+    def reset(self):
+        # Unhome all motors
+        for i in range(4): self.set('%dhomed' % i, False)
+
+        # Zero offsets and positions
+        for axis in 'xyzabc':
+            self.set(axis + 'p', 0)
+            self.set('offset_' + axis, 0)
+
+
+    def load_files(self):
+        self.files = []
+
+        upload = self.ctrl.get_upload()
+
+        if not os.path.exists(upload):
+            os.mkdir(upload)
+            from shutil import copy
+            copy(bbctrl.get_resource('http/buildbotics.nc'), upload)
+
+        for path in os.listdir(upload):
+            if os.path.isfile(upload + '/' + path):
+                self.files.append(path)
+
+        self.files.sort()
+        self.set('files', self.files)
+
+        if len(self.files): self.select_file(self.files[0])
+        else: self.select_file('')
+
+
+    def clear_files(self):
+        self.select_file('')
+        self.files = []
+        self.changes['files'] = self.files
+
+
+    def add_file(self, filename):
+        if not filename in self.files:
+            self.files.append(filename)
+            self.files.sort()
+            self.changes['files'] = self.files
+
+        self.select_file(filename)
+
+
+    def remove_file(self, filename):
+        if filename in self.files:
+            self.files.remove(filename)
+            self.changes['files'] = self.files
+
+        if self.get('selected', filename) == filename:
+            if len(self.files): self.select_file(self.files[0])
+            else: self.select_file('')
+
+
+    def select_file(self, filename):
+        self.set('selected', filename)
+        time = os.path.getmtime(self.ctrl.get_upload(filename))
+        self.set('selected_time', time)
+
+
+    def set_bounds(self, bounds):
+        for axis in 'xyzabc':
+            for name in ('min', 'max'):
+                var = '%s_%s' % (axis, name)
+                value = bounds[name][axis] if axis in bounds[name] else 0
+                self.set(var, value)
+
+
+    def ack_message(self, id):
+        self.log.info('Message %d acknowledged' % id)
+        msgs = self.vars['messages']
+        msgs = list(filter(lambda m: id < m['id'], msgs))
+        self.set('messages', msgs)
+
+
+    def add_message(self, text):
+        msg = dict(text = text, id = self.message_id)
+        self.message_id += 1
+        msgs = self.vars['messages']
+        msgs = msgs + [msg] # It's important we make a new list here
+        self.set('messages', msgs)
+
+
+    def _notify(self):
+        if not self.changes: return
+
+        for listener in self.listeners:
+            try:
+                listener(self.changes)
+
+            except Exception as e:
+                self.log.warning('Updating state listener: %s',
+                            traceback.format_exc())
+
+        self.changes = {}
+        self.timeout = None
+
+
+    def resolve(self, name):
+        # Resolve axis prefixes to motor numbers
+        if 2 < len(name) and name[1] == '_' and name[0] in 'xyzabc':
+            motor = self.find_motor(name[0])
+            if motor is not None: return str(motor) + name[2:]
+
+        return name
+
+
+    def has(self, name): return self.resolve(name) in self.vars
+    def set_callback(self, name, cb): self.callbacks[self.resolve(name)] = cb
+
+
+    def set(self, name, value):
+        name = self.resolve(name)
+
+        if not name in self.vars or self.vars[name] != value:
+            self.vars[name] = value
+            self.changes[name] = value
+
+            # Trigger listener notify
+            if self.timeout is None:
+                self.timeout = self.ctrl.ioloop.call_later(0.25, self._notify)
+
+
+    def update(self, update):
+        for name, value in update.items():
+            self.set(name, value)
+
+
+    def get(self, name, default = None):
+        name = self.resolve(name)
+
+        if name in self.vars: return self.vars[name]
+        if name in self.callbacks: return self.callbacks[name](name)
+        if default is None:
+            self.log.warning('State variable "%s" not found' % name)
+
+        return default
+
+
+    def snapshot(self):
+        vars = copy.deepcopy(self.vars)
+
+        for name in self.callbacks:
+            if not name in vars:
+                vars[name] = self.callbacks[name](name)
+
+        axis_motors = {axis: self.find_motor(axis) for axis in 'xyzabc'}
+        axis_vars = {}
+
+        for name, value in vars.items():
+            if name[0] in '0123':
+                motor = int(name[0])
+
+                for axis in 'xyzabc':
+                    if motor == axis_motors[axis]:
+                        axis_vars[axis + '_' + name[1:]] = value
+
+        vars.update(axis_vars)
+
+        return vars
+
+
+    def config(self, code, value):
+        # Set machine variables via mach, others directly
+        if code in self.machine_var_set: self.ctrl.mach.set(code, value)
+        else: self.set(code, value)
+
+
+    def add_listener(self, listener):
+        self.listeners.append(listener)
+        listener(self.vars)
+
+
+    def remove_listener(self, listener): self.listeners.remove(listener)
+
+
+    def set_machine_vars(self, vars):
+        # Record all machine vars, indexed or otherwise
+        self.machine_var_set = set()
+        for code, spec in vars.items():
+            if 'index' in spec:
+                for index in spec['index']:
+                    self.machine_var_set.add(index + code)
+            else: self.machine_var_set.add(code)
+
+
+    def get_position(self):
+        position = {}
+
+        for axis in 'xyzabc':
+            if self.is_axis_enabled(axis) and self.has(axis + 'p'):
+                position[axis] = self.get(axis + 'p')
+
+        return position
+
+
+    def get_axis_vector(self, name, scale = 1):
+        v = {}
+
+        for axis in 'xyzabc':
+            motor = self.find_motor(axis)
+
+            if motor is not None and self.motor_enabled(motor):
+                value = self.get(str(motor) + name, None)
+                if value is not None: v[axis] = value * scale
+
+        return v
+
+
+    def get_soft_limit_vector(self, var, default):
+        limit = self.get_axis_vector(var, 1)
+
+        for axis in 'xyzabc':
+            if not axis in limit or not self.is_axis_homed(axis):
+                limit[axis] = default
+
+        return limit
+
+
+    def find_motor(self, axis):
+        for motor in range(4):
+            if not ('%dan' % motor) in self.vars: continue
+            motor_axis = 'xyzabc'[self.vars['%dan' % motor]]
+            if motor_axis == axis.lower() and self.vars.get('%dme' % motor, 0):
+                return motor
+
+
+    def is_axis_homed(self, axis): return self.get('%s_homed' % axis, False)
+
+
+    def is_axis_enabled(self, axis):
+        motor = self.find_motor(axis)
+        return motor is not None and self.motor_enabled(motor)
+
+
+    def get_enabled_axes(self):
+        axes = []
+
+        for axis in 'xyzabc':
+            if self.is_axis_enabled(axis):
+                axes.append(axis)
+
+        return axes
+
+
+    def is_motor_faulted(self, motor):
+        return self.get('%ddf' % motor, 0) & 0x1f
+
+
+    def is_axis_faulted(self, axis):
+        motor = self.find_motor(axis)
+        return motor is not None and self.is_motor_faulted(motor)
+
+
+    def axis_homing_mode(self, axis):
+        motor = self.find_motor(axis)
+        if motor is None: return 'disabled'
+        return self.motor_homing_mode(motor)
+
+
+    def axis_home_fail_reason(self, axis):
+        motor = self.find_motor(axis)
+        if motor is None: return 'Not mapped to motor'
+        if not self.motor_enabled(motor): return 'Motor disabled'
+
+        mode = self.motor_homing_mode(motor)
+
+        if mode != 'manual':
+            if mode == 'switch-min' and not int(self.get(axis + '_ls', 0)):
+                return 'Configured for min switch but switch is disabled'
+
+            if mode == 'switch-max' and not int(self.get(axis + '_xs', 0)):
+                return 'Configured for max switch but switch is disabled'
+
+        softMin = int(self.get(axis + '_tn', 0))
+        softMax = int(self.get(axis + '_tm', 0))
+        if softMax <= softMin + 1:
+            return 'max-soft-limit must be at least 1mm greater ' \
+                'than min-soft-limit'
+
+
+    def motor_enabled(self, motor):
+        return bool(int(self.vars.get('%dme' % motor, 0)))
+
+
+    def motor_homing_mode(self, motor):
+        mode = str(self.vars.get('%dho' % motor, 0))
+        if mode == '0': return 'manual'
+        if mode == '1': return 'switch-min'
+        if mode == '2': return 'switch-max'
+        if mode == '3': return 'stall-min'
+        if mode == '4': return 'stall-max'
+        raise Exception('Unrecognized homing mode "%s"' % mode)
+
+
+    def motor_home_direction(self, motor):
+        mode = self.motor_homing_mode(motor)
+        if mode.endswith('-min'): return -1
+        if mode.endswith('-max'): return 1
+        return 0 # Disabled
+
+
+    def motor_home_position(self, motor):
+        mode = self.motor_homing_mode(motor)
+        # Return soft limit positions
+        if mode.endswith('-min'): return self.vars['%dtn' % motor]
+        if mode.endswith('-max'): return self.vars['%dtm' % motor]
+        return 0 # Disabled
+
+
+    def motor_home_travel(self, motor):
+        tmin = self.get(str(motor) + 'tm', 0)
+        tmax = self.get(str(motor) + 'tn', 0)
+        hdir = self.motor_home_direction(motor)
+
+        # (travel_max - travel_min) * 1.5 * home_dir
+        return (tmin - tmax) * 1.5 * hdir
+
+
+    def motor_latch_backoff(self, motor):
+        lb = self.get(str(motor) + 'lb', 0)
+        hdir = self.motor_home_direction(motor)
+        return -(lb * hdir) # -latch_backoff * home_dir
+
+
+    def motor_zero_backoff(self, motor):
+        zb = self.get(str(motor) + 'zb', 0)
+        hdir = self.motor_home_direction(motor)
+        return -(zb * hdir) # -zero_backoff * home_dir
+
+
+    def motor_search_velocity(self, motor):
+        return 1000 * self.get(str(motor) + 'sv', 0)
+
+
+    def motor_latch_velocity(self, motor):
+        return 1000 * self.get(str(motor) + 'lv', 0)
+
+
+    def get_axis_switch(self, axis, side):
+        axis = axis.lower()
+
+        if not axis in 'xyzabc':
+            raise Exception('Unsupported switch "%s-%s"' % (axis, side))
+
+        if not self.is_axis_enabled(axis):
+            raise Exception('Switch "%s-%s" axis not enabled' % (axis, side))
+
+        motor = self.find_motor(axis)
+
+        # This must match the switch ID enum in avr/src/switch.h
+        hmode = self.motor_homing_mode(motor)
+        if hmode.startswith('stall-'): return motor + 10
+        return 2 * motor + 2 + (0 if side.lower() == 'min' else 1)
+
+
+    def get_switch_id(self, switch):
+        # TODO Support other input switches in CAMotics gcode/machine/PortType.h
+        switch = switch.lower()
+        if switch == 'probe': return 1
+        if switch[1:] == '-min': return self.get_axis_switch(switch[0], 'min')
+        if switch[1:] == '-max': return self.get_axis_switch(switch[0], 'max')
+        raise Exception('Unsupported switch "%s"' % switch)

src/py/bbctrl/Web.py

@@ -0,0 +1,595 @@
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import sys
+import json
+import tornado
+import sockjs.tornado
+import datetime
+import shutil
+import tarfile
+import subprocess
+import socket
+import time
+from tornado.web import HTTPError
+from tornado import web, gen
+
+import bbctrl
+
+
+
+def call_get_output(cmd):
+    p = subprocess.Popen(cmd, stdout = subprocess.PIPE)
+    s = p.communicate()[0].decode('utf-8')
+    if p.returncode: raise HTTPError(400, 'Command failed')
+    return s
+
+
+def get_username():
+    return call_get_output(['getent', 'passwd', '1001']).split(':')[0]
+
+
+def set_username(username):
+    if subprocess.call(['usermod', '-l', username, get_username()]):
+        raise HTTPError(400, 'Failed to set username to "%s"' % username)
+
+
+def check_password(password):
+    # Get current password
+    s = call_get_output(['getent', 'shadow', get_username()])
+    current = s.split(':')[1].split('$')
+
+    # Check password type
+    if len(current) < 2 or current[1] != '1':
+        raise HTTPError(401, "Password invalid")
+
+    # Check current password
+    cmd = ['openssl', 'passwd', '-salt', current[2], '-1', password]
+    s = call_get_output(cmd).strip()
+
+    if s.split('$') != current: raise HTTPError(401, 'Wrong password')
+
+
+
+class RebootHandler(bbctrl.APIHandler):
+    def put_ok(self):
+        self.get_ctrl().lcd.goodbye('Rebooting...')
+        subprocess.Popen('reboot')
+
+
+class LogHandler(bbctrl.RequestHandler):
+    def get(self):
+        with open(self.get_ctrl().log.get_path(), 'r') as f:
+            self.write(f.read())
+
+
+    def set_default_headers(self):
+        fmt = socket.gethostname() + '-%Y%m%d.log'
+        filename = datetime.date.today().strftime(fmt)
+        self.set_header('Content-Disposition', 'filename="%s"' % filename)
+        self.set_header('Content-Type', 'text/plain')
+
+
+class MessageAckHandler(bbctrl.APIHandler):
+    def put_ok(self, id):
+        self.get_ctrl().state.ack_message(int(id))
+
+
+class BugReportHandler(bbctrl.RequestHandler):
+    def get(self):
+        import tarfile, io
+
+        buf = io.BytesIO()
+        tar = tarfile.open(mode = 'w:bz2', fileobj = buf)
+
+        def check_add(path, arcname = None):
+            if os.path.isfile(path):
+                if arcname is None: arcname = path
+                tar.add(path, self.basename + '/' + arcname)
+
+        def check_add_basename(path):
+            check_add(path, os.path.basename(path))
+
+        ctrl = self.get_ctrl()
+        path = ctrl.log.get_path()
+        check_add_basename(path)
+        for i in range(1, 8):
+            check_add_basename('%s.%d' % (path, i))
+        check_add_basename('/var/log/syslog')
+        check_add('config.json')
+        check_add(ctrl.get_upload(ctrl.state.get('selected', '')))
+
+        tar.close()
+
+        self.write(buf.getvalue())
+
+
+    def set_default_headers(self):
+        fmt = socket.gethostname() + '-%Y%m%d-%H%M%S'
+        self.basename = datetime.datetime.now().strftime(fmt)
+        filename = self.basename + '.tar.bz2'
+        self.set_header('Content-Disposition', 'filename="%s"' % filename)
+        self.set_header('Content-Type', 'application/x-bzip2')
+
+
+class HostnameHandler(bbctrl.APIHandler):
+    def get(self): self.write_json(socket.gethostname())
+
+    def put(self):
+        if self.get_ctrl().args.demo:
+            raise HTTPError(400, 'Cannot set hostname in demo mode')
+
+        if 'hostname' in self.json:
+            if subprocess.call(['/usr/local/bin/sethostname',
+                                self.json['hostname'].strip()]) == 0:
+                self.write_json('ok')
+                return
+
+        raise HTTPError(400, 'Failed to set hostname')
+
+
+class WifiHandler(bbctrl.APIHandler):
+    def get(self):
+        data = {'ssid': '', 'channel': 0}
+        try:
+            data = json.loads(call_get_output(['config-wifi', '-j']))
+        except: pass
+        self.write_json(data)
+
+
+    def put(self):
+        if self.get_ctrl().args.demo:
+            raise HTTPError(400, 'Cannot configure WiFi in demo mode')
+
+        if 'mode' in self.json:
+            cmd = ['config-wifi', '-r']
+            mode = self.json['mode']
+
+            if mode == 'disabled': cmd += ['-d']
+            elif 'ssid' in self.json:
+                cmd += ['-s', self.json['ssid']]
+
+                if mode == 'ap':
+                    cmd += ['-a']
+                    if 'channel' in self.json:
+                        cmd += ['-c', self.json['channel']]
+
+                if 'pass' in self.json:
+                    cmd += ['-p', self.json['pass']]
+
+            if subprocess.call(cmd) == 0:
+                self.write_json('ok')
+                return
+
+        raise HTTPError(400, 'Failed to configure wifi')
+
+
+class UsernameHandler(bbctrl.APIHandler):
+    def get(self): self.write_json(get_username())
+
+
+    def put_ok(self):
+        if self.get_ctrl().args.demo:
+            raise HTTPError(400, 'Cannot set username in demo mode')
+
+        if 'username' in self.json: set_username(self.json['username'])
+        else: raise HTTPError(400, 'Missing "username"')
+
+
+class PasswordHandler(bbctrl.APIHandler):
+    def put(self):
+        if self.get_ctrl().args.demo:
+            raise HTTPError(400, 'Cannot set password in demo mode')
+
+        if 'current' in self.json and 'password' in self.json:
+            check_password(self.json['current'])
+
+            # Set password
+            s = '%s:%s' % (get_username(), self.json['password'])
+            s = s.encode('utf-8')
+
+            p = subprocess.Popen(['chpasswd', '-c', 'MD5'],
+                                 stdin = subprocess.PIPE)
+            p.communicate(input = s)
+
+            if p.returncode == 0:
+                self.write_json('ok')
+                return
+
+        raise HTTPError(401, 'Failed to set password')
+
+
+class ConfigLoadHandler(bbctrl.APIHandler):
+    def get(self): self.write_json(self.get_ctrl().config.load())
+
+
+class ConfigDownloadHandler(bbctrl.APIHandler):
+    def set_default_headers(self):
+        fmt = socket.gethostname() + '-%Y%m%d.json'
+        filename = datetime.date.today().strftime(fmt)
+        self.set_header('Content-Type', 'application/octet-stream')
+        self.set_header('Content-Disposition',
+                        'attachment; filename="%s"' % filename)
+
+    def get(self):
+        self.write_json(self.get_ctrl().config.load(), pretty = True)
+
+
+class ConfigSaveHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().config.save(self.json)
+
+
+class ConfigResetHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().config.reset()
+
+
+class FirmwareUpdateHandler(bbctrl.APIHandler):
+    def prepare(self): pass
+
+
+    def put_ok(self):
+        if not 'password' in self.request.arguments:
+            raise HTTPError(401, 'Missing "password"')
+
+        if not 'firmware' in self.request.files:
+            raise HTTPError(401, 'Missing "firmware"')
+
+        check_password(self.request.arguments['password'][0])
+
+        firmware = self.request.files['firmware'][0]
+
+        if not os.path.exists('firmware'): os.mkdir('firmware')
+
+        with open('firmware/update.tar.bz2', 'wb') as f:
+            f.write(firmware['body'])
+
+        self.get_ctrl().lcd.goodbye('Upgrading firmware')
+        subprocess.Popen(['/usr/local/bin/update-bbctrl'])
+
+
+class UpgradeHandler(bbctrl.APIHandler):
+    def put_ok(self):
+        check_password(self.json['password'])
+        self.get_ctrl().lcd.goodbye('Upgrading firmware')
+        subprocess.Popen(['/usr/local/bin/upgrade-bbctrl'])
+
+
+class PathHandler(bbctrl.APIHandler):
+    @gen.coroutine
+    def get(self, filename, dataType, *args):
+        if not os.path.exists(self.get_upload(filename)):
+            raise HTTPError(404, 'File not found')
+
+        preplanner = self.get_ctrl().preplanner
+        future = preplanner.get_plan(filename)
+
+        try:
+            delta = datetime.timedelta(seconds = 1)
+            data = yield gen.with_timeout(delta, future)
+
+        except gen.TimeoutError:
+            progress = preplanner.get_plan_progress(filename)
+            self.write_json(dict(progress = progress))
+            return
+
+        try:
+            if data is None: return
+            meta, positions, speeds = data
+
+            if dataType == '/positions': data = positions
+            elif dataType == '/speeds': data = speeds
+            else:
+                self.get_ctrl().state.set_bounds(meta['bounds'])
+                self.write_json(meta)
+                return
+
+            filename = filename + '-' + dataType[1:] + '.gz'
+            self.set_header('Content-Disposition', 'filename="%s"' % filename)
+            self.set_header('Content-Type', 'application/octet-stream')
+            self.set_header('Content-Encoding', 'gzip')
+            self.set_header('Content-Length', str(len(data)))
+
+            # Respond with chunks to avoid long delays
+            SIZE = 102400
+            chunks = [data[i:i + SIZE] for i in range(0, len(data), SIZE)]
+            for chunk in chunks:
+                self.write(chunk)
+                yield self.flush()
+
+        except tornado.iostream.StreamClosedError as e: pass
+
+
+class HomeHandler(bbctrl.APIHandler):
+    def put_ok(self, axis, action, *args):
+        if axis is not None: axis = ord(axis[1:2].lower())
+
+        if action == '/set':
+            if not 'position' in self.json:
+                raise HTTPError(400, 'Missing "position"')
+
+            self.get_ctrl().mach.home(axis, self.json['position'])
+
+        elif action == '/clear': self.get_ctrl().mach.unhome(axis)
+        else: self.get_ctrl().mach.home(axis)
+
+
+class StartHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().mach.start()
+
+
+class EStopHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().mach.estop()
+
+
+class ClearHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().mach.clear()
+
+
+class StopHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().mach.stop()
+
+
+class PauseHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().mach.pause()
+
+
+class UnpauseHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().mach.unpause()
+
+
+class OptionalPauseHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().mach.optional_pause()
+
+
+class StepHandler(bbctrl.APIHandler):
+    def put_ok(self): self.get_ctrl().mach.step()
+
+
+class PositionHandler(bbctrl.APIHandler):
+    def put_ok(self, axis):
+        self.get_ctrl().mach.set_position(axis, float(self.json['position']))
+
+
+class OverrideFeedHandler(bbctrl.APIHandler):
+    def put_ok(self, value): self.get_ctrl().mach.override_feed(float(value))
+
+
+class OverrideSpeedHandler(bbctrl.APIHandler):
+    def put_ok(self, value): self.get_ctrl().mach.override_speed(float(value))
+
+
+class ModbusReadHandler(bbctrl.APIHandler):
+    def put_ok(self):
+        self.get_ctrl().mach.modbus_read(int(self.json['address']))
+
+
+class ModbusWriteHandler(bbctrl.APIHandler):
+    def put_ok(self):
+        self.get_ctrl().mach.modbus_write(int(self.json['address']),
+                                    int(self.json['value']))
+
+
+class JogHandler(bbctrl.APIHandler):
+    def put_ok(self):
+        # Handle possible out of order jog command processing
+        if 'ts' in self.json:
+            ts = self.json['ts']
+            id = self.get_cookie('client-id')
+
+            if not hasattr(self.app, 'last_jog'):
+                self.app.last_jog = {}
+
+            last = self.app.last_jog.get(id, 0)
+            self.app.last_jog[id] = ts
+
+            if ts < last: return # Out of order
+
+        self.get_ctrl().mach.jog(self.json)
+
+
+# Base class for Web Socket connections
+class ClientConnection(object):
+    def __init__(self, app):
+        self.app = app
+        self.count = 0
+
+
+    def heartbeat(self):
+        self.timer = self.app.ioloop.call_later(3, self.heartbeat)
+        self.send({'heartbeat': self.count})
+        self.count += 1
+
+
+    def send(self, msg): raise HTTPError(400, 'Not implemented')
+
+
+    def on_open(self, id = None):
+        self.ctrl = self.app.get_ctrl(id)
+
+        self.ctrl.state.add_listener(self.send)
+        self.ctrl.log.add_listener(self.send)
+        self.is_open = True
+        self.heartbeat()
+        self.app.opened(self.ctrl)
+
+
+    def on_close(self):
+        self.app.ioloop.remove_timeout(self.timer)
+        self.ctrl.state.remove_listener(self.send)
+        self.ctrl.log.remove_listener(self.send)
+        self.is_open = False
+        self.app.closed(self.ctrl)
+
+
+    def on_message(self, data): self.ctrl.mach.mdi(data)
+
+
+# Used by CAMotics
+class WSConnection(ClientConnection, tornado.websocket.WebSocketHandler):
+    def __init__(self, app, request, **kwargs):
+        ClientConnection.__init__(self, app)
+        tornado.websocket.WebSocketHandler.__init__(
+            self, app, request, **kwargs)
+
+    def send(self, msg): self.write_message(msg)
+    def open(self): self.on_open()
+
+
+# Used by Web frontend
+class SockJSConnection(ClientConnection, sockjs.tornado.SockJSConnection):
+    def __init__(self, session):
+        ClientConnection.__init__(self, session.server.app)
+        sockjs.tornado.SockJSConnection.__init__(self, session)
+
+
+    def send(self, msg):
+        try:
+            sockjs.tornado.SockJSConnection.send(self, msg)
+        except:
+            self.close()
+
+
+    def on_open(self, info):
+        cookie = info.get_cookie('client-id')
+        if cookie is None: self.send(dict(sid = '')) # Trigger client reset
+        else:
+            id = cookie.value
+
+            ip = info.ip
+            if 'X-Real-IP' in info.headers: ip = info.headers['X-Real-IP']
+            self.app.get_ctrl(id).log.get('Web').info('Connection from %s' % ip)
+            super().on_open(id)
+
+
+class StaticFileHandler(tornado.web.StaticFileHandler):
+    def set_extra_headers(self, path):
+        self.set_header('Cache-Control',
+                        'no-store, no-cache, must-revalidate, max-age=0')
+
+
+class Web(tornado.web.Application):
+    def __init__(self, args, ioloop):
+        self.args = args
+        self.ioloop = ioloop
+        self.ctrls = {}
+
+        # Init camera
+        if not args.disable_camera:
+            if self.args.demo: log = bbctrl.log.Log(args, ioloop, 'camera.log')
+            else: log = self.get_ctrl().log
+            self.camera = bbctrl.Camera(ioloop, args, log)
+        else: self.camera = None
+
+        # Init controller
+        if not self.args.demo:
+            self.get_ctrl()
+            self.monitor = bbctrl.MonitorTemp(self)
+
+        handlers = [
+            (r'/websocket', WSConnection),
+            (r'/api/log', LogHandler),
+            (r'/api/message/(\d+)/ack', MessageAckHandler),
+            (r'/api/bugreport', BugReportHandler),
+            (r'/api/reboot', RebootHandler),
+            (r'/api/hostname', HostnameHandler),
+            (r'/api/wifi', WifiHandler),
+            (r'/api/remote/username', UsernameHandler),
+            (r'/api/remote/password', PasswordHandler),
+            (r'/api/config/load', ConfigLoadHandler),
+            (r'/api/config/download', ConfigDownloadHandler),
+            (r'/api/config/save', ConfigSaveHandler),
+            (r'/api/config/reset', ConfigResetHandler),
+            (r'/api/firmware/update', FirmwareUpdateHandler),
+            (r'/api/upgrade', UpgradeHandler),
+            (r'/api/file(/[^/]+)?', bbctrl.FileHandler),
+            (r'/api/path/([^/]+)((/positions)|(/speeds))?', PathHandler),
+            (r'/api/home(/[xyzabcXYZABC]((/set)|(/clear))?)?', HomeHandler),
+            (r'/api/start', StartHandler),
+            (r'/api/estop', EStopHandler),
+            (r'/api/clear', ClearHandler),
+            (r'/api/stop', StopHandler),
+            (r'/api/pause', PauseHandler),
+            (r'/api/unpause', UnpauseHandler),
+            (r'/api/pause/optional', OptionalPauseHandler),
+            (r'/api/step', StepHandler),
+            (r'/api/position/([xyzabcXYZABC])', PositionHandler),
+            (r'/api/override/feed/([\d.]+)', OverrideFeedHandler),
+            (r'/api/override/speed/([\d.]+)', OverrideSpeedHandler),
+            (r'/api/modbus/read', ModbusReadHandler),
+            (r'/api/modbus/write', ModbusWriteHandler),
+            (r'/api/jog', JogHandler),
+            (r'/api/video', bbctrl.VideoHandler),
+            (r'/(.*)', StaticFileHandler,
+             {'path': bbctrl.get_resource('http/'),
+              'default_filename': 'index.html'}),
+            ]
+
+        router = sockjs.tornado.SockJSRouter(SockJSConnection, '/sockjs')
+        router.app = self
+
+        tornado.web.Application.__init__(self, router.urls + handlers)
+
+        try:
+            self.listen(args.port, address = args.addr)
+
+        except Exception as e:
+            raise Exception('Failed to bind %s:%d: %s' % (
+                args.addr, args.port, e))
+
+        print('Listening on http://%s:%d/' % (args.addr, args.port))
+
+
+    def opened(self, ctrl): ctrl.clear_timeout()
+
+
+    def closed(self, ctrl):
+        # Time out clients in demo mode
+        if self.args.demo: ctrl.set_timeout(self._reap_ctrl, ctrl)
+
+
+    def _reap_ctrl(self, ctrl):
+        ctrl.close()
+        del self.ctrls[ctrl.id]
+
+
+    def get_ctrl(self, id = None):
+        if not id or not self.args.demo: id = ''
+
+        if not id in self.ctrls:
+            ctrl = bbctrl.Ctrl(self.args, self.ioloop, id)
+            self.ctrls[id] = ctrl
+
+        else: ctrl = self.ctrls[id]
+
+        return ctrl
+
+
+    # Override default logger
+    def log_request(self, handler):
+        log = self.get_ctrl(handler.get_cookie('client-id')).log.get('Web')
+        log.info("%d %s", handler.get_status(), handler._request_summary())

src/py/bbctrl/__init__.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import sys
+import signal
+import tornado
+import argparse
+import datetime
+
+from pkg_resources import Requirement, resource_filename
+
+from bbctrl.RequestHandler import RequestHandler
+from bbctrl.APIHandler import APIHandler
+from bbctrl.FileHandler import FileHandler
+from bbctrl.Config import Config
+from bbctrl.LCD import LCD, LCDPage
+from bbctrl.Mach import Mach
+from bbctrl.Web import Web
+from bbctrl.Jog import Jog
+from bbctrl.Ctrl import Ctrl
+from bbctrl.Pwr import Pwr
+from bbctrl.I2C import I2C
+from bbctrl.Planner import Planner
+from bbctrl.Preplanner import Preplanner
+from bbctrl.State import State
+from bbctrl.Comm import Comm
+from bbctrl.CommandQueue import CommandQueue
+from bbctrl.MainLCDPage import MainLCDPage
+from bbctrl.IPLCDPage import IPLCDPage
+from bbctrl.Camera import Camera, VideoHandler
+from bbctrl.AVR import AVR
+from bbctrl.AVREmu import AVREmu
+from bbctrl.IOLoop import IOLoop
+from bbctrl.MonitorTemp import MonitorTemp
+import bbctrl.Cmd as Cmd
+import bbctrl.v4l2 as v4l2
+import bbctrl.Log as log
+import bbctrl.ObjGraph as ObjGraph
+
+
+ctrl = None
+
+
+def get_resource(path):
+    return resource_filename(Requirement.parse('bbctrl'), 'bbctrl/' + path)
+
+
+def on_exit(sig = 0, func = None):
+    global ctrl
+
+    print('Exit handler triggered: signal = %d', sig)
+
+    if ctrl is not None:
+        ctrl.close()
+        ctrl = None
+
+    sys.exit(0)
+
+
+def time_str():
+    return datetime.datetime.now().strftime('%Y%m%d-%H:%M:%S')
+
+
+
+class Debugger:
+    def __init__(self, ioloop, freq = 60 * 15, depth = 100):
+        self.ioloop = ioloop
+        self.freq = freq
+        self.depth = depth
+        self._callback()
+
+
+    def _callback(self):
+        with open('bbctrl-debug-%s.log' % time_str(), 'w') as log:
+            def line(name):
+                log.write('==== ' + name + ' ' + '=' * (74 - len(name)) + '\n')
+
+            line('Common')
+            ObjGraph.show_most_common_types(limit = self.depth, file = log)
+
+            log.write('\n')
+            line('Growth')
+            ObjGraph.show_growth(limit = self.depth, file = log)
+
+            log.write('\n')
+            line('New IDs')
+            ObjGraph.get_new_ids(limit = self.depth, file = log)
+
+            log.flush()
+            self.ioloop.call_later(self.freq, self._callback)
+
+
+
+def parse_args():
+    parser = argparse.ArgumentParser(
+        description = 'Buildbotics Machine Controller')
+
+    parser.add_argument('-p', '--port', default = 80,
+                        type = int, help = 'HTTP port')
+    parser.add_argument('-a', '--addr', metavar = 'IP', default = '0.0.0.0',
+                        help = 'HTTP address to bind')
+    parser.add_argument('-s', '--serial', default = '/dev/ttyAMA0',
+                        help = 'Serial device')
+    parser.add_argument('-b', '--baud', default = 230400, type = int,
+                        help = 'Serial baud rate')
+    parser.add_argument('--i2c-port', default = 1, type = int,
+                        help = 'I2C port')
+    parser.add_argument('--lcd-addr', default = [0x27, 0x3f], type = int,
+                        help = 'LCD I2C address')
+    parser.add_argument('--avr-addr', default = 0x2b, type = int,
+                        help = 'AVR I2C address')
+    parser.add_argument('--pwr-addr', default = 0x60, type = int,
+                        help = 'Power AVR I2C address')
+    parser.add_argument('-v', '--verbose', action = 'store_true',
+                        help = 'Verbose output')
+    parser.add_argument('-l', '--log', metavar = "FILE",
+                        help = 'Set a log file')
+    parser.add_argument('--disable-camera', action = 'store_true',
+                        help = 'Disable the camera')
+    parser.add_argument('--width', default = 640, type = int,
+                        help = 'Camera width')
+    parser.add_argument('--height', default = 480, type = int,
+                        help = 'Camera height')
+    parser.add_argument('--fps', default = 15, type = int,
+                        help = 'Camera frames per second')
+    parser.add_argument('--camera-clients', default = 4,
+                        help = 'Maximum simultaneous camera clients')
+    parser.add_argument('--demo', action = 'store_true',
+                        help = 'Enter demo mode')
+    parser.add_argument('--debug', default = 0, type = int,
+                        help = 'Enable debug mode and set frequency in seconds')
+    parser.add_argument('--fast-emu', action = 'store_true',
+                        help = 'Enter demo mode')
+    parser.add_argument('--client-timeout', default = 5 * 60, type = int,
+                        help = 'Demo client timeout in seconds')
+
+    return parser.parse_args()
+
+
+def run():
+    global ctrl
+
+    args = parse_args()
+
+    # Set signal handler
+    signal.signal(signal.SIGTERM, on_exit)
+
+    # Create ioloop
+    ioloop = tornado.ioloop.IOLoop.current()
+
+    # Set ObjGraph signal handler
+    if args.debug: Debugger(ioloop, args.debug)
+
+    # Start server
+    web = Web(args, ioloop)
+
+    try:
+        ioloop.start()
+
+    except KeyboardInterrupt: on_exit()
+
+
+if __name__ == '__main__': run()

src/py/bbctrl/http

@@ -0,0 +1 @@
+../../../build/http/

src/py/bbctrl/plan.py

@@ -0,0 +1,324 @@
+#!/usr/bin/env python3
+
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     The software is distributed in the hope that it will be useful, but      #
+#          WITHOUT ANY WARRANTY; without even the implied warranty of          #
+#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU       #
+#               Lesser General Public License for more details.                #
+#                                                                              #
+#       You should have received a copy of the GNU Lesser General Public       #
+#                License along with the software.  If not, see                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import sys
+import argparse
+import json
+import time
+import math
+import os
+import re
+import gzip
+import struct
+import math
+import camotics.gplan as gplan # pylint: disable=no-name-in-module,import-error
+
+
+reLogLine = re.compile(
+    r'^(?P<level>[A-Z])[0-9 ]:'
+    r'((?P<file>[^:]+):)?'
+    r'((?P<line>\d+):)?'
+    r'((?P<column>\d+):)?'
+    r'(?P<msg>.*)$')
+
+
+def compute_unit(a, b):
+    unit = dict()
+    length = 0
+
+    for axis in 'xyz':
+        if axis in a and axis in b:
+            unit[axis] = b[axis] - a[axis]
+            length += unit[axis] * unit[axis]
+
+    length = math.sqrt(length)
+
+    if length:
+        for axis in 'xyz':
+            if axis in unit: unit[axis] /= length
+
+    return unit
+
+
+def compute_move(start, unit, dist):
+    move = dict()
+
+    for axis in 'xyz':
+        if axis in unit and axis in start:
+            move[axis] = start[axis] + unit[axis] * dist
+
+    return move
+
+
+class Plan(object):
+    def __init__(self, path, state, config):
+        self.path = path
+        self.state = state
+        self.config = config
+
+        self.lines = sum(1 for line in open(path, 'rb'))
+
+        self.planner = gplan.Planner()
+        self.planner.set_resolver(self.get_var_cb)
+        self.planner.set_logger(self._log_cb, 1, 'LinePlanner:3')
+        self.planner.load(self.path, config)
+
+        self.messages = []
+        self.levels = dict(I = 'info', D = 'debug', W = 'warning', E = 'error',
+                           C = 'critical')
+
+        # Initialized axis states and bounds
+        self.bounds = dict(min = {}, max = {})
+        for axis in 'xyz':
+            self.bounds['min'][axis] = math.inf
+            self.bounds['max'][axis] = -math.inf
+
+        self.maxSpeed = 0
+        self.currentSpeed = None
+        self.lastProgress = None
+        self.lastProgressTime = 0
+        self.time = 0
+
+
+    def add_to_bounds(self, axis, value):
+        if value < self.bounds['min'][axis]: self.bounds['min'][axis] = value
+        if self.bounds['max'][axis] < value: self.bounds['max'][axis] = value
+
+
+    def get_bounds(self):
+        # Remove infinity from bounds
+        for axis in 'xyz':
+            if self.bounds['min'][axis] == math.inf:
+                del self.bounds['min'][axis]
+            if self.bounds['max'][axis] == -math.inf:
+                del self.bounds['max'][axis]
+
+        return self.bounds
+
+
+    def update_speed(self, s):
+        if self.currentSpeed == s: return False
+        self.currentSpeed = s
+        if self.maxSpeed < s: self.maxSpeed = s
+
+        return True
+
+
+    def get_var_cb(self, name, units):
+        value = 0
+
+        if len(name) and name[0] == '_':
+            value = self.state.get(name[1:], 0)
+            if units == 'IMPERIAL': value /= 25.4
+
+        return value
+
+
+    def log_cb(self, level, msg, filename, line, column):
+        if level in self.levels: level = self.levels[level]
+
+        # Ignore missing tool warning
+        if (level == 'warning' and
+            msg.startswith('Auto-creating missing tool')):
+            return
+
+        self.messages.append(
+            dict(level = level, msg = msg, filename = filename, line = line,
+                 column = column))
+
+
+    def _log_cb(self, line):
+        line = line.strip()
+        m = reLogLine.match(line)
+        if not m: return
+
+        level = m.group('level')
+        msg = m.group('msg')
+        filename = m.group('file')
+        line = m.group('line')
+        column = m.group('column')
+
+        where = ':'.join(filter(None.__ne__, [filename, line, column]))
+
+        if line is not None: line = int(line)
+        if column is not None: column = int(column)
+
+        self.log_cb(level, msg, filename, line, column)
+
+
+    def progress(self, x):
+        if time.time() - self.lastProgressTime < 1 and x != 1: return
+        self.lastProgressTime = time.time()
+
+        p = '%.4f\n' % x
+
+        if self.lastProgress == p: return
+        self.lastProgress = p
+
+        sys.stdout.write(p)
+        sys.stdout.flush()
+
+
+    def _run(self):
+        start = time.clock()
+        line = 0
+        maxLine = 0
+        maxLineTime = time.clock()
+        position = {axis: 0 for axis in 'xyz'}
+        rapid = False
+
+        # Execute plan
+        try:
+            while self.planner.has_more():
+                cmd = self.planner.next()
+                self.planner.set_active(cmd['id']) # Release plan
+
+                # Cannot synchronize with actual machine so fake it
+                if self.planner.is_synchronizing(): self.planner.synchronize(0)
+
+                if cmd['type'] == 'line':
+                    if not (cmd.get('first', False) or
+                            cmd.get('seeking', False)):
+                        self.time += sum(cmd['times']) / 1000
+
+                    target = cmd['target']
+                    move = {}
+                    startPos = dict()
+
+                    for axis in 'xyz':
+                        if axis in target:
+                            startPos[axis] = position[axis]
+                            position[axis] = target[axis]
+                            move[axis] = target[axis]
+                            self.add_to_bounds(axis, move[axis])
+
+                    if 'rapid' in cmd: move['rapid'] = cmd['rapid']
+
+                    if 'speeds' in cmd:
+                        unit = compute_unit(startPos, target)
+
+                        for d, s in cmd['speeds']:
+                            cur = self.currentSpeed
+
+                            if self.update_speed(s):
+                                m = compute_move(startPos, unit, d)
+
+                                if cur is not None:
+                                    m['s'] = cur
+                                    yield m
+
+                                move['s'] = s
+
+                    yield move
+
+                elif cmd['type'] == 'set':
+                    if cmd['name'] == 'line':
+                        line = cmd['value']
+                        if maxLine < line:
+                            maxLine = line
+                            maxLineTime = time.clock()
+
+                    elif cmd['name'] == 'speed':
+                        s = cmd['value']
+                        if self.update_speed(s): yield {'s': s}
+
+                elif cmd['type'] == 'dwell': self.time += cmd['seconds']
+
+                if args.max_time < time.clock() - start:
+                    raise Exception('Max planning time (%d sec) exceeded.' %
+                                    args.max_time)
+
+                if args.max_loop < time.clock() - maxLineTime:
+                    raise Exception('Max loop time (%d sec) exceeded.' %
+                                    args.max_loop)
+
+                if self.lines: self.progress(maxLine / self.lines)
+
+        except Exception as e:
+            self.log_cb('error', str(e), os.path.basename(self.path), line, 0)
+
+
+    def run(self):
+        lastS = 0
+        speed = 0
+        first = True
+        x, y, z = 0, 0, 0
+
+        with gzip.open('positions.gz', 'wb') as f1:
+            with gzip.open('speeds.gz', 'wb') as f2:
+                for move in self._run():
+                    x = move.get('x', x)
+                    y = move.get('y', y)
+                    z = move.get('z', z)
+                    rapid = move.get('rapid', False)
+                    speed = move.get('s', speed)
+                    s = struct.pack('<f', math.nan if rapid else speed)
+
+                    if not first and s != lastS:
+                        f1.write(p)
+                        f2.write(s)
+
+                    lastS = s
+                    first = False
+                    p = struct.pack('<fff', x, y, z)
+
+                    f1.write(p)
+                    f2.write(s)
+
+        with open('meta.json', 'w') as f:
+            meta = dict(
+                time = self.time,
+                lines = self.lines,
+                maxSpeed = self.maxSpeed,
+                bounds = self.get_bounds(),
+                messages = self.messages)
+
+            json.dump(meta, f)
+
+
+parser = argparse.ArgumentParser(description = 'Buildbotics GCode Planner')
+parser.add_argument('gcode', help = 'The GCode file to plan')
+parser.add_argument('state', help = 'GCode state variables')
+parser.add_argument('config', help = 'Planner config')
+
+parser.add_argument('--max-time', default = 600,
+                    type = int, help = 'Maximum planning time in seconds')
+parser.add_argument('--max-loop', default = 30,
+                    type = int, help = 'Maximum time in loop in seconds')
+parser.add_argument('--nice', default = 10,
+                    type = int, help = 'Set "nice" process priority')
+
+args = parser.parse_args()
+
+state = json.loads(args.state)
+config = json.loads(args.config)
+
+os.nice(args.nice)
+plan = Plan(args.gcode, state, config)
+plan.run()