From 5ab7adfa96f4546755cc31c571513e46185424d6 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Sun, 6 Nov 2016 11:49:02 +0000
Subject: [PATCH] - Ev3 better exception handling - lasertracker: reverted PID
 differntial output - lasertracker: added file trace output

git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@350 b431acfa-c32f-4a4a-93f1-934dc6c82436
---
 ev3/ev3.py    | 14 ++++++++++++--
 lasertrack.py | 25 ++++++++++++++-----------
 2 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/ev3/ev3.py b/ev3/ev3.py
index ae73285..2de2b67 100755
--- a/ev3/ev3.py
+++ b/ev3/ev3.py
@@ -76,7 +76,7 @@ class Port(object):
 		self._port_str = port_str
 		self.sock = None
 		self.mode = 'Unknown'
-		if (self.sock is None):
+		while (self.sock is None):
 			try:
 				addr = self._port_str.split(':')
 				_ = socket.inet_aton(addr[0])
@@ -84,8 +84,13 @@ class Port(object):
 				self.sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
 				self.sock.connect((addr[0], int(addr[1])))
 				self.mode = "TCP"
-				
+				break
+
 			except Exception as e:
+				print ("Can't connect to " + self._port_str)
+				raise (e)
+
+			try:
 				self.sock = serial.Serial(port=self._port_str,
 											baudrate=self.RFCOMM_BAUDRATE,
 											bytesize=serial.EIGHTBITS,
@@ -99,6 +104,11 @@ class Port(object):
 											interCharTimeout=None)
 
 				self.mode = 'Serial'
+				break
+
+			except Exception as e:
+				print ("Can't open " + self._port_str)
+				raise (e)
 
 	def read(self, size, timeout=1.0):
 		if self.mode == 'Serial':
diff --git a/lasertrack.py b/lasertrack.py
index 219ae60..68cfce4 100755
--- a/lasertrack.py
+++ b/lasertrack.py
@@ -40,11 +40,11 @@ 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=6.0,		# 1.0
+ap.add_argument("-kp", "--kp", type=float, default=2.0,		# 1.0
 	help="kp")
-ap.add_argument("-ki", "--ki", type=float, default=2.0,		# 0.2
+ap.add_argument("-ki", "--ki", type=float, default=0.1,		# 0.2
 	help="ki")
-ap.add_argument("-kd", "--kd", type=float, default=2.0,		# 8.0
+ap.add_argument("-kd", "--kd", type=float, default=6.0,		# 8.0
 	help="kd")
 args = vars(ap.parse_args())
 
@@ -63,7 +63,6 @@ class PidControl(object):
 		self.kpid = kpid
 		self.errorLast = 0
 		self.errorAccu = 0
-		self.pidValue = 0
 
 	def paramSet(self, kpid):
 		self.kpid = kpid
@@ -80,10 +79,8 @@ class PidControl(object):
 		d = error - self.errorLast
 		self.errorLast = error
 		pid = self.kpid[0]*error + self.kpid[1]*self.errorAccu + self.kpid[2]*d
-		dpid = pid - self.pidValue
-		self.pidValue = pid
 
-		return (pid, dpid)
+		return pid
 
 class LaserTrack(RgbProcess):
 	def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None, kpid=(0,0,0), preview=None):
@@ -97,13 +94,15 @@ class LaserTrack(RgbProcess):
 		self.pidCtrlX = PidControl(kpid)
 		self.pidCtrlY = PidControl(kpid)
 		self.preview = preview
-		
+		self.fileTrace = open('lasertrack.trace', 'w')
+
 	def onFirstFrame(self):
 		self.sendBrick(0, 0)
 
 	def onLastFrame(self):
 		print('onLastFrame')
 		self.sendBrick(0, 0)
+		self.fileTrace.close()
 
 	def onProcess(self):
 		data = self.read(0.1)
@@ -156,16 +155,20 @@ class LaserTrack(RgbProcess):
 				errorY = -100.0*(float(y)/self.res[1] - 0.5)
 			else:
 				if self.isVisible:
+					self.pidCtrlX.reset()
+					self.pidCtrlY.reset()
 					print ("NOT Visible")
 				self.isVisible = False
 			
+			self.fileTrace.write(str(errorX) + " " + str(errorY) + "\n")
+
 			# update PID
-			(motorX, dmotorX) = self.pidCtrlX.process(errorX)
-			(motorY, dmotorY) = self.pidCtrlY.process(errorY)
+			motorX = self.pidCtrlX.process(errorX)
+			motorY = self.pidCtrlY.process(errorY)
 
 			# Update brick
 			if self.isVisible:
-				self.sendBrick(dmotorX, dmotorY)
+				self.sendBrick(motorX, motorY)
 			else:
 				self.sendBrick(0, 0)