Here is the script so far:
Code: Select all
#thanks to frozenideasfromthenorth.blogspot.co.uk
#BY JIM BRUGES (chromebookbob) 2015
import logging
import sys
from threading import Thread
import time
sys.path.insert(0,"lib")
from cflib.crazyflie import Crazyflie
from cflib import crtp
#Import OpenCV
import cv2
#Import Numpy
import numpy as np
camera_feed = cv2.VideoCapture(0)
m_bShuttingDown = False
m_CrazyFlie = None
logging.basicConfig()
logging.getLogger().setLevel(logging.INFO)
#logging.getLogger().setLevel(logging.DEBUG)
logging.info("Initializing drivers.")
# Init drivers
crtp.init_drivers()
availableLinks = crtp.scan_interfaces()
logging.info("Available links: %s"%(availableLinks))
logging.info("Initializing Crazyflie.")
m_CrazyFlie = Crazyflie(ro_cache="cachero", rw_cache="cacherw")
logging.info("Setting radio link.")
if(len(availableLinks) == 0):
logging.error("Error, no links. Aborting.")
def OnConnected(linkUri):
logging.info("OnConnectSetupFinished")
linkUri = availableLinks[0][0]
m_CrazyFlie.connected.add_callback(OnConnected)
m_CrazyFlie.open_link(linkUri)
#set trim
roll = 0
pitch = 8
yawrate = 0
thrust = 29900
#set webcam resolution
screen_w = 640
screen_h = 480
#set midpoint
hold_pointx = screen_w / 2
hold_pointy = screen_h /2
start = 1
while not m_bShuttingDown:
roll = 0
pitch = 10
_,frame = camera_feed.read()
#Convert the current frame to HSV
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
#Define the threshold for finding a blue object with hsv
lower_blue = np.array([0, 100, 100])
upper_blue = np.array([255,255,255])
#Create a binary image, where anything blue appears white and everything else is black
mask = cv2.inRange(hsv, lower_blue, upper_blue)
#Get rid of background noise using erosion and fill in the holes using dilation and erode the final image on last time
element = cv2.getStructuringElement(cv2.MORPH_RECT,(3,3))
mask = cv2.erode(mask,element, iterations=2)
mask = cv2.dilate(mask,element,iterations=2)
mask = cv2.erode(mask,element)
#Create Contours for all blue objects
contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
maximumArea = 0
bestContour = None
for contour in contours:
currentArea = cv2.contourArea(contour)
if currentArea > maximumArea:
bestContour = contour
maximumArea = currentArea
#Create a bounding box around the biggest blue object
if bestContour is not None:
x,y,w,h = cv2.boundingRect(bestContour)
cv2.rectangle(frame, (x,y),(x+w,y+h), (100,240,240), 3)
#Show the original camera feed with a bounding box overlayed
cv2.imshow('frame',frame)
#Show the contours in a seperate window
cv2.imshow('mask',mask)
#Use this command to prevent freezes in the feed
k = cv2.waitKey(5) & 0xFF
#If escape is pressed close all windows
if k == 27:
break
#if key is Q, pitch forward 10
if k == 81:
pitch += 10
#if key is A, pitch back 10
if k == 65:
pitch -= 10
#if box coordinates are greater than the midpoint roll +10
if x > hold_pointx:
roll += 10
#if box coordinates are less than the midpoint roll -10
elif x < hold_pointx:
roll -= 10
#if box coordinates are greater than the midpoint thrust +100
if y < hold_pointy:
thrust -= 100
#if box coordinates are less than the midpoint thrust -100
elif y > hold_pointy:
thrust += 100
if start == 1:
#waits for 4 seconds so you can get ready to press escape if the crazyflie goes wild...
time.sleep(4)
start = 0
#send data to crazyflie
m_CrazyFlie.commander.send_setpoint(roll, pitch, yawrate, thrust)
cv2.destroyAllWindows() 
