Split firmware/micropython update, and update the README.md a little, add 1st accelerometer support, mqtt bits - publish on boot, and handle bad mqtt commands

software/boot.py

@@ -1,6 +1,6 @@
 # This file is executed on every boot (including wake-boot from deepsleep)
 #
-# boot.py: version: 2018-01-14 14:00
+# boot.py: version: 2018-01-20 00:45
 #
 # Usage
 # ~~~~~
@@ -28,6 +28,10 @@
 while not aiko.wifi.connect(configuration.wifi.ssids): time.sleep(0.5)
 aiko.led.set(aiko.led.colors["blue"], 0, True)
 
+import configuration.mpu9250
+import mpu9250
+mpu9250.initialise(configuration.mpu9250.settings, lolibot.i2c_bus)
+
 # import aiko.services
 # import configuration.services
 # aiko.services.initialise(configuration.services.settings)
@@ -44,6 +48,13 @@
 # aiko.mqtt.add_message_handler(aiko.mqtt.on_message_eval)  # must be last
 aiko.led.set(aiko.led.colors["green"], 0, True)
 
+import udp_control
+udp_control.initialise()
+udp_control.add_message_handler(aiko.led.on_message_led)
+udp_control.add_message_handler(lolibot.on_message_lolibot)
+
 while True:
   aiko.mqtt.ping_check()        # TODO: Create a general timer handler
+  mpu9250.accel_check()
   aiko.mqtt.client.check_msg()  # Then make this a blocking call
+  udp_control.check()

software/configuration/mpu9250.py

@@ -0,0 +1,6 @@
+# configuration/mpu9250.py: version: 2018-01-19 22:00
+
+settings = {
+  "i2c_addr": 0x68,
+  "accel_max_g": 2,
+}

software/lib/aiko/mqtt.py

@@ -1,4 +1,4 @@
-# lib/aiko/mqtt.py: version = "2018-01-14 14:00"
+# lib/aiko/mqtt.py: version = "2018-01-19 23:00"
 #
 # Usage
 # ~~~~~
@@ -25,6 +25,7 @@
 message_handlers = []
 ping_counter = 0
 time_last_ping = 0
+state_topic = None
 
 def add_message_handler(message_handler):
   message_handlers.append(message_handler)
@@ -39,8 +40,11 @@ def on_message(topic, payload_in):
   payload_in = payload_in.decode()
 
   for message_handler in message_handlers:
-    handled = message_handler(topic, payload_in)
-    if handled: break
+    try:
+      handled = message_handler(topic, payload_in)
+      if handled: break
+    except Exception as exception:
+      print("MQTT: handler {}() exception {} ".format (message_handler.__name__, str(exception)))
 
 def on_message_eval(topic, payload_in):
   try:
@@ -49,6 +53,10 @@ def on_message_eval(topic, payload_in):
     print("MQTT: eval(): " + str(exception))
   return True
 
+def set_state(state_msg):
+  global client, state_topic
+  client.publish(state_topic, state_msg, retain=True)
+
 def ping_check():
   global ping_counter, time_last_ping
   time_now = time.ticks_ms()
@@ -63,19 +71,22 @@ def ping_check():
       print("GC:", gc.mem_free(), gc.mem_alloc())   # 72272 23728
 
 def initialise(settings):
-  global client, keepalive
+  global client, keepalive, state_topic
 
   client_id = get_unique_id()
   keepalive = settings["keepalive"]
   topic_path = settings["topic_path"]
+  state_topic = topic_path + "/state"
 
   client = MQTTClient(client_id,
     settings["host"], settings["port"], keepalive=keepalive)
 
   client.set_callback(on_message)
-  client.set_last_will(topic_path + "/state", "nil")
+  client.set_last_will(state_topic, "nil")
   client.connect()
 
   for topic in settings["topic_subscribe"]: client.subscribe(topic)
 
+  set_state ("alive")
+
   print("Connected to MQTT: %s: %s" % (settings["host"], topic_path))

software/lib/lolibot.py

@@ -1,4 +1,4 @@
-# lib/lolibot.py: version: 2018-01-14 14:00
+# lib/lolibot.py: version: 2018-01-19 23:15
 #
 # Usage
 # ~~~~~
@@ -11,7 +11,15 @@
 #
 # MQTT commands
 # ~~~~~~~~~~~~~
+# Topic: /in  {forward|stop|left|right|reverse}
+#    - set the motors to a particular motion
 # Topic: /in  freq FREQUENCY
+#    - Set the motor PWM frequency
+# Topic: /in  motors (A1 B2 A2 B2)
+#    - Set a particular PWM config directly on the motor pins
+#      for finer control
+# Topic: /in servo [-100..100]
+#    - Set the servo motion from -100 to 100 (0 = stopped)
 #
 # REPL testing
 # ~~~~~~~~~~~~
@@ -32,11 +40,14 @@
 left_motor2 = None
 right_motor1 = None
 right_motor2 = None
+servo = None
 
 duty_cycle_max = 1023
 duty_cycle_min = 200
 pwm_frequency = 30
 
+i2c_bus = None
+
 motor_commands = {
   "stop":    (   0,    0,    0,    0),
   "forward": (1023,    0, 1023,    0),
@@ -51,6 +62,12 @@ def motor_action(motor_command):
   right_motor1.duty(motor_command[2])
   right_motor2.duty(motor_command[3])
 
+def servo_pos(servo_duty):
+  servo.duty(servo_duty)
+
+def map_vals(val, min_in, max_in, min_out, max_out):
+   return (val-min_in)/(max_in-min_in)*(max_out-min_out)+min_out
+
 def on_message_lolibot(topic, payload_in):
   global pwm_frequency
 
@@ -65,6 +82,18 @@ def on_message_lolibot(topic, payload_in):
     print("motor freq: " + str(pwm_frequency))
     return True
 
+  if len(tokens) == 2 and tokens[0] == "servo":
+    servo_position = map_vals(int(tokens[1]), -100, 100, 115, 40)
+    print("Servo control to : " + str(servo_position))
+    servo_pos(int (servo_position))
+    return True
+
+  if len(tokens) == 5 and tokens[0] == "motors":
+    motor_settings = [int(x) for x in tokens[1:]]
+    print("motor settings: {}".format(' '.join (tokens[1:])))
+    motor_action(motor_settings)
+    return True
+
   return False
 
 def initialise_motor(settings, motor_pin_name):
@@ -76,6 +105,8 @@ def initialise_motor(settings, motor_pin_name):
 def initialise(settings):
   global duty_cycle_max, duty_cycle_min, pwm_frequency
   global left_motor1, left_motor2, right_motor1, right_motor2
+  global servo
+  global i2c_bus
 
   if "duty_cycle_max" in settings:
     duty_cycle_max = int(settings["duty_cycle_max"])
@@ -89,7 +120,11 @@ def initialise(settings):
   right_motor1 = initialise_motor(settings, "right_motor_pin1")
   right_motor2 = initialise_motor(settings, "right_motor_pin2")
 
-  servo = Pin(settings["servo_pin"], Pin.OUT)
+  servo_pin = Pin(settings["servo_pin"], Pin.OUT)
+  servo = PWM(servo_pin)
+  servo.freq(50) # Set 50Hz PWM
+  servo.duty(77) # Off
 
   scl = settings["scl_pin"]
   sda = settings["sda_pin"]
+  i2c_bus = machine.I2C(scl=machine.Pin(scl), sda=machine.Pin(sda))

software/lib/mpu9250.py

@@ -1,37 +1,98 @@
-import struct
-import math
-
-class MPU9250:
-    """support for the MPU-9250"""
-
-    FS_2G = 0
-    FS_4G = 8
-    FS_8G = 16
-    FS_16G = 24
-
-    def __init__(self, bus, accel_max_g=2, addr=104):
-        self.bus = bus
-        self.addr = addr
-        if accel_max_g <= 2: 
-            accel_fs_sel = 0
-            self.accel_scale = 16384
-        elif accel_max_g <= 4:
-            accel_fs_sel = 8
-            self.accel_scale = 8192
-        elif accel_max_g <= 8:
-            accel_fs_sel = 16
-            self.accel_scale = 4096
-        elif accel_max_g <= 16:
-            accel_fs_sel = 24
-            self.accel_scale = 2048
-        else:
-            raise ValueError("accel_max_g must be <= 16")
-
-        self.bus.writeto_mem(self.addr, 107, bytes([0]))
-        self.bus.writeto_mem(self.addr, 28, bytes([accel_fs_sel]))
-
-    def read(self):
-        (ax, ay, az, _, gx, gy, gz) = struct.unpack(">7h", self.bus.readfrom_mem(104,0x3b,14))
-        #return math.sqrt(ax*ax + ay*ay + az*az) / self.accel_scale, math.sqrt(gx*gx + gy*gy + gz*gz)
-        return az / self.accel_scale, gz
+# mpu9250.py: version: 2018-01-20 00:45
+import struct
+import math
+import time
 
+mpu = None
+
+time_last_check = 0
+maxZ = -50.0
+minZ = 50.0
+
+class MPU9250Exception(Exception):
+    pass
+
+class MPU9250:
+    """support for the MPU-9250"""
+
+    FS_2G = 0
+    FS_4G = 8
+    FS_8G = 16
+    FS_16G = 24
+
+    def __init__(self, bus, accel_max_g=2, addr=0x68):
+        self.bus = bus
+        self.addr = addr
+
+        try:
+           b = self.bus.readfrom_mem(self.addr, 0x75, 1)
+        except OSError:
+           raise MPU9250Exception("mpu9250 not detected.")
+
+        if b[0] != 0x71:
+            raise MPU9250Exception("Device that is not mpu9250 detected. WhoAmI reg contained 0x{:x}".format(b[0]))
+
+        if accel_max_g <= 2:
+            accel_fs_sel = 0
+            self.accel_scale = 16384
+        elif accel_max_g <= 4:
+            accel_fs_sel = 8
+            self.accel_scale = 8192
+        elif accel_max_g <= 8:
+            accel_fs_sel = 16
+            self.accel_scale = 4096
+        elif accel_max_g <= 16:
+            accel_fs_sel = 24
+            self.accel_scale = 2048
+        else:
+            raise ValueError("accel_max_g must be <= 16")
+
+        self.bus.writeto_mem(self.addr, 0x6b, bytes([0]))
+        self.bus.writeto_mem(self.addr, 0x1c, bytes([accel_fs_sel]))
+
+    def update(self):
+        # (ax, ay, az, _, gx, gy, gz) = self.readings
+        self.readings = struct.unpack(">7h", self.bus.readfrom_mem(self.addr,0x3b,14))
+
+    def readZ(self):
+        # Returns a tuple of Z acceleration and Gyro reading
+        self.update()
+
+        az = self.readings[2]
+        gz = self.readings[6]
+        #return math.sqrt(ax*ax + ay*ay + az*az) / self.accel_scale, math.sqrt(gx*gx + gy*gy + gz*gz)
+        return az / self.accel_scale, gz
+
+def initialise(settings, i2c_bus):
+    global mpu
+
+    try:
+        mpu = MPU9250(i2c_bus, settings["accel_max_g"], settings["i2c_addr"])
+        print("MPU9250 initialised")
+    except MPU9250Exception:
+        print("MPU9250 module not detected.")
+
+def readZ():
+    if mpu:
+        return mpu.readZ()
+    return (0,0)
+
+# FIXME: Replace with general timer implementation
+def accel_check():
+    global time_last_check, maxZ, minZ
+
+    if mpu is None:
+        return
+
+    # Track the max/min accelerometer reading between updates
+    # and output them once per second
+    curZ = readZ()
+    maxZ = max(curZ[0], maxZ)
+    minZ = min(curZ[0], minZ)
+
+    time_now = time.ticks_ms()
+    if time_now >= time_last_check + 1000:
+        time_last_check = time_now
+        print ("Accel Z {} min {} max {}".format(readZ(), minZ, maxZ))
+        minZ = 50.0
+        maxZ = -50.0

software/lib/udp_control.py

@@ -0,0 +1,66 @@
+# lib/udp_control.py: version = "2018-01-22 17:00"
+#
+# Usage
+# ~~~~~
+# import udp_control
+# udp_contro.initialise()
+#
+# while True:
+#   udp_control.check()
+
+import machine
+import network
+import time
+import usocket
+
+protocol = None
+socket = None
+message_handlers = []
+time_last_cmd = -5000
+
+ip_address = None
+port = 4900
+
+def initialise():
+  global ip_address, socket
+
+  sta_if = network.WLAN(network.STA_IF)
+  ip_address = sta_if.ifconfig()[0]
+
+  socket = usocket.socket(usocket.AF_INET, usocket.SOCK_DGRAM)
+  socket.bind((ip_address, port))
+  socket.setblocking(False)
+
+def add_message_handler(message_handler):
+  message_handlers.append(message_handler)
+
+def send_announce():
+  global socket, port
+  # print ("Bip")
+  request = b"lolibot! {} {}".format (str(ip_address), port)
+  address = ("255.255.255.255", port)
+  socket.sendto(request, address)
+
+def check():
+  global socket
+
+  # Send announcement on 5 second timeout
+  global time_last_cmd
+  time_now = time.ticks_ms()
+  if time_now > time_last_cmd + 5000:
+    send_announce()
+    time_last_cmd = time_now
+
+  try:
+    response, addr = socket.recvfrom(1024)
+    payload_in = response.decode("utf-8")
+    messages = [x.strip() for x in payload_in.split('\n')]
+    for message in messages:
+      for message_handler in message_handlers:
+        try:
+          handled = message_handler('lolibot/udp/in', message)
+          if handled: break
+        except Exception as exception:
+          print("UDP: handler {}({}) exception {} ".format (message_handler.__name__, message, str(exception)))
+  except OSError:
+    pass