From 5ab9ce144766bfe8df5da8e7d943f94c87da3efd Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Sun, 23 Oct 2016 16:51:32 +0000 Subject: [PATCH] - added Ev3Server git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@339 b431acfa-c32f-4a4a-93f1-934dc6c82436 --- ev3/Ev3Server.py | 136 +++++++++++++++++++++++++++++++++++++++++++++++ ev3/ev3_test.py | 41 ++++++++++++++ 2 files changed, 177 insertions(+) create mode 100755 ev3/Ev3Server.py create mode 100755 ev3/ev3_test.py diff --git a/ev3/Ev3Server.py b/ev3/Ev3Server.py new file mode 100755 index 0000000..f892dee --- /dev/null +++ b/ev3/Ev3Server.py @@ -0,0 +1,136 @@ +import sys +import time +import signal +import struct +import socket +import select + +from threading import Thread + +class LocalServer(object): + def __init__(self, ip, port): + self.thread = None + self.cancel = False + self.sockTcpEv3 = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.sockTcpEv3.connect((ip, port)) + print ("EV3 connected") + self.sockTcpEv3.send(b'ET /target?sn=') + data = self.sockTcpEv3.recv(1024) + dataStr = data.decode("utf-8") + if "Accept:EV340" not in dataStr: + raise Exception("EV3 has not accepted connection") + + print ("EV3 has accepted connection") + + def __del__(self): + self.stop() + + def start(self): + if self.thread is None: + self.thread = Thread(target=self.run) + self.cancel = False + self.thread.start() + + def stop(self): + if self.thread is not None: + self.cancel = True + self.thread.join() + self.thread = None + + def run(self): + # create local socket + localListenAddr = ('0.0.0.0', 5555) + print ("Starting local server on " + str(localListenAddr[0]) + ':' + str(localListenAddr[1])) + sockTcpLocal = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sockTcpLocal.settimeout(1.0) + sockTcpLocal.bind(localListenAddr) + sockTcpLocal.listen(1) + + print ("Listening") + while not self.cancel: # Listening loop + try: + conn, addr = sockTcpLocal.accept() + print ('Connection opened:' + str(addr)) + connOpen = True + while not self.cancel and connOpen: # Data loop + try: + inputready,outputready,exceptready = select.select([conn, self.sockTcpEv3], [], [], 1.0) + for sock in inputready: + if sock == conn: + data = sock.recv(1024) + if data: + print ("Client: Received data (" + str(len(data)) + ")") + self.sockTcpEv3.send(data) # send to EV3 + else: + print ('Client: Connection closed:' + str(addr)) + connOpen = False + if sock == self.sockTcpEv3: + data = sock.recv(1024) + if data: + print ("EV3: Received data (" + str(len(data)) + ")") + conn.send(data) # send to Client + else: + print ('EV3: Connection closed:' + str(addr)) + connOpen = False + except: +# print ("Receive: Timeout") + pass + + conn.close() + except: +# print ("Listening: Timeout") + pass + + self.sockTcpEv3.close() + print ("Shutdown local server on " + str(localListenAddr[0]) + ':' + str(localListenAddr[1])) + +if ("__main__" == __name__): + UDP_IP = "0.0.0.0" + UDP_PORT = 3015 + sockUdp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sockUdp.settimeout(10.0) + sockUdp.bind((UDP_IP, UDP_PORT)) + localTcpServer = None + while True: + try: + data, addr = sockUdp.recvfrom(1024) # buffer size is 1024 bytes + except: + print ("UDP: Timeout") + if localTcpServer is not None: + localTcpServer.stop() + del localTcpServer + localTcpServer = None + continue +# print ("Received from " + str(addr)) +# print ("Message (" + str(len(data)) + ") :") + dataStr = data.decode("utf-8") + keyValueStrList = dataStr.replace('\r', '').replace(' ', '').split('\n') + + keyValueDict = {} + for keyValueStr in keyValueStrList: + keyValue = keyValueStr.split(':') + if len(keyValue) == 2: + keyValueDict[keyValue[0]] = keyValue[1] + +# print (keyValueDict) + if keyValueDict['Protocol'] == 'EV3': + print ("EV3 discovered") + sockUdp.sendto(b'A', addr) + if localTcpServer is None: + try: + localTcpServer = LocalServer(addr[0], int(keyValueDict['Port'])) + localTcpServer.start() + except Exception as e: + print (e) + if localTcpServer is not None: + localTcpServer = None + del localTcpServer + + if localTcpServer is not None: + localTcpServer.stop() + del localTcpServer + localTcpServer = None + + + + diff --git a/ev3/ev3_test.py b/ev3/ev3_test.py new file mode 100755 index 0000000..06adb3e --- /dev/null +++ b/ev3/ev3_test.py @@ -0,0 +1,41 @@ +import time +import struct +import math +import ev3.ev3 as ev3 + +#addr = '/dev/rfcomm0' +addr = "127.0.0.1:5555" + +if ("__main__" == __name__): + with ev3.EV3(addr) as brick: + print ("Connection opened (press 'q' to quit).") + time.sleep(5) + data = ev3.system_command.list_files(brick, ev3.KnownPaths.PROJECTS_PATH) + print (data) + for i in range(0, 200): + msg = "Count=" + str(i) + + print ("Send: " + msg) + ev3.system_command.write_mailbox(brick, 'Text Message', msg) + data = struct.pack("f", 100*math.sin(2*math.pi*float(i)/100)) + ev3.system_command.write_mailbox(brick, 'Value', data) + time.sleep(0.1) + + cmd = ev3.direct_command.DirectCommand() + cmd.add_output_speed(ev3.direct_command.OutputPort.PORT_D, 50) + cmd.add_output_start(ev3.direct_command.OutputPort.PORT_D) + cmd.add_output_ready(ev3.direct_command.OutputPort.PORT_D) + cmd.add_timer_wait(1000) + cmd.add_output_stop(ev3.direct_command.OutputPort.PORT_D, ev3.direct_command.StopType.BRAKE) + cmd.send(brick) + + for i in range(0, 2): + cmd = ev3.direct_command.DirectCommand() + cmd.add_output_ready(ev3.direct_command.OutputPort.PORT_D) + cmd.add_output_step_speed(ev3.direct_command.OutputPort.PORT_D, 20, 30, 360, 90, ev3.direct_command.StopType.BRAKE) + cmd.add_output_ready(ev3.direct_command.OutputPort.PORT_D) + cmd.add_output_step_speed(ev3.direct_command.OutputPort.PORT_D, -20, 30, 360, 90, ev3.direct_command.StopType.BRAKE) + cmd.send(brick) + + time.sleep(5.0) +