From 7580c7376daf9919e051413d6e11cb48d68fe70c Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Sun, 30 Oct 2016 10:47:28 +0000
Subject: [PATCH] - improved ev3Server - improved laserTracker

git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@345 b431acfa-c32f-4a4a-93f1-934dc6c82436
---
 ev3/Ev3Server.py |  3 ++-
 lasertrack.py    | 29 ++++++++++++++++++++---------
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/ev3/Ev3Server.py b/ev3/Ev3Server.py
index 203e0e4..c45a48d 100755
--- a/ev3/Ev3Server.py
+++ b/ev3/Ev3Server.py
@@ -116,10 +116,11 @@ if ("__main__" == __name__):
 
 #		print (keyValueDict)
 		if keyValueDict['Protocol'] == 'EV3':
-			sockUdp.sendto(b'A', addr)
 			if localTcpServer is None:
+				sockUdp.sendto(b'A', addr)
 				print ("EV3 discovered")
 				try:
+					time.sleep(0.2)
 					localTcpServer = LocalServer(addr[0], int(keyValueDict['Port']))
 					localTcpServer.start()
 				except Exception as e:
diff --git a/lasertrack.py b/lasertrack.py
index 47d6775..51e60df 100755
--- a/lasertrack.py
+++ b/lasertrack.py
@@ -28,6 +28,8 @@ ap.add_argument("-n", "--seconds", type=int, default=10,
 	help="# of seconds to run")
 ap.add_argument("-r", "--framerate", type=int, default=30,
 	help="Framerate in frame/s")
+ap.add_argument("-p", "--with-preview", type=str, default='off',
+	help="Show camera preview")
 ap.add_argument("-e", "--with-ev3", type=str, default='off',
 	help="Connect to ev3")
 ap.add_argument("-c", "--conn-addr", type=str, default='127.0.0.1:5555',
@@ -38,17 +40,18 @@ 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,
+ap.add_argument("-kp", "--kp", type=float, default=2.0,		# 2.0
 	help="kp")
-ap.add_argument("-ki", "--ki", type=float, default=0.1,
+ap.add_argument("-ki", "--ki", type=float, default=0.1,		# 0.2
 	help="ki")
-ap.add_argument("-kd", "--kd", type=float, default=5.0,
+ap.add_argument("-kd", "--kd", type=float, default=4.0,		# 8.0
 	help="kd")
 args = vars(ap.parse_args())
 
 videoFile = args["filename"]
 framerate = args["framerate"]
 seconds = args["seconds"]
+with_preview = args["with_preview"] == 'on'
 with_ev3 = args["with_ev3"] == 'on'
 width = args["width"]
 height = args["height"]
@@ -76,17 +79,20 @@ class PidControl(object):
 		d = self.kpid[2]*(error - self.errorLast)
 		self.errorLast = error
 
-		return self.kpid[0]*error + self.errorAccu + d
+		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)):
+	def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None, kpid=(0,0,0), preview=None):
 		super(LaserTrack, self).__init__(name, resolution, numEntries, next)
+		self.motorX = -1000
+		self.motorY = -1000
 		self.brick = brick
 		self.isVisible = False
 		self.frameSkip = 3
 		self.frameSkipCounter = 0
 		self.pidCtrlX = PidControl(kpid)
 		self.pidCtrlY = PidControl(kpid)
+		self.preview = preview
 
 	def onFirstFrame(self):
 		self.sendBrick(0, 0)
@@ -165,7 +171,7 @@ class LaserTrack(RgbProcess):
 				self.frameSkipCounter -= 1
 			else:
 				self.frameSkipCounter = self.frameSkip-1
-				showFrame = True
+				showFrame = self.preview
 
 			if showFrame:
 				cv2.imshow('Objects', img1_objects)
@@ -175,7 +181,12 @@ class LaserTrack(RgbProcess):
 				self.next.write(data)
 
 	def sendBrick(self, motorX, motorY):
-		if self.brick is not None:
+		doSend = motorX != self.motorX
+		doSend |= motorY != self.motorY
+		self.motorX = motorX
+		self.motorY = motorY
+
+		if self.brick is not None and doSend:
 			data = struct.pack("f", motorX)
 			ev3.system_command.write_mailbox(self.brick, 'MotorX', data)
 			data = struct.pack("f", motorY)
@@ -195,7 +206,7 @@ if with_ev3:
 	with ev3.EV3(connAddr) as brick:
 
 		videoSource = VideoSource(videoFile, (width, height,3), framerate)
-		proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, brick=brick, kpid=kpid)
+		proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, brick=brick, kpid=kpid, preview=with_preview)
 		videoSource.processorAdd(proc)
 		videoSource.start()
 
@@ -205,7 +216,7 @@ if with_ev3:
 		proc.stop()
 else:
 	videoSource = VideoSource(videoFile, (width, height,3), framerate)
-	proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, kpid=kpid)
+	proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, kpid=kpid, preview=with_preview)
 	videoSource.processorAdd(proc)
 	videoSource.start()