- 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

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):
-	def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None):
-		super(MyRgbProcess, self).__init__(name, resolution, numEntries, next)
+class PidControl(object):
+	def __init__(self, kpid):
+		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
-				errorY = float(y)-self.res[1]/2
-				self.sendBrick(errorX/2, errorY/2)
+				errorX = -100.0*(float(x)/self.res[0] - 0.5)
+				errorY = -100.0*(float(y)/self.res[1] - 0.5)
 			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)
-			ev3.system_command.write_mailbox(self.brick, 'ErrorX', data)
-			data = struct.pack("f", errorY)
-			ev3.system_command.write_mailbox(self.brick, 'ErrorY', data)
+			data = struct.pack("f", motorX)
+			ev3.system_command.write_mailbox(self.brick, 'MotorX', data)
+			data = struct.pack("f", motorY)
+			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()