From fb12a3c94b6f3b9de25499fca2d3054714a40458 Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Fri, 14 Jun 2019 10:03:02 +0200 Subject: [PATCH] implemented queue for controls --- micropython_firmware/main.py | 91 ++++++++++++++++++------------------ 1 file changed, 46 insertions(+), 45 deletions(-) diff --git a/micropython_firmware/main.py b/micropython_firmware/main.py index 5d395dc..9790ee5 100644 --- a/micropython_firmware/main.py +++ b/micropython_firmware/main.py @@ -36,6 +36,8 @@ class Robot: self.poller = uselect.poll() self.poller_timeout = 2 # timeout in ms + self.control_queue = [] + def remote_control(self): while True: @@ -63,8 +65,6 @@ class Robot: duration_current = 0 t_current = utime.ticks_ms() duration_next = None - u1_next = None - u2_next = None stopped = True timeouts = 0 while listening: @@ -73,13 +73,42 @@ class Robot: 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 + else: + # current control timed out -> applying next control + if len(self.control_queue) > 0: + print("previous control applied for {} ms too long".format(elapsed - t_current - duration_current)) + u_next = self.control_queue.pop(0) + #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(u_next[0]) + self.m2.speed(u_next[1]) + + t_current = utime.ticks_ms() + duration_current = duration_next + + 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 + + #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" + # expected data: '(t, u1_0, u2_0, u1_1, u2_1, ...)'\n" # where ui = control for motor i # ui \in [-1.0, 1.0] #print("poller waiting..") @@ -95,15 +124,19 @@ class Robot: #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)) + self.control_queue = [] + print("putting data into queue") + for i in range((len(l)-1)/2): + u1_next = int(float(l[2*i+1])*100) + print("u1 = {}".format(u1_next)) + u2_next = int(float(l[2*i+2])*100) + print("u2 = {}".format(u2_next)) + self.control_queue.append((u1_next, u2_next)) except ValueError: print("ValueError: Data has wrong format.") print("Data received: {}".format(data_str)) print("Shutting down ...") - u1_next = u2_next = 0 + self.control_queue = [] duration_current = 0 listening = False comm_socket.close() @@ -115,7 +148,7 @@ class Robot: print("IndexError: Data has wrong format.") print("Data received: {}".format(data_str)) print("Shutting down ...") - u1_next = u2_next = 0 + self.control_queue = [] duration_current = 0 listening = False comm_socket.close() @@ -127,7 +160,7 @@ class Robot: print("Some other error occured") print("Exception: {}".format(e)) print("Shutting down ...") - u1_next = u2_next = 0 + self.control_queue = [] duration_current = 0 listening = False comm_socket.close() @@ -137,38 +170,6 @@ class Robot: 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()