implemented queue for controls
This commit is contained in:
parent
05d80fa6ed
commit
fb12a3c94b
|
|
@ -36,6 +36,8 @@ class Robot:
|
||||||
self.poller = uselect.poll()
|
self.poller = uselect.poll()
|
||||||
self.poller_timeout = 2 # timeout in ms
|
self.poller_timeout = 2 # timeout in ms
|
||||||
|
|
||||||
|
self.control_queue = []
|
||||||
|
|
||||||
|
|
||||||
def remote_control(self):
|
def remote_control(self):
|
||||||
while True:
|
while True:
|
||||||
|
|
@ -63,8 +65,6 @@ class Robot:
|
||||||
duration_current = 0
|
duration_current = 0
|
||||||
t_current = utime.ticks_ms()
|
t_current = utime.ticks_ms()
|
||||||
duration_next = None
|
duration_next = None
|
||||||
u1_next = None
|
|
||||||
u2_next = None
|
|
||||||
stopped = True
|
stopped = True
|
||||||
timeouts = 0
|
timeouts = 0
|
||||||
while listening:
|
while listening:
|
||||||
|
|
@ -73,13 +73,42 @@ class Robot:
|
||||||
if remaining >= 0:
|
if remaining >= 0:
|
||||||
timeouts = 0
|
timeouts = 0
|
||||||
print("start of loop\n I have {} ms until next control needs to be applied".format(remaining))
|
print("start of loop\n I have {} ms until next control needs to be applied".format(remaining))
|
||||||
elif timeouts < 10:
|
else:
|
||||||
print("start of loop\n I have {} ms until next control needs to be applied, timeouts = {}".format(remaining, timeouts))
|
# current control timed out -> applying next control
|
||||||
timeouts = timeouts + 1
|
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()
|
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
|
# where ui = control for motor i
|
||||||
# ui \in [-1.0, 1.0]
|
# ui \in [-1.0, 1.0]
|
||||||
#print("poller waiting..")
|
#print("poller waiting..")
|
||||||
|
|
@ -95,15 +124,19 @@ class Robot:
|
||||||
#print("l = {}".format(l))
|
#print("l = {}".format(l))
|
||||||
duration_next = int(float(l[0])*1000)
|
duration_next = int(float(l[0])*1000)
|
||||||
#print("duration = {}".format(duration_next))
|
#print("duration = {}".format(duration_next))
|
||||||
u1_next = int(float(l[1])*100)
|
self.control_queue = []
|
||||||
#print("u1 = {}".format(u1_next))
|
print("putting data into queue")
|
||||||
u2_next = int(float(l[2])*100)
|
for i in range((len(l)-1)/2):
|
||||||
#print("u2 = {}".format(u2_next))
|
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:
|
except ValueError:
|
||||||
print("ValueError: Data has wrong format.")
|
print("ValueError: Data has wrong format.")
|
||||||
print("Data received: {}".format(data_str))
|
print("Data received: {}".format(data_str))
|
||||||
print("Shutting down ...")
|
print("Shutting down ...")
|
||||||
u1_next = u2_next = 0
|
self.control_queue = []
|
||||||
duration_current = 0
|
duration_current = 0
|
||||||
listening = False
|
listening = False
|
||||||
comm_socket.close()
|
comm_socket.close()
|
||||||
|
|
@ -115,7 +148,7 @@ class Robot:
|
||||||
print("IndexError: Data has wrong format.")
|
print("IndexError: Data has wrong format.")
|
||||||
print("Data received: {}".format(data_str))
|
print("Data received: {}".format(data_str))
|
||||||
print("Shutting down ...")
|
print("Shutting down ...")
|
||||||
u1_next = u2_next = 0
|
self.control_queue = []
|
||||||
duration_current = 0
|
duration_current = 0
|
||||||
listening = False
|
listening = False
|
||||||
comm_socket.close()
|
comm_socket.close()
|
||||||
|
|
@ -127,7 +160,7 @@ class Robot:
|
||||||
print("Some other error occured")
|
print("Some other error occured")
|
||||||
print("Exception: {}".format(e))
|
print("Exception: {}".format(e))
|
||||||
print("Shutting down ...")
|
print("Shutting down ...")
|
||||||
u1_next = u2_next = 0
|
self.control_queue = []
|
||||||
duration_current = 0
|
duration_current = 0
|
||||||
listening = False
|
listening = False
|
||||||
comm_socket.close()
|
comm_socket.close()
|
||||||
|
|
@ -137,38 +170,6 @@ class Robot:
|
||||||
print("disconnected!")
|
print("disconnected!")
|
||||||
trecv_end = utime.ticks_ms()
|
trecv_end = utime.ticks_ms()
|
||||||
print("communication (incl. polling) took {} ms".format(trecv_end - trecv_start))
|
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 = Robot()
|
||||||
wall_e.remote_control()
|
wall_e.remote_control()
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user