forked from Telos4/RoboRally
use micropython driver for d1 mini motor shield
parent
74520ff438
commit
6562ddf826
|
|
@ -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()
|
||||
|
||||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
Loading…
Reference in New Issue