- improved Ev3Server

- implemted PID controller in Lasertrack. Provide motor power directly to EV3
  (use lasertracker.ev3)



git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@343 b431acfa-c32f-4a4a-93f1-934dc6c82436

ev3/Ev3Server.py

@@ -12,6 +12,7 @@ class LocalServer(object):
 		self.thread = None
 		self.cancel = False
 		self.sockTcpEv3 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+		self.sockTcpEv3.settimeout(1.0)
 		self.sockTcpEv3.connect((ip, port))
 		print ("EV3 connected")
 		self.sockTcpEv3.send(b'ET /target?sn=')
@@ -59,7 +60,7 @@ class LocalServer(object):
 							if sock == conn:
 								data = sock.recv(1024)
 								if data:
-									print ("Client: Received data (" + str(len(data)) + ")")
+#									print ("Client: Received data (" + str(len(data)) + ")")
 									self.sockTcpEv3.send(data)  # send to EV3
 								else:
 									print ('Client: Connection closed:' + str(addr))
@@ -67,7 +68,7 @@ class LocalServer(object):
 							if sock == self.sockTcpEv3:
 								data = sock.recv(1024)
 								if data:
-									print ("EV3: Received data (" + str(len(data)) + ")")
+#									print ("EV3: Received data (" + str(len(data)) + ")")
 									conn.send(data)  # send to Client
 								else:
 									print ('EV3: Connection closed:' + str(addr))
@@ -88,7 +89,7 @@ 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.settimeout(30.0)
 	sockUdp.bind((UDP_IP, UDP_PORT))
 	localTcpServer = None
 	while True:
@@ -114,17 +115,17 @@ if ("__main__" == __name__):
 
 #		print (keyValueDict)
 		if keyValueDict['Protocol'] == 'EV3':
-			print ("EV3 discovered")
 			sockUdp.sendto(b'A', addr)
 			if localTcpServer is None:
+				print ("EV3 discovered")
 				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
+						localTcpServer = None
 
 	if localTcpServer is not None:
 		localTcpServer.stop()

lasertrack.py

@@ -38,6 +38,12 @@ ap.add_argument("-x", "--width", type=int, default=320,
 	help="width")
 ap.add_argument("-y", "--height", type=int, default=240,
 	help="height")
+ap.add_argument("-kp", "--kp", type=float, default=1.5,
+	help="kp")
+ap.add_argument("-ki", "--ki", type=float, default=0.1,
+	help="ki")
+ap.add_argument("-kd", "--kd", type=float, default=5.0,
+	help="kd")
 args = vars(ap.parse_args())
 
 videoFile = args["filename"]
@@ -47,22 +53,50 @@ with_ev3 = args["with_ev3"] == 'on'
 width = args["width"]
 height = args["height"]
 connAddr = args["conn_addr"]
+kpid = (args["kp"], args["ki"], args["kd"])
 
-class MyRgbProcess(RgbProcess):
+class PidControl(object):
-	def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None):
+	def __init__(self, kpid):
-		super(MyRgbProcess, self).__init__(name, resolution, numEntries, next)
+		self.kpid = kpid
+		self.errorLast = 0
+		self.errorAccu = 0
+
+	def paramSet(self, kpid):
+		self.kpid = kpid
+
+	def paramGet(self):
+		return self.kpid
+
+	def reset(self):
+		self.errorLast = 0
+		self.errorAccu = 0
+
+	def process(self, error):
+		self.errorAccu += self.kpid[1]*error
+		d = self.kpid[2]*(error - self.errorLast)
+		self.errorLast = error
+
+		return self.kpid[0]*error + self.errorAccu + d
+
+class LaserTrack(RgbProcess):
+	def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None, kpid=(0,0,0)):
+		super(LaserTrack, self).__init__(name, resolution, numEntries, next)
 		self.brick = brick
 		self.isVisible = False
 		self.frameSkip = 3
 		self.frameSkipCounter = 0
+		self.pidCtrlX = PidControl(kpid)
+		self.pidCtrlY = PidControl(kpid)
+
 	def onFirstFrame(self):
 		self.sendBrick(0, 0)
 
 	def onLastFrame(self):
+		print('onLastFrame')
 		self.sendBrick(0, 0)
 
 	def onProcess(self):
-		data = self.read(1.0)
+		data = self.read(0.1)
 		if data is not None:
 			gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY)
 
@@ -101,20 +135,30 @@ class MyRgbProcess(RgbProcess):
 						break
 			
 			# Is object found?
+			errorX = 0
+			errorY = 0
 			if rect is not None:
 				if not self.isVisible:
 					print ("Visible")
 				self.isVisible = True
 				img1_objects = cv2.rectangle(img1_objects,rect[0],rect[1],(255,0,0),1)
-				errorX = float(x)-self.res[0]/2
+				errorX = -100.0*(float(x)/self.res[0] - 0.5)
-				errorY = float(y)-self.res[1]/2
+				errorY = -100.0*(float(y)/self.res[1] - 0.5)
-				self.sendBrick(errorX/2, errorY/2)
 			else:
 				if self.isVisible:
 					print ("NOT Visible")
-					self.sendBrick(0, 0)
 				self.isVisible = False
 			
+			# update PID
+			motorX = self.pidCtrlX.process(errorX)
+			motorY = self.pidCtrlY.process(errorY)
+
+			# Update brick
+			if self.isVisible:
+				self.sendBrick(motorX, motorY)
+			else:
+				self.sendBrick(0, 0)
+
 			# Update preview
 			showFrame = False
 			if self.frameSkipCounter > 0:
@@ -130,12 +174,12 @@ class MyRgbProcess(RgbProcess):
 			if self.next is not None:
 				self.next.write(data)
 
-	def sendBrick(self, errorX, errorY):
+	def sendBrick(self, motorX, motorY):
 		if self.brick is not None:
-			data = struct.pack("f", errorX)
+			data = struct.pack("f", motorX)
-			ev3.system_command.write_mailbox(self.brick, 'ErrorX', data)
+			ev3.system_command.write_mailbox(self.brick, 'MotorX', data)
-			data = struct.pack("f", errorY)
+			data = struct.pack("f", motorY)
-			ev3.system_command.write_mailbox(self.brick, 'ErrorY', data)
+			ev3.system_command.write_mailbox(self.brick, 'MotorY', data)
 
 	def getROI(self, img, x, y, w, h):
 		w2 = int((w-1)/2)
@@ -151,7 +195,7 @@ if with_ev3:
 	with ev3.EV3(connAddr) as brick:
 
 		videoSource = VideoSource(videoFile, (width, height,3), framerate)
-		proc = MyRgbProcess('MyRgbProcess', (width, height, 3), 4, brick=brick)
+		proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, brick=brick, kpid=kpid)
 		videoSource.processorAdd(proc)
 		videoSource.start()
 
@@ -161,7 +205,7 @@ if with_ev3:
 		proc.stop()
 else:
 	videoSource = VideoSource(videoFile, (width, height,3), framerate)
-	proc = MyRgbProcess('MyRgbProcess', (width, height, 3), 4)
+	proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, kpid=kpid)
 	videoSource.processorAdd(proc)
 	videoSource.start()