diff --git a/lasertrack.py b/lasertrack.py
index 51e60df..e6638e9 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=2.0,		# 2.0
+ap.add_argument("-kp", "--kp", type=float, default=6.0,		# 1.0
 	help="kp")
-ap.add_argument("-ki", "--ki", type=float, default=0.1,		# 0.2
+ap.add_argument("-ki", "--ki", type=float, default=2.0,		# 0.2
 	help="ki")
-ap.add_argument("-kd", "--kd", type=float, default=4.0,		# 8.0
+ap.add_argument("-kd", "--kd", type=float, default=2.0,		# 8.0
 	help="kd")
 args = vars(ap.parse_args())
 
@@ -63,6 +63,7 @@ class PidControl(object):
 		self.kpid = kpid
 		self.errorLast = 0
 		self.errorAccu = 0
+		self.pidValue = 0
 
 	def paramSet(self, kpid):
 		self.kpid = kpid
@@ -75,11 +76,14 @@ class PidControl(object):
 		self.errorAccu = 0
 
 	def process(self, error):
-		self.errorAccu += self.kpid[1]*error
-		d = self.kpid[2]*(error - self.errorLast)
+		self.errorAccu += error
+		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 self.kpid[0]*(error + self.errorAccu + d)
+		return (pid, dpid)
 
 class LaserTrack(RgbProcess):
 	def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None, kpid=(0,0,0), preview=None):
@@ -93,7 +97,7 @@ class LaserTrack(RgbProcess):
 		self.pidCtrlX = PidControl(kpid)
 		self.pidCtrlY = PidControl(kpid)
 		self.preview = preview
-
+		
 	def onFirstFrame(self):
 		self.sendBrick(0, 0)
 
@@ -112,7 +116,7 @@ class LaserTrack(RgbProcess):
 			x = maxLoc[0]
 			y = maxLoc[1]
 			# get ROI with size roi_size centered at maxLoc
-			roi_size = 21
+			roi_size = 33
 			roi = self.getROI(gray, x, y, roi_size, roi_size)
 			# do the blur on ROI
 			roi_blurred = cv2.medianBlur(roi,5)
@@ -126,7 +130,7 @@ class LaserTrack(RgbProcess):
 			rect = None
 			state = 0
 			ref_color = 1
-			for roi_size in range(3,33,2):
+			for roi_size in range(3,67,2):
 				roi = self.getROI(gray, x, y, roi_size, roi_size)
 				mean_color = cv2.mean(roi)
 				if state == 0:
@@ -156,12 +160,12 @@ class LaserTrack(RgbProcess):
 				self.isVisible = False
 			
 			# update PID
-			motorX = self.pidCtrlX.process(errorX)
-			motorY = self.pidCtrlY.process(errorY)
+			(motorX, dmotorX) = self.pidCtrlX.process(errorX)
+			(motorY, dmotorY) = self.pidCtrlY.process(errorY)
 
 			# Update brick
 			if self.isVisible:
-				self.sendBrick(motorX, motorY)
+				self.sendBrick(dmotorX, dmotorY)
 			else:
 				self.sendBrick(0, 0)
 
@@ -211,9 +215,13 @@ if with_ev3:
 		videoSource.start()
 
 		print("[INFO] sampling THREADED frames from `" + videoFile + "` at " + str(framerate) + " frame/s")
-		time.sleep(seconds)
+		try:
+			time.sleep(seconds)
+		except(KeyboardInterrupt):
+			print ("\nKeyboardInterrupt")
+
 		videoSource.stop()
-		proc.stop()
+
 else:
 	videoSource = VideoSource(videoFile, (width, height,3), framerate)
 	proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, kpid=kpid, preview=with_preview)
@@ -221,10 +229,16 @@ else:
 	videoSource.start()
 
 	print("[INFO] sampling THREADED frames from `" + videoFile + "` at " + str(framerate) + " frame/s")
-	time.sleep(seconds)
+	try:
+		time.sleep(seconds)
+	except(KeyboardInterrupt):
+		print ("\nKeyboardInterrupt")
+
 	videoSource.stop()
 
+
 # do a bit of cleanup
 cv2.destroyAllWindows()
+print ("End of program")