Ejemplo n.º 1
0
// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description  : This is the entry point of the program.
// Return value : SUCCESS:0  ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char* argv[])
{
	const std::string Training = "quad2.png";
	quadrotor_matcher::image_template_gray = cv::imread(Training, 0);
	if( !quadrotor_matcher::image_template_gray.data )
	{
		std::cout << "...Couldn't find template image." << std::endl;
		return -1;
	}

	Draw draw;

	quadrotor_affine::initialize_st();

	quadrotor_job::ctrl_gain = 0.0025;
	quadrotor_job::ctrl_sweetspot = 60000.0;
	quadrotor_job::ctrl_gain2 = 1.55;//1.15;
	
	pipe* stages[] = {new quadrotor_image, new quadrotor_sift, new quadrotor_matcher, new quadrotor_affine};

	parallel_pipeline::pipeline quadrotor_pipeline;
	quadrotor_pipeline.addStages(4, stages);
	//quadrotor_pipeline.startDebugging();

	//Send the template into the pipe
	quadrotor_job* newjob = new quadrotor_job;
	newjob->image_gray = quadrotor_matcher::image_template_gray;
	quadrotor_pipeline.pushJob("Sift",newjob);

	// AR.Drone class
	ARDrone ardrone;

	// Initialize
	if (!ardrone.open(/*"192.168.1.3"*/)) {
		printf("Failed to initialize.\n");
		return -1;
	}

	ardrone.setFlatTrim();
	
	// Instructions
	printf("***************************************\n");
	printf("*      CV Drone Tracker Program       *\n");
	printf("***************************************\n");
	printf("*                                     *\n");
	printf("* - Controls -                        *\n");
	printf("*    'Space' -- Takeoff/Landing       *\n");
	printf("*                                     *\n");
	printf("* - Others -                          *\n");
	printf("*    'C'     -- Change camera         *\n");
	printf("*    'Esc'   -- Exit                  *\n");
	printf("*                                     *\n");
	printf("***************************************\n\n");

	{
		cv::Mat image = ardrone.getImage();
		quadrotor_affine::image_center_query.at<double>(0) = (double)(image.cols)/2.0;
		quadrotor_affine::image_center_query.at<double>(1) = (double)(image.rows)/2.0;
		quadrotor_affine::image_center_query.at<double>(2) = 1.0;
	}

	{		
		quadrotor_affine::image_center_training.at<double>(0) = (double)(quadrotor_matcher::image_template_gray.cols)/2.0;
		quadrotor_affine::image_center_training.at<double>(1) = (double)(quadrotor_matcher::image_template_gray.rows)/2.0;
		quadrotor_affine::image_center_training.at<double>(2) = 1.0;
	}

	unsigned long curTime = timeGetTime();
	unsigned long lastTime = curTime;
	unsigned long lastBatteryTime = curTime;
	unsigned long lastControl = curTime;
	int cameraMode = 0;

	cv::Mat savedControl;
	while (1)
	{
		curTime = timeGetTime();

		// Battery
		if(lastBatteryTime < curTime)
		{
			lastBatteryTime = curTime + 10000;
			printf("Battery = %d%%\n", ardrone.getBatteryPercentage());
		}

		// Sleep and get key
		int key = cv::waitKey(30);
		switch(key)
		{
		case 0x1b:
			quadrotor_pipeline.stopDebugging();
			goto quit;
		case ' ':
			if (ardrone.onGround()) 
				ardrone.takeoff();
			else                    
				ardrone.landing();
			break;
		case 'c':
			ardrone.setCamera(++cameraMode%4);
			break;
		}
		
		if(ardrone.willGetNewImage())
		{
			quadrotor_job* newjob = new quadrotor_job;
			newjob->image_color = ardrone.getImage();
			quadrotor_pipeline.pushJob("Image",newjob);
		}
		
		std::vector<std::shared_ptr<job_base>> finished_jobs;
		quadrotor_pipeline.getFinishedJobs(finished_jobs);

		if(!finished_jobs.empty())
		{
			std::shared_ptr<quadrotor_job> job = std::dynamic_pointer_cast<quadrotor_job, job_base>(finished_jobs.back());
			if (job->good_match)
			{
				savedControl = job->control;
				lastControl = curTime+500;
				draw.drawContour(quadrotor_matcher::image_template_gray, job->image_gray, quadrotor_affine::Contour, quadrotor_matcher::keypoints_template, job->keypoints, job->Inliers_T, job->Inliers_Q, job->AT, cv::Scalar(0,255,255), "video", true);
			}
			else
			{
				//cout << "WARNING!  No Affine Transformation could be computed" << endl;
				draw.drawMatches(quadrotor_matcher::image_template_gray, job->image_gray, quadrotor_matcher::keypoints_template, job->keypoints, job->Initial_Matches_T, job->Initial_Matches_Q,"video",true);
			}
		}

		if(curTime < lastControl)
		{
			ardrone.move3D(savedControl.at<double>(0), savedControl.at<double>(1), savedControl.at<double>(2), savedControl.at<double>(3));
		}
	}
	// See you
	quit:;
	ardrone.close();

	return 0;
}
int main(int argc, char **argv)
{
	//int i;
	//static IplImage *src_img = 0, *src_gray = 0;
	CascadeClassifier face_cascade;
	//OK 2014.02.14 精度は荒いが速度はよい ※速度重視
	face_cascade.load("..\\..\\data\\haarcascade_frontalface_alt2.xml");
	
	//setup image files used in the capture process
	Mat captureFrame;
	Mat grayscaleFrame;

	static pLeapData pLeapData;
	pLeapData.init();

	static bool mLeapnot = false;
	static bool mTakOffFlag = false;
	static bool mSendCommandflag = false;
	static int  mSendCommandcounter = 0;
	static int  mSoundCommandcounter = 0;
	static int  mSoundCommandOKcounter = 20;
	
	float pitch = 0;		//前p:-0.5 後p: 0.9
	float yaw   = 0;		//左y:-1.0 右y: 0.7
	float roll  = 0;		//左R: 0.8 右R:-1.0
	float pitch_pre = 0;	//前p:-0.5 後p: 0.9
	float yaw_pre  = 0;		//左y:-1.0 右y: 0.7
	float roll_pre = 0;		//左R: 0.8 右R:-1.0

	float PosX = 0;			//左右   左  -150 〜 右 150
	float PosY = 0;			//上下昇降 下    50 〜 上 300
	float PosZ = 0;			//前後     手前-100 〜 奥 100
	float PosX_pre = 0;		//左右   左  -150 〜 右 150
	float PosY_pre = 0;		//上下昇降 下    50 〜 上 300
	float PosZ_pre = 0;		//前後     手前-100 〜 奥 100

	float Para_pre = 0.80f;	//
	float Para_cur = 0.2f;	//

	Leap::Frame frame;		// controller is a Leap::Controller object
	Leap::HandList hands;
	Leap::Hand firstHand;

	//double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;

	int mbatValue = 0;

	//マウスイベント用
	//http://ameblo.jp/banquet-of-merry-widow/entry-11101618791.html
	MouseParam mparam;
	mparam.x = 0; mparam.y = 0; mparam.event = 0; mparam.flags = 0;
	//ウインドウへコールバック関数とコールバック関数からイベント情報を受け取る変数を渡す。
	//setMouseCallback( wname, &mfunc, &mparam );

	// AR.Drone class
    // ARDrone ardrone;

	if(mNonDronDebug == true)
	{
	}else
	{   
		// Initialize
		//if (!ardrone.open()) {
		if ( initdrone(&ardrone) == -1) {
			printf("Failed to initialize.\n");
			return -1;
		}
	}

#ifdef MCISOUND
	PlayWaveSound();
#endif

    // Battery
    printf("Battery = %d%%\n", ardrone.getBatteryPercentage());

    // Instructions
    printf("***************************************\n");
    printf("*       CV Drone sample program       *\n");
    printf("*           - How to Play -           *\n");
    printf("***************************************\n");
    printf("*                                     *\n");
    printf("* - Controls -                        *\n");
    printf("*    'Space' -- Takeoff/Landing       *\n");
    printf("*    'Up'    -- Move forward          *\n");
    printf("*    'Down'  -- Move backward         *\n");
    printf("*    'Left'  -- Turn left             *\n");
    printf("*    'Right' -- Turn right            *\n");
    printf("*    'Q'     -- Move upward           *\n");
    printf("*    'A'     -- Move downward         *\n");
    printf("*                                     *\n");
    printf("* - Others -                          *\n");
    printf("*    'C'     -- Change camera         *\n");
    printf("*    'Esc'   -- Exit                  *\n");
    printf("*                                     *\n");
    printf("*    'F' --mFaceDetectMode:スイッチ  *\n");
    printf("*    'L' --LeapMode:スイッチ         *\n");
    printf("*                                     *\n");
    printf("***************************************\n\n");
	// 
	//2014.01.15 add
	Leap::Controller leapController;
        
	// Get an image
	static IplImage *image;
	//ardrone.setCamera(0);
	ardrone.setCamera(1);//下面カメラ指定

	//顔検出後の枠用
	CvPoint pt1;
	pt1.x = 100;
	pt1.y = 100;
	CvScalar rcolor;
	rcolor = CV_RGB( 128,  80, 128);

	//ウィンドウの表示
	cvNamedWindow ("FaceDetectW", CV_WINDOW_AUTOSIZE);
	cvNamedWindow ("camera", CV_WINDOW_AUTOSIZE);
	//ウインドウへコールバック関数とコールバック関数からイベント情報を受け取る変数を渡す。
	cvSetMouseCallback( "camera", &mMouseEventfunc, &mparam );

	time_t now = time(NULL);
	struct tm *pnow = localtime(&now);

    while (1) 
	{
        // Key input
        int key = cvWaitKey(33);
        //int key = cvWaitKey(15);
		if (key == 0x1b){
			break;
		}

		//2014.03.09 add
		vx = 0.0;
		vy = 0.0;
		vz = 0.0;
		vr = 0.0;

		//音声出力タイミング用ワーク
		if (mSendCommandflag == true)
		{
			if(mSendCommandcounter++ > 50)
			{
				mSendCommandflag = false;
				mSendCommandcounter = 0;
			}
		}
        // Update
		if(mNonDronDebug == false)
		{
	        if (!ardrone.update()) 
				break;

			// Get an image
			image = ardrone.getImage();
			if((mbatValue = ardrone.getBatteryPercentage()) < 30){
				printf("Battery = %d%%\n",mbatValue );

				if(mArDroneCommandFlag == false)
					ardrone.move3D(0.0, 0.0, 0.0, 0.0);
				msleep(80);
				ardrone.landing();
				printf("Landing\n");
				msleep(180);
			}
		//}
	
#ifndef FACEDETECT
		try{
			//2014.02.15 FaceDetection追加
			// (3)メモリを確保し,読み込んだ画像のグレースケール化,ヒストグラムの均一化を行う
			CvMemStorage *storage = 0;
			storage = cvCreateMemStorage (0);
			cvClearMemStorage (storage);

			//Mat captureFrame;
			//Mat grayscaleFrame;
			Mat captureFrameMat = cvarrToMat(image);
			cvtColor(captureFrameMat, grayscaleFrame, CV_BGR2GRAY);
			equalizeHist(grayscaleFrame, grayscaleFrame);
 
			// mFaceDetectMode:Fキーにてスイッチ
			if((mFaceDetectMode == true)
				&&((ardrone.getCameraMode() == 0)||(ardrone.getCameraMode() == 2)))//正面カメラの場合に有効
			{
				// (4)物体(顔)検出
				//create a vector array to store the face found
				std::vector<Rect> faces;
				face_cascade.detectMultiScale(grayscaleFrame, faces, 1.2, 4, CV_HAAR_FIND_BIGGEST_OBJECT|CV_HAAR_SCALE_IMAGE, Size(30,30));
				//printf("FaceNum:%02d\n",faces.size());

				// (5)検出された全ての顔位置に,四角を描画する
				Point pt1;
				Point pt2;
				Point cPt1;//Center Mark
				int mFaceHeight=0;
				int mFaceWidth=0;
				//複数検出の場合は、最大のものをTrackingする。
				for(int i = 0; i < (signed)faces.size(); i++)
				{
					if(i==0)
					{
						pt1.x = faces[i].x + faces[i].width;
						pt1.y = faces[i].y + faces[i].height;
						mFaceHeight = faces[i].height;
						mFaceWidth = faces[i].width;
						pt2.x = faces[i].x ;
						pt2.y = faces[i].y ;
						cPt1.x = faces[i].x + faces[i].width/2;
						cPt1.y = faces[i].y + faces[i].height/2;
					}else
					{
						//最大の検出対象の値をキープ
						if(faces[i-1].height < faces[i].height)
						{
							pt1.x = faces[i].x + faces[i].width;
							pt1.y = faces[i].y + faces[i].height;
							mFaceHeight = faces[i].height;
							mFaceWidth = faces[i].width;
							pt2.x = faces[i].x;
							pt2.y = faces[i].y;
							cPt1.x = faces[i].x + faces[i].width/2;
							cPt1.y = faces[i].y + faces[i].height/2;
						}
					}
				}

				//printf("FaceNum:%02d",faces.size());
				if(faces.size() > 0)
				{
					//顔検出した場合の処理
					mFaceLostFlag = false;

					rectangle(captureFrameMat, pt1, pt2, cvScalar(0, 255, 0, 0), 1, 8, 0);
					//Center Mark
					circle (captureFrameMat,cPt1,5,rcolor,-2);

					//double vx=0.0, vy=0.0, vr=0.0, vz=0.0;
					if((cPt1.x > 0)&&(cPt1.x < 200)){
						vr = 1.0;
					}
					if((cPt1.x > 280)&&(cPt1.x <350)){
						vr = 1.0;
					}
					if((cPt1.x > 450)&&(cPt1.x < 520)){
						vr = -1.0;
					}
					if((cPt1.x > 600)&&(cPt1.x < 800)){
						vr = -1.0;
					}
					
					if((cPt1.y > 0)&&(cPt1.y < 350)){
						//vz = 0.75;
					}else if((cPt1.y > 400)&&(cPt1.y < 600)){
						//vz = -0.75;
					}
					if((mFaceHeight > 1)&&(mFaceHeight < 200)){
						//vx = 0.75;
						//vz = 0.75;
					}else if((mFaceHeight > 300)&&(mFaceHeight < 600)){
						//vx = -0.75;
						//vz = -0.75;
					} 

					if(!mNonDronRDebug)
					{
						if((!ardrone.onGround())&&(mArDroneCommandFlag == false))
						{
							//time_t now = time(NULL);
							//struct tm *pnow = localtime(&now);
							//printf("FT:%02d:%02d:%02d X:%03d Y:%03d vx:%02.1f vy:%02.1f vz:%02.1f vr:%02.1f FH:%03d\n",pnow->tm_hour,pnow->tm_min,pnow->tm_sec, cPt1.x,cPt1.y, vx, vy, vz, vr, mFaceHeight);
							//ardrone.move3D(vx, vy, vz, vr);
							//msleep(30);
						}
					}
				}else
				{
					//Face Lostモード
					if(mFaceLostFlag == false)
					{
						mFaceLostFlag = true;
						if(!mNonDronRDebug)
						{
							if (!ardrone.onGround())
							{
								if(mArDroneCommandFlag == false)
								{
									//ardrone.move3D(0.0, 0.0, 0.0, 0.0);
									//printf(" X:%03d Y:%03d vx:%02d vy:%02d vz:%02d vr:%02d FH:%03d\n", 0, 0, 0, 0, 0, 0, 0);
									printf("Face Lostモード\n");
									//msleep(100);
								}
							}
						}
					}
				}

				//2014.02.22
				// height value enable 150 - 400
				// x 150 - 600  center:400
				// y 150 - 600  center:400
				//printf(" x:%02d y:%02d w:%02d h:%02d",faces[0].x,faces[0].y,faces[0].width,faces[0].height);
				//printf(" cx:%02d cy:%02d w:%02d h:%02d",cPt1.x,cPt1.y,faces[0].width,faces[0].height);
				//printf("\n");
				//
				IplImage wimage = captureFrameMat;
				//static IplImage wimage = grayscaleFrame;
				//cvCopy( image, wimage);
				image = &wimage;
			}

		}catch(char *e)
		{
			printf("%s\n",e);
		}
#endif
		}

        //2014.03.09 add
		if((mLeapnot != true)&&(pLeapData.mLeapMode == true)&&(leapController.isConnected()))
		{
			frame = leapController.frame(); // controller is a Leap::Controller object
			hands = frame.hands();
			firstHand = hands[0];

			pitch_pre = pitch;
			pitch = firstHand.direction().pitch();//前p:-0.5 後p: 0.9
			pitch = pitch_pre*Para_pre + pitch*Para_cur;	//Para_pre:0.80 Para_cur:0.20

			yaw_pre = yaw;    //左y:-1.0 右y: 0.7
			yaw = firstHand.direction().yaw();    //左y:-1.0 右y: 0.7
			yaw = yaw_pre*Para_pre + yaw*Para_cur;

			roll_pre = roll; //左R: 0.8 右R:-1.0
			roll = firstHand.palmNormal().roll(); //左R: 0.8 右R:-1.0
			roll = roll_pre*Para_pre + roll*Para_cur;

			PosX = frame.pointables().leftmost().tipPosition().x;       //左右   左-150 〜 右 150
			PosY = frame.pointables().leftmost().tipPosition().y;       //上下昇降 下  50 〜 上 300
			PosZ = frame.pointables().leftmost().tipPosition().z * (1); //前後   手前-100 〜 奥 100

			if(pLeapData.mLeapDebugPrint == true){
				printf("%03d XYZ:%03.02f:%03.02f:%03.02f p:%03.02f y:%03.02f r:%03.02f TF:%01i: %i\n",mSoundCommandcounter, PosX,PosY,PosZ,pitch,yaw,roll,(int)mTakOffFlag,mSendCommandcounter);
			}

			//LeapMotion Value set
			//LeapMotionに近づけると TakeOFF
			if((PosY > 50) && (PosY < 75) && (mTakOffFlag == true))
			{
				if(mNonDronDebug == false)
				{
					if (ardrone.onGround())
					{
						mTakOffFlag = false;
					}else
					{
						if(mSoundCommandcounter>mSoundCommandOKcounter){
							sndPlaySound("..\\..\\src\\resource\\HackathonUser1orimasu.wav", SND_ASYNC);//orimasu
							mSoundCommandcounter = 0;
						}
						
						if(!mNonDronRDebug) {
							ardrone.landing();
						}

						mTakOffFlag = false;
						mSendCommandflag = true;

						if(!mNonDronRDebug) 
							if((pLeapData.mLeapMode == true)&&(mArDroneCommandFlag == false))
								ardrone.move3D(0.0, 0.0, 0.0, 0.0);

						msleep(250);
					}
				}
			}

			if((PosY > 200) && (PosY < 250) && (mTakOffFlag == false))
			{
				if(mNonDronDebug == false)
				{
					if (ardrone.onGround())
					{
						if(pLeapData.mLeapMode == true)
						{
							mSendCommandflag = true;
							if(!mNonDronRDebug) 
								ardrone.takeoff();

							msleep(250);
						
							printf("Leap takeoff\n");
							mTakOffFlag = true;
							ardrone.move3D(0.0, 0.0, 0.0, 0.0);
							//msleep(50);
							msleep(100);
						}
					}
				}
			}

			if((pitch > -0.6) && (pitch < -0.45)){
			//前進
				//vx =  1.0;
			}else if((pitch < 0.9)&&(pitch > 0.5)){
			//back
				//vx =  -1.0;
			}

			if((roll > 0.5)&&(roll < 0.8)){
			//左傾斜
				//vy = 1.0;
			}else if((roll < -1.0)&&(roll > -1.4)){
			//右傾斜
				//vy = -1.0;
			}else if((vx == 0) &&(vy == 0))
			{
				//左向き
				if((yaw < -0.5)&&(yaw > -0.8)){
					vr = 1.0;
				}
				//右向き
				if((yaw > 0.05)&&(yaw < 0.5)){
					vr = -1.0;
				}
			}else
			{
				vr = 0.0;
			}

			if(!pLeapData.mLeapDebugPrint == true){
				printf("vxyr:%02.01f %02.01f %02.01f: %02.01f %02.01f \n",vx,vy,vr,roll,roll_pre);
			}
		}//(mLeapnot != true)

		//キーコントロール入力
        // Take off / Landing 
		if(mNonDronDebug == false)
		{
			if (key == ' ') 
			{
				if (ardrone.onGround())
				{
					if(!mNonDronRDebug) 
						ardrone.takeoff();

					msleep(300);
					printf("takeoff\n");
					if(mArDroneCommandFlag == false)
					{
						ardrone.move3D(0.0, 0.0, 0.0, 0.0);
						msleep(200);
					}

					if(mSoundCommandcounter>mSoundCommandOKcounter)
					{
						sndPlaySound("..\\..\\src\\resource\\HackathonUser1tobimasu.wav", SND_ASYNC);//orimasu
						mSoundCommandcounter = 0;
					}

					mTakOffFlag = true;
					mSendCommandflag = true;
					//msleep(500);
				}else//
				{
					if(!mNonDronRDebug)// false
					{
						ardrone.landing();
						printf("Landing\n");
						msleep(500);
						ardrone.move3D(0.0, 0.0, 0.0, 0.0);
						msleep(200);
					}

					if(mSoundCommandcounter>mSoundCommandOKcounter)
					{
						sndPlaySound("..\\..\\src\\resource\\HackathonUser1orimasu.wav", SND_ASYNC);//orimasu
						mSoundCommandcounter = 0;
					}

					mTakOffFlag = false;
					mSendCommandflag = true;
					//msleep(500);
				}
			}//'Space'
		}

		//printf("*    'Space' -- Takeoff/Landing       *\n");
		//printf("*    'Up'    -- Move forward          *\n");
		//printf("*    'Down'  -- Move backward         *\n");
		//printf("*    'Left'  -- Turn left             *\n");
		//printf("*    'Right' -- Turn right            *\n");
		//printf("*    'Q'     -- Move upward           *\n");
		//printf("*    'A'     -- Move downward         *\n");
        // Move
        //vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (key == 0x260000) vx =  1.0;//Up arrow
        if (key == 0x280000) vx = -1.0;//Down arrow key
        if (key == 0x250000) vr =  1.0;//Left arrow key
        if (key == 0x270000) vr = -1.0;//Right arrow key
        if (key == 'q')      vz =  1.0;
        if (key == 'a')      vz = -1.0;

        if (key == 'r')
		{
			//Reset
			//ardrone.emergency();
			if(mNonDronDebug == true)
			{
			}else
			{   
				//2014.03.09 add
				vx = 0.0;
				vy = 0.0;
				vz = 0.0;
				vr = 0.0;
				//ardrone.close();
				if ( initdrone(&ardrone) == -1) {
					printf("Failed to initialize.\n");
					return -1;
				}
			}
		}
		
		// 2014.03.02 add
        if((key == 'f')||(key == 'F')){
			mFaceDetectMode = !mFaceDetectMode;
			printf("Face Mode:%02X  Battery = %d%%\n",mFaceDetectMode, ardrone.getBatteryPercentage());
		}
		
		if((key == 'l')||(key == 'L')){
			pLeapData.mLeapMode = !pLeapData.mLeapMode;
			printf("Leap Mode:%02X  Battery = %d%%\n",pLeapData.mLeapMode, ardrone.getBatteryPercentage());
		}

		if((key == 'v')||(key == 'V')){
			printf("Btry:%d%% mSendCommandflag:%02d\n", ardrone.getBatteryPercentage(), mSendCommandflag);
		}

		if((key == '0')||(key == '0')){
			printf("Btry:%d%% reset setFlatTrim():%02d\n", ardrone.getBatteryPercentage(), key);
			ardrone.setFlatTrim();
			msleep(500);
		}


		if(mNonDronDebug == false)
		{
			gotoPlaySound(vx, vy, vr, mSoundCommandcounter, mSoundCommandOKcounter);
			if((!mNonDronRDebug)&&(!ardrone.onGround()))
			{
				if((mArDroneCommandFlag == false)&&(MouseARMode == false))
				{
					ardrone.move3D(vx, vy, vz, vr);
					//ardrone.move3D(vx, vy, vz, vr);
					//ardrone.move3D(vx,  vr, 0.0, vy);
					msleep(150);
					time_t now = time(NULL);
					struct tm *pnow = localtime(&now);
					printf("KLT:%02d:%02d:%02d vx:%02.1f vy:%02.1f vz:%02.1f vr:%02.1f \n",pnow->tm_hour,pnow->tm_min,pnow->tm_sec, vx, vy, vz, vr);
				}
			}
			//ardrone.move3D(0, 0, 0, 0);
		}

        // Change camera
		if(mNonDronDebug == false)
		{
	        static int mode = 0;
		    if((key == 'c')||(key == 'C')) 
				ardrone.setCamera(++mode%2);
				//ardrone.setCamera(++mode%4);

	        // Display the image
			//cvCircle (image,pt1,30,rcolor,2);
			IplImage wGrayImage = grayscaleFrame;
			cvShowImage ("FaceDetectW", &wGrayImage);             
		    cvShowImage("camera", image);

			cvMoveWindow( "FaceDetectW", 600, 0 );
			cvMoveWindow( "camera", 50, 0 );
			//WindowFromPoint(point(100,200));
		
			if ((key == 's') && (ardrone.getCameraMode() == 1))
			{
				imgSave("..\\..\\SaveFileName.jpg", &wGrayImage);
			}
		}
    }//while loop

    // See you
	if(mNonDronDebug == false)
	{
	    ardrone.close();
		cvDestroyWindow ("camera");
		cvDestroyWindow ("FaceDetectW");
	}

    return 0;
}