Bitcraze Forums

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()