Add motion control server

main.py

@@ -0,0 +1,93 @@
+import os
+import logging
+logging.basicConfig(
+        format='[%(asctime)s] %(levelname)s %(module)s/%(funcName)s - %(message)s',
+        level=logging.DEBUG if os.environ.get('DEBUG') else logging.INFO)
+
+logging.info('')
+logging.info('BOOT UP')
+
+import asyncio
+import aiomqtt
+
+import dyn4
+
+dmm = None
+
+async def process_mqtt(message):
+    try:
+        text = message.payload.decode()
+    except UnicodeDecodeError:
+        return
+
+    topic = message.topic.value
+    logging.debug('MQTT topic: %s, message: %s', topic, text)
+
+    if topic == 'motion/set_rpm':
+        rpm = int(float(text))
+        dmm.set_speed(rpm)
+
+
+async def monitor_mqtt():
+    await asyncio.sleep(1)
+
+    # from https://sbtinstruments.github.io/aiomqtt/reconnection.html
+    # modified to make new client since their code didn't work
+    # https://github.com/sbtinstruments/aiomqtt/issues/269
+    while True:
+        try:
+            async with aiomqtt.Client('localhost') as client:
+                await client.subscribe('motion/#')
+                async for message in client.messages:
+                    await process_mqtt(message)
+        except aiomqtt.MqttError:
+            logging.info('MQTT connection lost, reconnecting in 5 seconds...')
+            await asyncio.sleep(5)
+
+
+async def monitor_dyn4():
+    while True:
+        await asyncio.sleep(0.1)
+
+        if not dmm:
+            continue
+
+        pos = dmm.read_AbsPos32()
+        encoder_ppr = 65536
+
+        revolutions = pos / 65536
+
+        async with aiomqtt.Client('localhost') as client:
+            message = str(revolutions)
+            topic = 'server/position'
+            await client.publish(topic, payload=message.encode())
+
+
+async def init():
+    global dmm
+
+    dmm = dyn4.DMMDrive('/dev/ttyUSB0', 0)
+    dmm.set_speed(0)
+
+
+def task_died(future):
+    if os.environ.get('SHELL'):
+        logging.error('Motion control task died!')
+    else:
+        logging.error('Motion control task died! Waiting 60s and exiting...')
+        time.sleep(60)
+    exit()
+
+
+def main():
+    loop = asyncio.get_event_loop()
+
+    a = loop.create_task(monitor_dyn4()).add_done_callback(task_died)
+    b = loop.create_task(monitor_mqtt()).add_done_callback(task_died)
+    z = loop.create_task(init())
+
+    loop.run_forever()
+
+
+if __name__ == '__main__':
+    main()