Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

test with polling for new data (not working well)

Browse Source
This commit is contained in:
Simon Pirkelmann 2019-06-14 08:45:00 +02:00
parent c31bb9cb11
commit 05d80fa6ed
1 changed files with 114 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

171
micropython_firmware/main.py
View File

@ -4,8 +4,9 @@ from machine import I2C, Pin
import d1motor import d1motor
import time import utime
import usocket import usocket
import uselect
import esp import esp
class Robot: class Robot:
@ -32,6 +33,10 @@ class Robot:
# setup socket for remote control # setup socket for remote control
self.addr = usocket.getaddrinfo(ip, 1234)[0][-1] self.addr = usocket.getaddrinfo(ip, 1234)[0][-1]
self.poller = uselect.poll()
self.poller_timeout = 2 # timeout in ms
def remote_control(self): def remote_control(self):
while True: while True:
print("setting up socket communication ...") print("setting up socket communication ...")
@ -45,73 +50,125 @@ class Robot:
socket_setup_complete = True socket_setup_complete = True
except Exception as e: except Exception as e:
print("could not create socket. error msg: {}\nwaiting 1 sec and retrying...".format(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)) print("waiting for connections on {} ...".format(self.addr))
socket.listen(1) socket.listen(1)
res = socket.accept() # this blocks until someone connects to the socket res = socket.accept() # this blocks until someone connects to the socket
comm_socket = res[0] comm_socket = res[0]
self.poller.register(comm_socket, uselect.POLLIN)
print("connected!") print("connected!")
listening = True 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: 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" # expected data: '(t, u1, u2)'\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]
try: #print("poller waiting..")
data = comm_socket.readline() poll_res = self.poller.poll(self.poller_timeout) # wait 100 milliseconds for socket data
data_str = data.decode() if poll_res:
print("Data received: {}".format(data_str)) print("new data available")
print("processing data = {}".format(data_str)) try:
l = data_str.strip('()\n').split(',') data = comm_socket.readline()
print("l = {}".format(l)) data_str = data.decode()
t = float(l[0]) #print("Data received: {}".format(data_str))
print("t = {}".format(t)) #print("processing data = {}".format(data_str))
u1 = int(float(l[1])*100) l = data_str.strip('()\n').split(',')
print("u1 = {}".format(u1)) #print("l = {}".format(l))
u2 = int(float(l[2])*100) duration_next = int(float(l[0])*1000)
print("u2 = {}".format(u2)) #print("duration = {}".format(duration_next))
except ValueError: u1_next = int(float(l[1])*100)
print("ValueError: Data has wrong format.") #print("u1 = {}".format(u1_next))
print("Data received: {}".format(data_str)) u2_next = int(float(l[2])*100)
print("Shutting down ...") #print("u2 = {}".format(u2_next))
u1 = u2 = 0 except ValueError:
t = 0.0 print("ValueError: Data has wrong format.")
listening = False print("Data received: {}".format(data_str))
comm_socket.close() print("Shutting down ...")
socket.close() u1_next = u2_next = 0
del comm_socket duration_current = 0
del socket listening = False
print("disconnected!") comm_socket.close()
except IndexError: socket.close()
print("IndexError: Data has wrong format.") del comm_socket
print("Data received: {}".format(data_str)) del socket
print("Shutting down ...") print("disconnected!")
u1 = u2 = 0 except IndexError:
t = 0.0 print("IndexError: Data has wrong format.")
listening = False print("Data received: {}".format(data_str))
comm_socket.close() print("Shutting down ...")
socket.close() u1_next = u2_next = 0
del comm_socket duration_current = 0
del socket listening = False
print("disconnected!") comm_socket.close()
except Exception as e: socket.close()
print("Some other error occured") del comm_socket
print("Exception: {}".format(e)) del socket
print("Shutting down ...") print("disconnected!")
u1 = u2 = 0 except Exception as e:
t = 0.0 print("Some other error occured")
listening = False print("Exception: {}".format(e))
comm_socket.close() print("Shutting down ...")
socket.close() u1_next = u2_next = 0
del comm_socket duration_current = 0
del socket listening = False
print("disconnected!") comm_socket.close()
finally: socket.close()
self.m1.speed(u1) del comm_socket
self.m2.speed(u2) del socket
time.sleep(t) print("disconnected!")
self.m1.speed(0) trecv_end = utime.ticks_ms()
self.m2.speed(0) 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()