From 05d80fa6ed3e4eaa3102f77f92a8c9e17159a8d4 Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Fri, 14 Jun 2019 08:45:00 +0200 Subject: [PATCH] test with polling for new data (not working well) --- micropython_firmware/main.py | 171 +++++++++++++++++++++++------------ 1 file changed, 114 insertions(+), 57 deletions(-) diff --git a/micropython_firmware/main.py b/micropython_firmware/main.py index 315eaf1..5d395dc 100644 --- a/micropython_firmware/main.py +++ b/micropython_firmware/main.py @@ -4,8 +4,9 @@ from machine import I2C, Pin import d1motor -import time +import utime import usocket +import uselect import esp class Robot: @@ -32,6 +33,10 @@ class Robot: # setup socket for remote control self.addr = usocket.getaddrinfo(ip, 1234)[0][-1] + self.poller = uselect.poll() + self.poller_timeout = 2 # timeout in ms + + def remote_control(self): while True: print("setting up socket communication ...") @@ -45,73 +50,125 @@ class Robot: socket_setup_complete = True except Exception as e: print("could not create socket. error msg: {}\nwaiting 1 sec and retrying...".format(e)) - time.sleep(1.0) + utime.sleep(1.0) 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] + self.poller.register(comm_socket, uselect.POLLIN) print("connected!") listening = True + + duration_current = 0 + t_current = utime.ticks_ms() + duration_next = None + u1_next = None + u2_next = None + stopped = True + timeouts = 0 while listening: + elapsed = utime.ticks_ms() + remaining = duration_current - (elapsed-t_current) + if remaining >= 0: + timeouts = 0 + print("start of loop\n I have {} ms until next control needs to be applied".format(remaining)) + elif timeouts < 10: + print("start of loop\n I have {} ms until next control needs to be applied, timeouts = {}".format(remaining, timeouts)) + timeouts = timeouts + 1 + + trecv_start = utime.ticks_ms() + # expected data: '(t, u1, u2)'\n" # where ui = control for motor i # ui \in [-1.0, 1.0] - try: - data = comm_socket.readline() - data_str = data.decode() - print("Data received: {}".format(data_str)) - print("processing data = {}".format(data_str)) - l = data_str.strip('()\n').split(',') - print("l = {}".format(l)) - t = float(l[0]) - print("t = {}".format(t)) - u1 = int(float(l[1])*100) - print("u1 = {}".format(u1)) - u2 = int(float(l[2])*100) - print("u2 = {}".format(u2)) - except ValueError: - print("ValueError: Data has wrong format.") - print("Data received: {}".format(data_str)) - print("Shutting down ...") - u1 = u2 = 0 - t = 0.0 - listening = False - comm_socket.close() - socket.close() - del comm_socket - del socket - print("disconnected!") - except IndexError: - print("IndexError: Data has wrong format.") - print("Data received: {}".format(data_str)) - print("Shutting down ...") - u1 = u2 = 0 - t = 0.0 - listening = False - comm_socket.close() - socket.close() - del comm_socket - del socket - print("disconnected!") - except Exception as e: - print("Some other error occured") - print("Exception: {}".format(e)) - print("Shutting down ...") - u1 = u2 = 0 - t = 0.0 - listening = False - comm_socket.close() - socket.close() - del comm_socket - del socket - print("disconnected!") - finally: - self.m1.speed(u1) - self.m2.speed(u2) - time.sleep(t) - self.m1.speed(0) - self.m2.speed(0) + #print("poller waiting..") + poll_res = self.poller.poll(self.poller_timeout) # wait 100 milliseconds for socket data + if poll_res: + print("new data available") + try: + data = comm_socket.readline() + data_str = data.decode() + #print("Data received: {}".format(data_str)) + #print("processing data = {}".format(data_str)) + l = data_str.strip('()\n').split(',') + #print("l = {}".format(l)) + duration_next = int(float(l[0])*1000) + #print("duration = {}".format(duration_next)) + u1_next = int(float(l[1])*100) + #print("u1 = {}".format(u1_next)) + u2_next = int(float(l[2])*100) + #print("u2 = {}".format(u2_next)) + except ValueError: + print("ValueError: Data has wrong format.") + print("Data received: {}".format(data_str)) + print("Shutting down ...") + u1_next = u2_next = 0 + duration_current = 0 + listening = False + comm_socket.close() + socket.close() + del comm_socket + del socket + print("disconnected!") + except IndexError: + print("IndexError: Data has wrong format.") + print("Data received: {}".format(data_str)) + print("Shutting down ...") + u1_next = u2_next = 0 + duration_current = 0 + listening = False + comm_socket.close() + socket.close() + del comm_socket + del socket + print("disconnected!") + except Exception as e: + print("Some other error occured") + print("Exception: {}".format(e)) + print("Shutting down ...") + u1_next = u2_next = 0 + duration_current = 0 + listening = False + comm_socket.close() + socket.close() + del comm_socket + del socket + print("disconnected!") + trecv_end = utime.ticks_ms() + print("communication (incl. polling) took {} ms".format(trecv_end - trecv_start)) + elapsed = utime.ticks_ms() + if elapsed - t_current >= duration_current: # check if duration of current control has timed out + if u1_next is not None: # check if new control is available + print("previous control applied for {} ms too long".format(elapsed - t_current - duration_current)) + #print("duration of previous control = {}".format((elapsed - t_current)/1000.0)) + #print("applying new control (duration, u1, u2) = ({}, {}, {})".format(duration_next, u1_next, u2_next)) + # if so, apply it + self.m1.speed(u1_next) + self.m2.speed(u2_next) + + t_current = utime.ticks_ms() + duration_current = duration_next + + # reset next control + u1_next = None + u2_next = None + duration_next = None + + stopped = False + elif not stopped: + #print("previous control applied for {} ms too long".format(elapsed - t_current - duration_current)) + #print("duration of previous control = {}".format((elapsed - t_current)/1000.0)) + # no new control available -> shutdown + #print("no new control available -> stopping") + + #self.m1.speed(0) + #self.m2.speed(0) + + t_current = utime.ticks_ms() + duration_current = 0 # as soon as new control will become available we directly want to apply it immediately + + stopped = True wall_e = Robot() wall_e.remote_control()