From 371adad7f4d33626c7ece558b2b3e54cdfd032e8 Mon Sep 17 00:00:00 2001
From: Stelios Giakoumidis <steyiam@gmail.com>
Date: Mon, 20 Jan 2020 10:42:19 +0200
Subject: [PATCH] Made changes to test servo motor control.

---
 camera module/main.py | 58 +++++++++++++++++++++++--------------------
 1 file changed, 31 insertions(+), 27 deletions(-)

diff --git a/camera module/main.py b/camera module/main.py
index 89ad7b9..4fb549b 100644
--- a/camera module/main.py	
+++ b/camera module/main.py	
@@ -1,9 +1,13 @@
 import cv2
 import base64
 import zmq
+import servo
 
 # Constant variables definition.
 MAJOR_VERSION = cv2.getVersionMajor()
+SERVO_INITIAL_X_ANGLE = 0	# The initial horizontal angle of the camera.
+SERVO_INITIAL_Y_ANGLE = 0	# The initial vertical angle of the camera.
+SERVO_STEP_ANGLE = 5		# The angle at which the servo motors move each frame.
 DESIRED_HEIGHT = 480		# The input image will be resized to this height, preserving its aspect ratio.
 BLUE_THRESHOLD = 150		# If the blue channel is bigger than this, it is considered background and removed.
 BINARY_THRESHOLD = 30		# If the pixel is not brighter than this, it is removed before detection.
@@ -32,30 +36,6 @@ def resizeImage(img):
 	img = cv2.resize(img, ( DESIRED_HEIGHT, int(width) ))
 	return img
 
-def removeColors(img):
-	out = None
-	dim = img.shape
-	blue = img.copy()
-	for i in range(dim[0]):
-		for j in range(dim[1]):
-			pixel = img[i,j]
-			if pixel[0] > 150:
-				blue[i,j,:] = 255
-			else:
-				blue[i,j,:] = 0
-	
-	gray = cv2.cvtColor(blue, cv2.COLOR_BGR2GRAY)
-	if MAJOR_VERSION == 3:
-		_, contours, hierarchy = cv2.findContours(gray, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
-	else:
-		contours, hierarchy = cv2.findContours(gray, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
-	if len(contours) > 0:
-		maxContour = max(contours, key = cv2.contourArea)
-		x,y,w,h = cv2.boundingRect(maxContour)
-		out = img[y:y+h,x:x+w,:]
-
-	return out
-
 def findMatchingContour(img, objX, objY):
 
 	dilated = img.copy()
@@ -105,7 +85,7 @@ def processImage(img):
 	if MAJOR_VERSION == 3:
 		_, contours, hierarchy = cv2.findContours(dilated, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
 	else:
-		contours, hierarchy = cv2.findContours(gray, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+		contours, hierarchy = cv2.findContours(dilated, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
 	if len(contours) > 0:
 		maxContour = max(contours, key = cv2.contourArea)
 	else:
@@ -170,9 +150,10 @@ footage_socket.connect('tcp://' + client_ip.decode() + ':5555')
 
 #camera = cv2.VideoCapture(0)  # init the camera
 
+servoX = SERVO_INITIAL_X_ANGLE
+servoY = SERVO_INITIAL_Y_ANGLE
 
-
-cap = cv2.VideoCapture('C:/Users/Giorgos Ger/Desktop/drone_test.mp4')
+cap = cv2.VideoCapture('C:/Users/steyi/Desktop/drone_test.mp4')
 if (cap.isOpened() == False):
 	print('Error opening stream.')
 	quit()
@@ -189,6 +170,29 @@ while (cap.isOpened()):
 			jpg_as_text = base64.b64encode(buffer)
 			footage_socket.send(jpg_as_text)
 			cv2.waitKey(33)
+
+			# Move the servo motors.
+			if yDir == 1:
+				servoY = servoY + SERVO_STEP_ANGLE
+				if servoY > 180:
+					servoY = 180
+				servo.SetAngleUp(servoY)
+			elif yDir == -1:
+				servoY = servoY - SERVO_STEP_ANGLE
+				if servoY < 0:
+					servoY = 0
+				servo.SetAngleUp(servoY)
+			if xDir == 1:
+				servoX = servoX + SERVO_STEP_ANGLE
+				if servoX > 180:
+					servoX = 180
+				if servoX < 0:
+					servoX = 0
+				servo.SetAngleDown(servoX)
+			elif xDir == -1:
+				servoX = servoX - SERVO_STEP_ANGLE
+				servo.SetAngleDown(servoX)
+
 		else:
 			break
 	except KeyboardInterrupt: