use micropython driver for d1 mini motor shield

micropython_firmware/d1motor.py

@@ -0,0 +1,72 @@
+"""
+Source: https://bitbucket.org/thesheep/micropython-d1motor
+
+import d1motor
+from machine import I2C, Pin
+i2c = I2C(Pin(5), Pin(4), freq=100000)
+m0 = d1motor.Motor(0, i2c)
+m1 = d1motor.Motor(1, i2c)
+m0.speed(5000)
+
+"""
+import ustruct
+
+
+_STATE_BRAKE = const(0)
+_STATE_RIGHT = const(1) # clockwise
+_STATE_LEFT = const(2) # counter-clockwise
+_STATE_STOP = const(3)
+_STATE_SLEEP = const(4)
+
+
+class Motor:
+    def __init__(self, index, i2c, address=0x30, standby_pin=None):
+        if index not in (0, 1):
+            raise ValueError("Index must be 0 or 1")
+        self.index = index
+        self.i2c = i2c
+        self.address = address
+        self.standby_pin = standby_pin
+        self._speed = 0
+        self._state = 0
+        if standby_pin is not None:
+            standby_pin.init(standby_pin.OUT, 0)
+        self.frequency(1000)
+
+    def frequency(self, frequency=None):
+        if frequency is None:
+            return self._pwm_frequency
+        self._pwm_frequency = frequency
+        self.i2c.writeto_mem(self.address, 0x00 | self.index,
+            ustruct.pack(">BH", 0x00, frequency))
+
+    def update(self):
+        if self.standby_pin is not None:
+            self.standby_pin.value(not self._state == _STATE_SLEEP)
+        self.i2c.writeto_mem(self.address, 0x10 | self.index,
+            ustruct.pack(">BH", self._state, self._speed))
+
+    def speed(self, speed=None):
+        if speed is None:
+            return self._speed
+        if speed > 0:
+            self._speed = min(10000, max(1, speed))
+            self._state = _STATE_RIGHT
+        elif speed < 0:
+            self._speed = min(10000, max(1, -speed))
+            self._state = _STATE_LEFT
+        else:
+            self._speed = 0
+            self._state = _STATE_STOP
+        self.update()
+
+    def sleep(self):
+        self._speed = 0
+        self._state = _STATE_SLEEP
+        self.update()
+
+    def brake(self):
+        self._speed = 0
+        self._state = _STATE_BRAKE
+        self.update()
+

micropython_firmware/main.py

@@ -1,84 +1,30 @@
 import machine
+from machine import I2C, Pin
+
+import d1motor
+
 import time
 import usocket
 
-class Motor:
-    def __init__(self, enable, dir1, dir2):
-        self.enable_pin = machine.Pin(enable, machine.Pin.OUT)
-        self.enable_pwm = machine.PWM(self.enable_pin)
-        self.enable_pwm.freq(500)
-
-        self.dir1_pin = machine.Pin(dir1, machine.Pin.OUT)
-        self.dir2_pin = machine.Pin(dir2, machine.Pin.OUT)
-
-        self.direction = 1 # default direction: dir1_pin = HIGH, dir2_pin = LOW
-        self.reverse()
-
-    def reverse(self):
-        self.direction = not self.direction
-        self.dir1_pin.value(self.direction)
-        self.dir2_pin.value(not self.direction)
-
-    def control(self, value):
-        if value > 0.0: # forward
-            if not self.direction: # switch direction if necessary
-                self.reverse()
-        else: # backward
-            if self.direction: # switch direction if necessary
-                self.reverse()
-        # apply value as pwm signal
-        self.enable_pwm.duty(int(abs(value)*1023))
-
-
 class Robot:
     def __init__(self):
+        print("setting up I2C")
+        d1 = Pin(5)
+        d2 = Pin(4)
+        i2c = I2C(scl=d1, sda=d2)
+        addr = i2c.scan()[0]
+        print("i2c scan = {}".format(addr))
         print("setting up motors")
-        self.m1 = Motor(4, 16, 5)
+        self.m1 = d1motor.Motor(0, i2c, address=addr)
-        self.m2 = Motor(14, 0, 2)
+        self.m2 = d1motor.Motor(1, i2c, address=addr)
+        print("motor setup complete")
         
-        # wait for connections on port 1234
+        # setup socket for remote control 
+        print("setting up socket communication")
         self.socket = usocket.socket()
         self.addr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1]
         self.socket.bind(self.addr)
-        print("setup complete")
+        print("socket setup complete")
-
-    def forward(self, seconds):
-        print("Onward, to glory!")
-        self.m1.control(1.0)
-        self.m2.control(1.0)
-        time.sleep(seconds)
-        self.m1.control(0.0)
-        self.m2.control(0.0)
-    
-    def backward(self, seconds):
-        print("Better retreat...")
-        self.m1.control(-1.0)
-        self.m2.control(-1.0)
-        time.sleep(seconds)
-        self.m1.control(0.0)
-        self.m2.control(0.0)
-    
-    def spin_right(self, seconds):
-        print("You spin my head right round, right round..")
-        self.m1.control(1.0)
-        self.m2.control(-1.0)
-        time.sleep(seconds)
-        self.m1.control(0.0)
-        self.m2.control(0.0)
-    
-    def spin_left(self, seconds):
-        print("spinning left")
-        self.m1.control(-1.0)
-        self.m2.control(1.0)
-        time.sleep(seconds)
-        self.m1.control(0.0)
-        self.m2.control(0.0)
-
-    def sequence(self):
-        self.spin_left(0.3)
-        self.spin_right(0.3)
-        self.forward(0.5)
-        self.backward(0.5)
 
     def remote_control(self):
         print("waiting for connections on {}".format(self.addr))
@@ -90,7 +36,7 @@ class Robot:
         while listening:
              # expected data: '(u1, u2)'\n"
              # where ui = control for motor i
-             # ui \in [-1.0, 1.0]
+             # ui \in [10000, 10000]
              data = comm_socket.readline()
              data_str = data.decode()
              print("Data received: {}".format(data_str))
@@ -98,25 +44,25 @@ class Robot:
                  print("processing data = {}".format(data_str))
                  l = data_str.strip('()\n').split(',')
                  print("l = {}".format(l))
-                 u1 = float(l[0])
+                 u1 = int(l[0])
                  print("u1 = {}".format(u1))
-                 u2 = float(l[1])
+                 u2 = int(l[1])
                  print("u2 = {}".format(u2))
              except ValueError:
                  print("ValueError: Data has wrong format.")
                  print("Data received: {}".format(data_str))
                  print("Shutting down")
-                 u1 = u2 = 0.0
+                 u1 = u2 = 0
                  listening = False
              except IndexError:
                  print("IndexError: Data has wrong format.")
                  print("Data received: {}".format(data_str))
                  print("Shutting down")
-                 u1 = u2 = 0.0
+                 u1 = u2 = 0
                  listening = False
              finally:    
-                 self.m1.control(u1)
+                 self.m1.speed(u1)
-                 self.m2.control(u2)
+                 self.m2.speed(u2)
 
 wall_e = Robot()
 wall_e.remote_control()

micropython_firmware/remote_control.py

@@ -16,24 +16,24 @@ while True:
     for event in events:
         if event.type == pygame.KEYDOWN:
             if event.key == pygame.K_LEFT:
-                u1 = -1.0
+                u1 = 10000
-                u2 = 1.0
+                u2 = -10000
                 print("turn left: ({},{})".format(u1, u2))
             elif event.key == pygame.K_RIGHT:
-                u1 = 1.0
+                u1 = -10000
-                u2 = -1.0
+                u2 = 10000
                 print("turn right: ({},{})".format(u1, u2))
             elif event.key == pygame.K_UP:
-                u1 = 1.0
+                u1 = -10000
-                u2 = 1.0
+                u2 = -10000
                 print("forward: ({},{})".format(u1, u2))
             elif event.key == pygame.K_DOWN:
-                u1 = -1.0
+                u1 = 10000
-                u2 = -1.0
+                u2 = 10000
                 print("forward: ({},{})".format(u1, u2))
             rc_socket.send('({},{})\n'.format(u1, u2))
         elif event.type == pygame.KEYUP:
-            u1 = 0.0
+            u1 = 0
-            u2 = 0.0
+            u2 = 0
             print("key released, resetting: ({},{})".format(u1, u2))
             rc_socket.send('({},{})\n'.format(u1, u2))