RoboRally/micropython_firmware/main.py

123 lines
3.9 KiB
Python
Raw Normal View History

2019-04-12 17:17:12 +00:00
import machine
import time
import usocket
2019-04-12 17:17:12 +00:00
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)
2019-04-12 17:17:12 +00:00
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 motors")
self.m1 = Motor(4, 16, 5)
self.m2 = Motor(14, 0, 2)
# wait for connections on port 1234
self.socket = usocket.socket()
self.addr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1]
self.socket.bind(self.addr)
print("setup complete")
2019-04-12 17:17:12 +00:00
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..")
2019-04-13 12:25:41 +00:00
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")
2019-04-13 12:25:41 +00:00
self.m1.control(-1.0)
self.m2.control(1.0)
time.sleep(seconds)
self.m1.control(0.0)
self.m2.control(0.0)
2019-04-12 17:17:12 +00:00
def sequence(self):
self.spin_left(0.3)
self.spin_right(0.3)
self.forward(0.5)
self.backward(0.5)
2019-04-12 17:17:12 +00:00
def remote_control(self):
print("waiting for connections on {}".format(self.addr))
self.socket.listen(1)
res = self.socket.accept() # this blocks until someone connects to the socket
comm_socket = res[0]
print("connected!")
listening = True
while listening:
# expected data: '(u1, u2)'\n"
# where ui = control for motor i
# ui \in [-1.0, 1.0]
data = comm_socket.readline()
data_str = data.decode()
print("Data received: {}".format(data_str))
try:
print("processing data = {}".format(data_str))
l = data_str.strip('()\n').split(',')
print("l = {}".format(l))
u1 = float(l[0])
print("u1 = {}".format(u1))
u2 = float(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
listening = False
except IndexError:
print("IndexError: Data has wrong format.")
print("Data received: {}".format(data_str))
print("Shutting down")
u1 = u2 = 0.0
listening = False
finally:
self.m1.control(u1)
self.m2.control(u2)
2019-04-13 12:25:41 +00:00
wall_e = Robot()
wall_e.remote_control()