diff --git a/micropython_firmware/d1motor.py b/micropython_firmware/d1motor.py new file mode 100644 index 0000000..6ca87a6 --- /dev/null +++ b/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() + diff --git a/micropython_firmware/main.py b/micropython_firmware/main.py index dfcdd1a..e1133d1 100644 --- a/micropython_firmware/main.py +++ b/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.m2 = Motor(14, 0, 2) + self.m1 = d1motor.Motor(0, i2c, address=addr) + 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") - - 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) + print("socket setup complete") 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.m2.control(u2) + self.m1.speed(u1) + self.m2.speed(u2) wall_e = Robot() wall_e.remote_control() diff --git a/micropython_firmware/remote_control.py b/micropython_firmware/remote_control.py index cab279a..d28e210 100644 --- a/micropython_firmware/remote_control.py +++ b/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 - u2 = 1.0 + u1 = 10000 + u2 = -10000 print("turn left: ({},{})".format(u1, u2)) elif event.key == pygame.K_RIGHT: - u1 = 1.0 - u2 = -1.0 + u1 = -10000 + u2 = 10000 print("turn right: ({},{})".format(u1, u2)) elif event.key == pygame.K_UP: - u1 = 1.0 - u2 = 1.0 + u1 = -10000 + u2 = -10000 print("forward: ({},{})".format(u1, u2)) elif event.key == pygame.K_DOWN: - u1 = -1.0 - u2 = -1.0 + u1 = 10000 + u2 = 10000 print("forward: ({},{})".format(u1, u2)) rc_socket.send('({},{})\n'.format(u1, u2)) elif event.type == pygame.KEYUP: - u1 = 0.0 - u2 = 0.0 + u1 = 0 + u2 = 0 print("key released, resetting: ({},{})".format(u1, u2)) rc_socket.send('({},{})\n'.format(u1, u2))