diff --git a/micropython_firmware/main.py b/micropython_firmware/main.py index 8dcafff..dfcdd1a 100644 --- a/micropython_firmware/main.py +++ b/micropython_firmware/main.py @@ -1,5 +1,6 @@ import machine import time +import usocket class Motor: def __init__(self, enable, dir1, dir2): @@ -34,6 +35,11 @@ class Robot: 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") def forward(self, seconds): @@ -74,31 +80,43 @@ class Robot: self.forward(0.5) self.backward(0.5) + 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) + wall_e = Robot() - -import usocket - -# wait for connections on port 1234 -mysocket = usocket.socket() -myaddr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1] -mysocket.bind(myaddr) -mysocket.listen(1) -res = mysocket.accept() # this blocks until someone connects to the socket -comm_socket = res[0] - -# read data until 'q\n' is sent -listening = True -while listening: - data = comm_socket.readline() - data_str = data.decode() - print(data_str) - if data_str == 'f\n': - wall_e.forward(0.5) - elif data_str == 'b\n': - wall_e.backward(0.5) - elif data_str == 'l\n': - wall_e.spin_left(0.5) - elif data_str == 'r\n': - wall_e.spin_right(0.5) - elif data_str == 'q\n': - listening = False +wall_e.remote_control() diff --git a/micropython_firmware/remote_control.py b/micropython_firmware/remote_control.py index dbfaa0c..ccfe3a8 100644 --- a/micropython_firmware/remote_control.py +++ b/micropython_firmware/remote_control.py @@ -1,10 +1,35 @@ import socket +import pygame rc_socket = socket.socket() rc_socket.connect(('192.168.4.1', 1234)) # connect to robot -rc_socket.send('l\n') # left -rc_socket.send('r\n') # right -rc_socket.send('f\n') # forward -rc_socket.send('b\n') # backward -rc_socket.send('q\n') # quit +pygame.init() +pygame.display.set_mode() + +while True: + events = pygame.event.get() + for event in events: + if event.type == pygame.KEYDOWN: + if event.key == pygame.K_LEFT: + u1 = -1.0 + u2 = 1.0 + print("turn left: ({},{})".format(u1, u2)) + elif event.key == pygame.K_RIGHT: + u1 = 1.0 + u2 = -1.0 + print("turn right: ({},{})".format(u1, u2)) + elif event.key == pygame.K_UP: + u1 = 1.0 + u2 = 1.0 + print("forward: ({},{})".format(u1, u2)) + elif event.key == pygame.K_DOWN: + u1 = -1.0 + u2 = -1.0 + print("forward: ({},{})".format(u1, u2)) + rc_socket.send('({},{})\n'.format(u1, u2)) + elif event.type == pygame.KEYUP: + u1 = 0.0 + u2 = 0.0 + print("key released, resetting: ({},{})".format(u1, u2)) + rc_socket.send('({},{})\n'.format(u1, u2))