RoboRally/micropython_firmware/main.py

84 lines
2.9 KiB
Python

import machine
from machine import I2C, Pin
import d1motor
import time
import usocket
import esp
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 = d1motor.Motor(0, i2c, address=addr)
self.m2 = d1motor.Motor(1, i2c, address=addr)
print("motor setup complete")
# setup socket for remote control
self.addr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1]
def remote_control(self):
while True:
# dirty hack: free memory such that reuse of socket is possible
print(esp.freemem())
if esp.freemem() < 9 * 1024:
s = time.ticks_ms()
while esp.freemem() < 9 * 1024:
pass
print("waited {} ms".format((time.ticks_ms() - s)))
print("setting up socket communication")
socket = usocket.socket()
socket.bind(self.addr)
print("socket setup complete")
print("waiting for connections on {}".format(self.addr))
socket.listen(1)
res = 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 = int(float(l[0])*10000)
print("u1 = {}".format(u1))
u2 = int(float(l[1])*10000)
print("u2 = {}".format(u2))
except ValueError:
print("ValueError: Data has wrong format.")
print("Data received: {}".format(data_str))
print("Shutting down")
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
listening = False
finally:
self.m1.speed(u1)
self.m2.speed(u2)
comm_socket.close()
socket.close()
del comm_socket
del socket
print("disconnected")
wall_e = Robot()
wall_e.remote_control()