void goMousePoint(int x, int y, int MouseCtrlMode)
{
	//printf("cmode:%02d vx:%02d vy:%02d \n",ardrone.getCameraMode(),vx,vy);

	if(!mNonDronRDebug)
	{
		double vx=0.0, vy=0.0, vr=0.0, vz=0.0;

		if (!ardrone.onGround())
		{
			//CameraMode 0:正面カメラ
			//           1:下面カメラ
			//           2:正面カメラ + 小 下面カメラ
			//           3:下面カメラ + 小 正面カメラ

			int threshValue = 300;
			//正面カメラ
			if((ardrone.getCameraMode() == 0)||(ardrone.getCameraMode() == 2))
			{
				//printf("cmode:%02d \n",ardrone.getCameraMode());
				if((x>0)&&(x<800)&&(y>0)&&(y<600))
				{
					if((!MouseCtrlMode))
					{
						if((x>0)&&(x < 300)){
							//vy = -1.5;
						}
						if((x>500)&&(x < 800)){
							//vy = 1.5;
						}
						if((y>0)&&(y < 150)){
							vz = 1.0;
						}
						if((y>450)&&(y < 600)){
							vz = -1.0;
						}

						/*
						if(((x - preX) > 5)&&((x - preX) < threshValue)){
							vy = -1;
						}
						if(((x - preX) < -5)&&((x - preX) > (-1 * threshValue))){
							vy = 1;
						}
						if(((y - preY) < 10)&&((y - preY) > -10)){
							if((y>0)&&(y < 150)){
								vz = 1;
							}
							if((y>450)&&(y < 600)){
								vz = -1;
							}
						}

						if(((y - preY) > 5)&&((y - preY) < threshValue/2)){
							//画面下へドラッグ
							//vz = -1;
						}
						if(((y - preY) < -5)&&((y - preY) > (-1 * threshValue/2))){
							//vz = 1;
						}
						*/

					}else{
						//マルチタッチモード
						//前後飛行
						if(((x - preX) > 5)&&((x - preX) < threshValue)){
							vz = 1.0;
						}
						if(((x - preX) < -5)&&((x - preX) > (-1 * threshValue))){
							vz = -1.0;
						}
					}
				}
			}

			//下面カメラ
			if((ardrone.getCameraMode() == 1)||(ardrone.getCameraMode() == 3))
			{
				//printf("cmode:%02d \n",ardrone.getCameraMode());
				if((x>0)&&(x<800)&&(y>0)&&(y<600))
				{
					if((!MouseCtrlMode))
					{
						if((x>0)&&(x < 300)){
							vy = 1.5;
						}
						if((x>500)&&(x < 800)){
							vy = -1.5;
						}
						if((y>0)&&(y < 200)){
							vx = 1.5;
						}
						if((y>400)&&(y < 600)){
							vx = -1.5;
						}
						/*
						if(((x - preX) > 5)&&((x - preX) < threshValue)){
							vy = -1;
						}
						if(((x - preX) < -5)&&((x - preX) > (-1 * threshValue))){
							vy = 1;
						}
						if(((y - preY) > 5)&&((y - preY) < threshValue/2)){
							vx = 1;
						}
						if(((y - preY) < -5)&&((y - preY) > (-1 * threshValue/2))){
							vx = -1;
						}
						*/
					}else{
						//マルチタッチモード
						//昇降下降
						if(((x - preX) > 5)&&((x - preX) < threshValue)){
							vz = 1.0;
						}
						if(((x - preX) < -5)&&((x - preX) > (-1 * threshValue))){
							vz = -1.0;
						}
					}
				}
			}
				
			printf("cmode:%02d pX:%02d-%02d pY:%02d-%02d vx:%02.1f vy:%02.1f vz:%02.1f vr:%02.1f \n",ardrone.getCameraMode(), preX, x, preY, y, vx, vy, vz, vr);
			//ardrone.move3D(vx, vy, vz, vr);
			if(mArDroneCommandFlag == false)
			{
				MouseARMode = true;
				ardrone.move3D(vx,  vy, vz, vr);
				msleep(200);
				MouseARMode = false;
			}
			preX = x;
			preY = y;
		}
		//ardrone.move3D(0, 0, 0, 0);
	}

}
Exemple #2
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[])
{
    // AR.Drone class
    ARDrone ardrone;

    // Initialize
    if (!ardrone.open()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

    // Battery
    std::cout << "Battery = " << ardrone.getBatteryPercentage() << "[%]" << std::endl;

    // Instructions
    std::cout << "***************************************" << std::endl;
    std::cout << "*       CV Drone sample program       *" << std::endl;
    std::cout << "*           - How to play -           *" << std::endl;
    std::cout << "***************************************" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "* - Controls -                        *" << std::endl;
    std::cout << "*    'Space' -- Takeoff/Landing       *" << std::endl;
    std::cout << "*    'Up'    -- Move forward          *" << std::endl;
    std::cout << "*    'Down'  -- Move backward         *" << std::endl;
    std::cout << "*    'Left'  -- Turn left             *" << std::endl;
    std::cout << "*    'Right' -- Turn right            *" << std::endl;
    std::cout << "*    'Q'     -- Move upward           *" << std::endl;
    std::cout << "*    'A'     -- Move downward         *" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "* - Others -                          *" << std::endl;
    std::cout << "*    'T'     -- Track marker          *" << std::endl;
    std::cout << "*    'C'     -- Change camera         *" << std::endl;
    std::cout << "*    'Esc'   -- Exit                  *" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "***************************************" << std::endl;

    while (1) {
        double cx = 0;
        double cy = 0;
        cv::Rect trackRect;
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (key == 'i' || key == CV_VK_UP)    vx =  1.0;
        if (key == 'k' || key == CV_VK_DOWN)  vx = -1.0;
        if (key == 'u' || key == CV_VK_LEFT)  vr =  1.0;
        if (key == 'o' || key == CV_VK_RIGHT) vr = -1.0;
        if (key == 'j') vy =  1.0;
        if (key == 'l') vy = -1.0;
        if (key == 'q') vz =  1.0;
        if (key == 'a') vz = -1.0;
        ardrone.move3D(vx, vy, vz, vr);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode % 4);

        // Switch tracking ON/OFF
        static int track = 0;
        if (key == 't') track = !track;

        // People detect
        trackRect = ardrone.detectHuman(image);

        cx = trackRect.x + (trackRect.width / 2);
        cy = trackRect.y + (trackRect.height / 2); 
        cv::Point2f mc = cv::Point2f(cx, cy);

        //std::cout << "cx: " << cx << " cy: " << cy <<std::endl;
        cv::circle(image, mc, 5, cv::Scalar(0,0,255));
        //std::cout << "rect size: " << trackRect.width * trackRect.height << std::endl;

        // Tracking
        if (track) {
            if (cx == 0 && cy == 0)
            {
                vx = 0.0;
                vy = 0.0;
                vr = 0.0;
                vz = 0.0;
            } else {
                const double kp = 0.005;
                const double ka = 0.005;
                const double first_area = 30000;
                double rec_area = trackRect.width * trackRect.height;
                vx = ka * (first_area - rec_area);
                vy = 0.0;
                vr = kp * (image.cols / 2 - mc.x);
                vz = kp * (image.rows / 2 - mc.y);
                // const double kp = 0.005;
                // vx = 0.1;
                // vy = 0.0;
                // vz = kp * (image.rows / 2 - cy);
                // vr = kp * (image.cols / 2 - cx);
            }
        }
        
        // Display the image
        cv::putText(image, (track) ? "track on" : "track off", cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, (track) ? cv::Scalar(0, 0, 255) : cv::Scalar(0, 255, 0), 1, cv::LINE_AA);
        cv::imshow("camera", image);
        ardrone.move3D(vx, vy, vz, vr);

    }

    // See you
    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;
}
// --------------------------------------------------------------------------
// 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[])
{
    // AR.Drone class
    ARDrone ardrone;

    // Initialize
    if (!ardrone.open()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

    // Battery
    std::cout << "Battery = " << ardrone.getBatteryPercentage() << "[%]" << std::endl;

    // Instructions
    std::cout << "***************************************" << std::endl;
    std::cout << "*       CV Drone sample program       *" << std::endl;
    std::cout << "*           - How to play -           *" << std::endl;
    std::cout << "***************************************" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "* - Controls -                        *" << std::endl;
    std::cout << "*    'Space' -- Takeoff/Landing       *" << std::endl;
    std::cout << "*    'Up'    -- Move forward          *" << std::endl;
    std::cout << "*    'Down'  -- Move backward         *" << std::endl;
    std::cout << "*    'Left'  -- Turn left             *" << std::endl;
    std::cout << "*    'Right' -- Turn right            *" << std::endl;
    std::cout << "*    'Q'     -- Move upward           *" << std::endl;
    std::cout << "*    'A'     -- Move downward         *" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "* - Others -                          *" << std::endl;
    std::cout << "*    'C'     -- Change camera         *" << std::endl;
    std::cout << "*    'Esc'   -- Exit                  *" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "***************************************" << std::endl;

    while (1) {
        // Key input
        int key = cv::waitKey(33);

		if (key != -1) std::cout << "Key pressed: " << key << std::endl;
		
        if (key == 0x1b) break;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (key == 'i' || key == CV_VK_UP)    vx =  1.0;
		//if (key == 2490368) vx = 1.0; // Key up
        if (key == 'k' || key == CV_VK_DOWN)  vx = -1.0;
		//if (key == 2621440) vx = -1.0; // Key down
        if (key == 'u' || key == CV_VK_LEFT)  vr =  1.0;
		//if (key == 2424832) vr = 1.0; // Key left
        if (key == 'o' || key == CV_VK_RIGHT) vr = -1.0;
		//if (key == 2555904) vr = -1.0; // Key right
        if (key == 'j') vy =  1.0;
        if (key == 'l') vy = -1.0;
        if (key == 'q') vz =  1.0;
        if (key == 'a') vz = -1.0;
		//ardrone.move3D(1.0, 0.0, 0.0, 0.0);
		//std::cout << "vx: " << vx << ", vy: " << vy << ", vz: " << vz << ", vr: " << vr << std::endl;
        ardrone.move3D(vx, vy, vz, vr);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode % 4);
		
        // Display the image
        cv::imshow("Camera", image);
    }

    // See you
    ardrone.close();

    return 0;
}
Exemple #5
0
// --------------------------------------------------------------------------
// main(Number of arguments, Value of arguments)
// Description  : This is the main function.
// Return value : SUCCESS:0  ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
    // AR.Drone class
    ARDrone ardrone;

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

    // Image of AR.Drone's camera
    IplImage *image = ardrone.getImage();

    // Valuables for optical flow
    IplImage *gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
    IplImage *prev = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
    cvCvtColor(image, prev, CV_BGR2GRAY);
    IplImage *eig_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
    IplImage *tmp_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
    IplImage *prev_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
    IplImage *curr_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
    CvPoint2D32f *corners1 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));
    CvPoint2D32f *corners2 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));

    // Main loop
    while (!GetAsyncKeyState(VK_ESCAPE)) {
        // Update
        if (!ardrone.update()) break;

        // Get an image
        image = ardrone.getImage();

        // Take off / Landing
        if (KEY_PUSH(VK_SPACE)) {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (KEY_DOWN(VK_UP))    vx =  0.5;
        if (KEY_DOWN(VK_DOWN))  vx = -0.5;
        if (KEY_DOWN(VK_LEFT))  vr =  0.5;
        if (KEY_DOWN(VK_RIGHT)) vr = -0.5;
        if (KEY_DOWN('Q'))      vz =  0.5;
        if (KEY_DOWN('A'))      vz = -0.5;
        ardrone.move3D(vx, vy, vz, vr);

        // Convert the camera image to grayscale
        cvCvtColor(image, gray, CV_BGR2GRAY);

        // Detect features
        int corner_count = 50;
        cvGoodFeaturesToTrack(prev, eig_img, tmp_img, corners1, &corner_count, 0.1, 5.0, NULL);

        // Corner detected
        if (corner_count > 0) {
            char *status = (char*)malloc(corner_count * sizeof(char));

            // Calicurate optical flows
            CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.3);
            cvCalcOpticalFlowPyrLK(prev, gray, prev_pyramid, curr_pyramid, corners1, corners2, corner_count, cvSize(10, 10), 3, status, NULL, criteria, 0);

            // Drow the optical flows
            for (int i = 0; i < corner_count; i++) {
                cvCircle(image, cvPointFrom32f(corners1[i]), 1, CV_RGB (255, 0, 0));
                if (status[i]) cvLine(image, cvPointFrom32f(corners1[i]), cvPointFrom32f(corners2[i]), CV_RGB (0, 0, 255), 1, CV_AA, 0);
            }

            free(status);
        }

        // Save the last frame
        cvCopy(gray, prev);

        // Display the image
        cvShowImage("camera", image);
        cvWaitKey(1);
    }

    // Release the images
    cvReleaseImage(&gray);
    cvReleaseImage(&prev);
    cvReleaseImage(&eig_img);
    cvReleaseImage(&tmp_img);
    cvReleaseImage(&prev_pyramid);
    cvReleaseImage(&curr_pyramid);
    free(corners1);
    free(corners2);

    // See you
    ardrone.close();

    return 0;
}
Exemple #6
0
// --------------------------------------------------------------------------
// controls()
// This function sends commands to the drone using an Xbox Controller.
// --------------------------------------------------------------------------
void controls()
{
	// Check for Xbox Controller
	if(player1->IsConnected())
	{
		// 'A' button
		if(player1->GetState().Gamepad.wButtons & XINPUT_GAMEPAD_A)
		{
			// Change camera
			static int cameraToggle = 0;
			ardrone.setCamera(++cameraToggle%4);
		}
		// 'B' button
		if(player1->GetState().Gamepad.wButtons & XINPUT_GAMEPAD_B)
		{
			ardrone.emergency();
		}
		// 'X' button
		if(player1->GetState().Gamepad.wButtons & XINPUT_GAMEPAD_X)
		{
			ardrone.flatTrim();	
		}
		// 'Y' button
		/*if(player1->GetState().Gamepad.wButtons & XINPUT_GAMEPAD_Y)
		{

		}*/
		// 'Start' button
		if(player1->GetState().Gamepad.wButtons & XINPUT_GAMEPAD_START)
		{
			if (ardrone.onGround()) 
			{
				ardrone.takeoff();
			}
			else                    
			{
				ardrone.landing();
			}
		}
		// 'Back' button
		if(player1->GetState().Gamepad.wButtons & XINPUT_GAMEPAD_BACK)
		{
			// Exit the program.
			if (ardrone.onGround())
			{
				exit(0);
			}
		}
		// Left Thumb-Stick Press
		/*if(player1->GetState().Gamepad.wButtons & XINPUT_GAMEPAD_LEFT_THUMB)
		{

		}*/
		// Right Thumb-Stick Press
		/*if(player1->GetState().Gamepad.wButtons & XINPUT_GAMEPAD_RIGHT_THUMB)
		{

		}*/

		// AR.Drone flight controls
		//if (!ardrone.onGround()) 
		{
			// Check left thumbStick
			double leftThumbY = player1->GetState().Gamepad.sThumbLY / STICK_RANGE;
			double leftThumbX = player1->GetState().Gamepad.sThumbLX / STICK_RANGE;
			// Check the dead zone
			if (leftThumbY < STICK_DEAD_ZONE && leftThumbY > -STICK_DEAD_ZONE)
			{
				leftThumbY = 0;
			}
			if (leftThumbX < STICK_DEAD_ZONE && leftThumbX > -STICK_DEAD_ZONE)
			{
				leftThumbX = 0;
			}

			// Check left thumbStick
			double rightThumbY = player1->GetState().Gamepad.sThumbRY / TRIGGER_RANGE;
			double rightThumbX = player1->GetState().Gamepad.sThumbRX / TRIGGER_RANGE;
			// Check the dead zone
			if (rightThumbY < STICK_DEAD_ZONE && rightThumbY > -STICK_DEAD_ZONE)
			{
				rightThumbY = 0;
			}
			if (rightThumbX < STICK_DEAD_ZONE && rightThumbX> -STICK_DEAD_ZONE)
			{
				rightThumbX = 0;
			}
			
			double rightTrigger = player1->GetState().Gamepad.bRightTrigger / TRIGGER_RANGE;
			double leftTrigger = player1->GetState().Gamepad.bLeftTrigger / TRIGGER_RANGE;
			double gaz = rightTrigger - leftTrigger;
			
			// Move
			ardrone.move3D(rightThumbY*10, -rightThumbX*10, gaz*10, -leftThumbX*10);
				
		}
	}
}
// --------------------------------------------------------------------------
// 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[])
{
    // AR.Drone class
    ARDrone ardrone;

    // Initialize
    if (!ardrone.open()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

    // Battery
    std::cout << "Battery = " << ardrone.getBatteryPercentage() << "%" << std::endl;

    // Instructions
    std::cout << "***************************************" << std::endl;
    std::cout << "*       CV Drone sample program       *" << std::endl;
    std::cout << "*           - How to Play -           *" << std::endl;
    std::cout << "***************************************" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "* - Controls -                        *" << std::endl;
    std::cout << "*    'Space' -- Takeoff/Landing       *" << std::endl;
    std::cout << "*    'Up'    -- Move forward          *" << std::endl;
    std::cout << "*    'Down'  -- Move backward         *" << std::endl;
    std::cout << "*    'Left'  -- Turn left             *" << std::endl;
    std::cout << "*    'Right' -- Turn right            *" << std::endl;
    std::cout << "*    'Q'     -- Move upward           *" << std::endl;
    std::cout << "*    'A'     -- Move downward         *" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "* - Others -                          *" << std::endl;
    std::cout << "*    'T'     -- Track marker          *" << std::endl;
    std::cout << "*    'C'     -- Change camera         *" << std::endl;
    std::cout << "*    'Esc'   -- Exit                  *" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "***************************************" << std::endl;

    // Thresholds
    int minH = 0, maxH = 255;
    int minS = 0, maxS = 255;
    int minV = 0, maxV = 255;

    // XML save data
    std::string filename("thresholds.xml");
    cv::FileStorage fs(filename, cv::FileStorage::READ);

    // If there is a save file then read it
    if (fs.isOpened()) {
        maxH = fs["H_MAX"];
        minH = fs["H_MIN"];
        maxS = fs["S_MAX"];
        minS = fs["S_MIN"];
        maxV = fs["V_MAX"];
        minV = fs["V_MIN"];
        fs.release();
    }

    // Create a window
    cv::namedWindow("binalized");
    cv::createTrackbar("H max", "binalized", &maxH, 255);
    cv::createTrackbar("H min", "binalized", &minH, 255);
    cv::createTrackbar("S max", "binalized", &maxS, 255);
    cv::createTrackbar("S min", "binalized", &minS, 255);
    cv::createTrackbar("V max", "binalized", &maxV, 255);
    cv::createTrackbar("V min", "binalized", &minV, 255);
    cv::resizeWindow("binalized", 0, 0);

    // Main loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (key == 'i' || key == CV_VK_UP)    vx =  1.0;
        if (key == 'k' || key == CV_VK_DOWN)  vx = -1.0;
        if (key == 'u' || key == CV_VK_LEFT)  vr =  1.0;
        if (key == 'o' || key == CV_VK_RIGHT) vr = -1.0;
        if (key == 'j') vy =  1.0;
        if (key == 'l') vy = -1.0;
        if (key == 'q') vz =  1.0;
        if (key == 'a') vz = -1.0;

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode % 4);

        // Switch tracking ON/OFF
        static int track = 0;
        if (key == 't') track = !track;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // HSV image
        cv::Mat hsv;
        cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV_FULL);

        // Binalize
        cv::Mat binalized;
        cv::Scalar lower(minH, minS, minV);
        cv::Scalar upper(maxH, maxS, maxV);
        cv::inRange(hsv, lower, upper, binalized);

        // Show result
        cv::imshow("binalized", binalized);

        // De-noising
        cv::Mat kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
        cv::morphologyEx(binalized, binalized, cv::MORPH_CLOSE, kernel);
        //cv::imshow("morphologyEx", binalized);

        // Detect contours
        std::vector< std::vector<cv::Point> > contours;
        cv::findContours(binalized.clone(), contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);

        // Find largest contour
        int contour_index = -1;
        double max_area = 0.0;
        for (size_t i = 0; i < contours.size(); i++) {
            double area = fabs(cv::contourArea(contours[i]));
            if (area > max_area) {
                contour_index = i;
                max_area = area;
            }
        }

        // Object detected
        if (contour_index >= 0) {
            // Moments
            cv::Moments moments = cv::moments(contours[contour_index], true);
            double marker_y = (int)(moments.m01 / moments.m00);
            double marker_x = (int)(moments.m10 / moments.m00);

            // Show result
            cv::Rect rect = cv::boundingRect(contours[contour_index]);
            cv::rectangle(image, rect, cv::Scalar(0, 255, 0));

            // Tracking
            if (track) {
                const double kp = 0.005;
                vx = 0.1;
                vy = 0.0;
                vz = kp * (binalized.rows / 2 - marker_y);
                vr = kp * (binalized.cols / 2 - marker_x);
            }
        }

        // Display the image
        cv::putText(image, (track) ? "track on" : "track off", cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, (track) ? cv::Scalar(0, 0, 255) : cv::Scalar(0, 255, 0), 1, cv::LINE_AA);
        cv::imshow("camera", image);
        ardrone.move3D(vx, vy, vz, vr);
    }

    // Save thresholds
    fs.open(filename, cv::FileStorage::WRITE);
    if (fs.isOpened()) {
        cv::write(fs, "H_MAX", maxH);
        cv::write(fs, "H_MIN", minH);
        cv::write(fs, "S_MAX", maxS);
        cv::write(fs, "S_MIN", minS);
        cv::write(fs, "V_MAX", maxV);
        cv::write(fs, "V_MIN", minV);
        fs.release();
    }

    // See you
    ardrone.close();

    return 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)
{
    // AR.Drone class
    ARDrone ardrone;

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

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

    // Map
    cv::Mat map = cv::Mat::zeros(500, 500, CV_8UC3);

    // Kalman filter
    cv::KalmanFilter kalman(6, 4, 0);

    // Sampling time [s]
    const double dt = 0.033;

    // Transition matrix (x, y, z, vx, vy, vz)
    cv::Mat1f F(6, 6);
    F << 1.0, 0.0, 0.0,  dt, 0.0, 0.0,
         0.0, 1.0, 0.0, 0.0,  dt, 0.0,
         0.0, 0.0, 1.0, 0.0, 0.0,  dt,
         0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
         0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
         0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
    kalman.transitionMatrix = F;

    // Measurement matrix (0, 0, z, vx, vy, vz)
    cv::Mat1f H(4, 6);
    H << 0, 0, 1, 0, 0, 0,
         0, 0, 0, 1, 0, 0,
         0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 1;
    kalman.measurementMatrix = H;

    // Process noise covairance (x, y, z, vx, vy, vz)
    cv::Mat1f Q(6, 6);
    Q << 0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
         0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
         0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
         0.0, 0.0, 0.0, 0.3, 0.0, 0.0,
         0.0, 0.0, 0.0, 0.0, 0.3, 0.0,
         0.0, 0.0, 0.0, 0.0, 0.0, 0.3;
    kalman.processNoiseCov = Q;

    // Measurement noise covariance (z, vx, vy, vz)
    cv::Mat1f R(4, 4);
    R << 0.1, 0.0, 0.00, 0.00,
         0.0, 0.1, 0.00, 0.00,
         0.0, 0.0, 0.05, 0.00,
         0.0, 0.0, 0.00, 0.05;
    kalman.measurementNoiseCov = R;

    // Main loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Update
        if (!ardrone.update()) break;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // Prediction
        cv::Mat prediction = kalman.predict();

        // Altitude
        double altitude = ardrone.getAltitude();

        // Orientations
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();

        // Velocities
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        cv::Mat V  = (cv::Mat1f(3,1) << vx, vy, vz);

        // Rotation matrices
        cv::Mat RZ = (cv::Mat1f(3,3) <<   cos(yaw), -sin(yaw),        0.0,
                                          sin(yaw),  cos(yaw),        0.0,
                                               0.0,       0.0,        1.0);
        cv::Mat RY = (cv::Mat1f(3,3) << cos(pitch),       0.0,  sin(pitch),
                                               0.0,       1.0,        0.0,
                                       -sin(pitch),       0.0,  cos(pitch));
        cv::Mat RX = (cv::Mat1f(3,3) <<        1.0,       0.0,        0.0,
                                               0.0, cos(roll), -sin(roll),
                                               0.0, sin(roll),  cos(roll));

        // Time [s]
        static int64 last = cv::getTickCount();
        double dt = (cv::getTickCount() - last) / cv::getTickFrequency();
        last = cv::getTickCount();

        // Local movements (z, vx, vy, vz)
        cv::Mat1f M = RZ * RY * RX * V * dt;
        cv::Mat measurement = (cv::Mat1f(4,1) << altitude, M(0,0), M(1,0), M(2,0));

        // Correction
        cv::Mat1f estimated = kalman.correct(measurement);

        // Position (x, y, z)
        double pos[3] = {estimated(0,0), estimated(1,0), estimated(2,0)};
        printf("x = %3.2fm, y = %3.2fm, z = %3.2fm\n", pos[0], pos[1], pos[2]);

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
        if (key == 0x260000) x =  1.0;
        if (key == 0x280000) x = -1.0;
        if (key == 0x250000) r =  1.0;
        if (key == 0x270000) r = -1.0;
        if (key == 'q')      z =  1.0;
        if (key == 'a')      z = -1.0;
        ardrone.move3D(x, y, z, r);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode%4);

        // Display the image
        cv::circle(map, cv::Point(-pos[1]*100.0 + map.cols/2, -pos[0]*100.0 + map.rows/2), 2, CV_RGB(255,0,0));
        cv::imshow("map", map);
        cv::imshow("camera", image);
    }

    // See you
    ardrone.close();

    return 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[])
{
    // AR.Drone class
    ARDrone ardrone;

    // Initialize
    if (!ardrone.open()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

    // Battery
    std::cout << "Battery = " << ardrone.getBatteryPercentage() << " [%]" << std::endl;

    // Map
    cv::Mat map = cv::Mat::zeros(500, 500, CV_8UC3);

    // Position matrix
    cv::Mat P = cv::Mat::zeros(3, 1, CV_64FC1);

    // Main loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // Altitude
        double altitude = ardrone.getAltitude();

        // Orientations
        double roll = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw = ardrone.getYaw();

        // Velocities
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        cv::Mat V = (cv::Mat1f(3, 1) << vx, vy, vz);

        // Rotation matrices
        cv::Mat RZ = (cv::Mat1f(3, 3) << cos(yaw), -sin(yaw), 0.0,
                                         sin(yaw),  cos(yaw), 0.0,
                                              0.0,       0.0, 1.0);
        cv::Mat RY = (cv::Mat1f(3, 3) << cos(pitch), 0.0, sin(pitch),
                                                0.0, 1.0,        0.0,
                                        -sin(pitch), 0.0, cos(pitch));
        cv::Mat RX = (cv::Mat1f(3, 3) << 1.0,       0.0,        0.0,
                                         0.0, cos(roll), -sin(roll),
                                         0.0, sin(roll),  cos(roll));

        // Time [s]
        static int64 last = cv::getTickCount();
        double dt = (cv::getTickCount() - last) / cv::getTickFrequency();
        last = cv::getTickCount();

        // Dead-reckoning
        P = P + RZ * RY * RX * V * dt;

        // Position (x, y, z)
        double pos[3] = { P.at<double>(0, 0), P.at<double>(1, 0), P.at<double>(2, 0) };
        std::cout << "x = " << pos[0] << "[m], " << "y = " << pos[1] << "[m], " << "z = " << pos[2] << "[m]" << std::endl;

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
        if (key == 'i' || key == CV_VK_UP)    vx =  1.0;
        if (key == 'k' || key == CV_VK_DOWN)  vx = -1.0;
        if (key == 'u' || key == CV_VK_LEFT)  vr =  1.0;
        if (key == 'o' || key == CV_VK_RIGHT) vr = -1.0;
        if (key == 'j') vy =  1.0;
        if (key == 'l') vy = -1.0;
        if (key == 'q') vz =  1.0;
        if (key == 'a') vz = -1.0;
        ardrone.move3D(x, y, z, r);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode % 4);

        // Display the image
        cv::circle(map, cv::Point(-pos[1] * 100.0 + map.cols / 2, -pos[0] * 100.0 + map.rows / 2), 2, CV_RGB(255, 0, 0));
        cv::imshow("map", map);
        cv::imshow("camera", image);
    }

    // See you
    ardrone.close();

    return 0;
}
Exemple #10
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)
{
    // AR.Drone class
    ARDrone ardrone;

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

    // 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("***************************************\n\n");

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

        // Update
        if (!ardrone.update()) break;

        // Get an image
        IplImage *image = ardrone.getImage();

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (key == 0x260000) vx =  1.0;
        if (key == 0x280000) vx = -1.0;
        if (key == 0x250000) vr =  1.0;
        if (key == 0x270000) vr = -1.0;
        if (key == 'q')      vz =  1.0;
        if (key == 'a')      vz = -1.0;
        ardrone.move3D(vx, vy, vz, vr);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode%4);

        // Display the image
        cvShowImage("camera", image);
    }

    // See you
    ardrone.close();

    return 0;
}