- added Ev3Server

git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@339 b431acfa-c32f-4a4a-93f1-934dc6c82436
2016-10-23 16:51:32 +00:00
parent 93a003bf2b
commit 5ab9ce1447
2 changed files with 177 additions and 0 deletions
@@ -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
+		
+
+
+
@@ -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)
+