C++ ARDrone::getBatteryPercentage方法代码示例

// --------------------------------------------------------------------------
// controls()
// This function displays sensory data from the drone to the console.
// --------------------------------------------------------------------------
void display()
{
		clearScreen();
        // Orientation
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();
        printf("Roll \t\t = %3.2f [deg]\n", roll  * RAD_TO_DEG);
        printf("Pitch\t\t = %3.2f [deg]\n", pitch * RAD_TO_DEG);
        printf("Yaw  \t\t = %3.2f [deg]\n", yaw   * RAD_TO_DEG);

        // Altitude
        double altitude = ardrone.getAltitude();
        printf("Altitude\t = %3.2f [m]\n", altitude);

        // Velocity
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        printf("X Velocity\t = %3.2f [m/s]\n", vx);
        printf("Y Velocity\t = %3.2f [m/s]\n", vy);
        printf("Z Velocity\t = %3.2f [m/s]\n", vz);

        // Battery
        int battery = ardrone.getBatteryPercentage();
        printf("Battery\t\t = %d [%%]\n", battery);
}

int main(){
	cout<<"Hol<"<<endl;

	//Mat img=imread("/home/mottamx/Pictures/batman.jpg");
	//namedWindow("batman");
	//imshow("batman", img);
	//waitKey(1);
	cout<<"Hol<<"<<endl;
	ARDrone ardrone;
	if (!ardrone.open()){
		cout<<"error de comunicación"<<endl;
		return -1;
	}
	cout<<"Bat: "<<ardrone.getBatteryPercentage()<<endl;
	sleep(2);
	cout<<"Bat: "<<ardrone.getBatteryPercentage()<<endl;
	sleep(2);
	cout<<"Bat: "<<ardrone.getBatteryPercentage()<<endl;
	sleep(2);
	time_t start, end;
	time (&start);
	cout.flush();
	double elapsed=0;
	namedWindow("dron");
	namedWindow("dron2");
	while(elapsed<5){
		//sleep(2);
		IplImage *im=ardrone.getImage();
		Mat img = Mat(im);
		Mat img2;
		resize(img, img2, Size(), 2,2, INTER_AREA);
		resize(img, img, Size(), 2,2, INTER_LANCZOS4);
		imshow("dron", img);
		waitKey(1);
		imshow("dron2", img2);
		waitKey(1);
		time(&end);
		elapsed=difftime(end,start);
	}
	cout<<"Tiempo: "<<setprecision(3)<<elapsed<<"segundos"<<endl;
	cout<<"<dios"<<endl;

	ardrone.close();
	ardrone.emergency();
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 == 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);

        // Display the image
        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[])
{
	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();
//.........这里部分代码省略.........

// --------------------------------------------------------------------------
// 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
    int battery = ardrone.getBatteryPercentage();
    printf("ardrone.battery = %d [%%]\n", battery);

    // Instructions
    printf("***************************************\n");
    printf("*       CV Drone sample program       *\n");
    printf("*           - Haw 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("*    'Shift+Up'    -- Move upward     *\n");
    printf("*    'Shift+Down'  -- Move downward   *\n");
    printf("*    'Shift+Left'  -- Move left       *\n");
    printf("*    'Shift+Right' -- Move right      *\n");
    printf("*                                     *\n");
    printf("* - Others -                          *\n");
    printf("*    'C'     -- Change camera         *\n");
    printf("*    'Esc'   -- Exit                  *\n");
    printf("*                                     *\n");
    printf("***************************************\n\n");

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

        // Get an image
        IplImage *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_SHIFT)) {
            if (KEY_DOWN(VK_UP))    vz =  1.0;
            if (KEY_DOWN(VK_DOWN))  vz = -1.0;
            if (KEY_DOWN(VK_LEFT))  vy =  1.0;
            if (KEY_DOWN(VK_RIGHT)) vy = -1.0;
        }
        else {
            if (KEY_DOWN(VK_UP))    vx =  1.0;
            if (KEY_DOWN(VK_DOWN))  vx = -1.0;
            if (KEY_DOWN(VK_LEFT))  vr =  1.0;
            if (KEY_DOWN(VK_RIGHT)) vr = -1.0;
        }
        ardrone.move3D(vx, vy, vz, vr);

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

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

    // See you
    ardrone.close();

    return 0;
}

int runArdrone(void)
{
    // AR.Drone class
    ARDrone ardrone;

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

    // Battery
    battery = ardrone.getBatteryPercentage();

    int tmpOrder = -99;
    int nbLabelPos = 8;
    int occLabelPos[nbLabelPos];
    list<int> listOrder(5,-1);


float vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;

    while (1) {
        usleep(33000);

        imgFromKarmen = ardrone.getImage();

        battery = ardrone.getBatteryPercentage();
        tmpOrder = -99;

        for (int i = 0; i < nbLabelPos; i++)
            occLabelPos[i] = 0;

        // Key input
        //       int key = cvWaitKey(0);
        //        if (key == 0x1b) break; //TODO

        // Get an image

        // Move
        vx = 0.0;
        vy = 0.0;
        vz = 0.0;
        vr = 0.0;

        if(order == 100){
            orderName  = "Land";
            ardrone.landing();   // Land
            newOrder = false;
        }
        else{
            if(newOrder){
                newOrder = false;
                listOrder.pop_front();
                listOrder.push_back(order);
            }

            for (list<int>::iterator it=listOrder.begin(); it != listOrder.end(); ++it){
                if(*it >= 0 && *it < nbLabelPos)
                    occLabelPos[*it]++;
            }

            for (int i = 0; i < nbLabelPos; i++){
                if(occLabelPos[i] >= 3){
                    tmpOrder = i;
                    break;
                }
            }


            switch (tmpOrder) {
            case 0 :
                orderName = "Up";
                vz = 1.0;
                break;
            case 1 :
                orderName  = "Down";
                vz = -1.0;
                break;
            case 2 :
                orderName  = "Left";
                vy = 1.0;
                break;
            case 3 :
                orderName  = "Right";
                vy = -1.0;
                break;
            case 4 :
                orderName  = "Land";
                ardrone.landing();   // Land
                break;
            case 5 :
                if (ardrone.onGround()){
                    orderName  = "Take Off";
                    ardrone.takeoff();    // Take-off
                }
                break;
            case 6 :
                orderName  = "Forward";
                vx = 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[])
{
    // AR.Drone class
    ARDrone ardrone;

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

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

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

        // Orientation
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();
        std::cout << "ardrone.roll  = " << roll  * RAD_TO_DEG << " [deg]" << std::endl;
        std::cout << "ardrone.pitch = " << pitch * RAD_TO_DEG << " [deg]" << std::endl;
        std::cout << "ardrone.yaw   = " << yaw   * RAD_TO_DEG << " [deg]" << std::endl;

        // Altitude
        double altitude = ardrone.getAltitude();
        std::cout << "ardrone.altitude = " << altitude << " [m]" << std::endl;

        // Velocity
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        std::cout << "ardrone.vx = " << vx << " [m/s]" << std::endl;
        std::cout << "ardrone.vy = " << vy << " [m/s]" << std::endl;
        std::cout << "ardrone.vz = " << vz << " [m/s]" << std::endl;

        // Battery
        int battery = ardrone.getBatteryPercentage();
        std::cout << "ardrone.battery = " << battery << " [%%]" << 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 == 0x260000) x =  1.0;
        if (key == 0x280000) x = -1.0;
        if (key == 0x250000) r =  1.0;
        if (key == 0x270000) r = -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::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()) {
        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;
}

// --------------------------------------------------------------------------
// 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;

	zbar::ImageScanner scanner;
	scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
    
	// Main loop
	while (1) {
		// Key input
		int key = cv::waitKey(33);
		if (key == 0x1b) break;

		// Get an image
		cv::Mat image = ardrone.getImage();
		//cout << image.channels() << " channels " << endl;
		cvtColor(image, image, CV_RGB2GRAY);
		//cout << image.channels() << " channels efter RGB2GRAY" << endl;
		cv::Mat imgout;
		//cout << image.cols << "image ardrone cols" << endl;
		cvtColor(image, imgout, CV_GRAY2RGB);
		//cvtColor(image, imgout);


		// 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)    x = 1.0;
		if (key == 'k' || key == CV_VK_DOWN)  x = -1.0;
		if (key == 'u' || key == CV_VK_LEFT)  r = 1.0;
		if (key == 'o' || key == CV_VK_RIGHT) r = -1.0;
		if (key == 'j') y = 1.0;
		if (key == 'l') y = -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);

		//cv::cvtColor(image, image, CV_GRAY2RGB);
		//cv::cvtColor(image, image, CV_RGB2GRAY);
		int width = image.cols;
		int height = image.rows;
		uchar *raw = (uchar *) image.data;
		// wrap image data  
		zbar::Image imageQR(width, height, "Y800", raw, width * height);
		// scan the image for barcodes  
		int n = scanner.scan(imageQR);
		// extract results  
		for (zbar::Image::SymbolIterator symbol = imageQR.symbol_begin();
			symbol != imageQR.symbol_end();
			++symbol) {
			cout << "Inde i for loop" << endl;
			vector<Point> vp;
			// do something useful with results  
			cout << "decoded " << symbol->get_type_name()
				<< " symbol \"" << symbol->get_data() << '"' << " " << endl;
			int n = symbol->get_location_size();
			for (int i = 0; i<n; i++) {
				vp.push_back(Point(symbol->get_location_x(i), symbol->get_location_y(i)));
			}
			RotatedRect r = minAreaRect(vp);
			Point2f pts[4];
			r.points(pts);
			for (int i = 0; i<4; i++) {
				line(imgout, pts[i], pts[(i + 1) % 4], Scalar(255, 0, 0), 3);
			}
			cout << "Angle: " << r.angle << endl;
		}
		//if(imgout)
		imshow("imgout.jpg", imgout);
		//cout << imgout.cols << " cols" << imgout.rows << " rows" << endl;
		// clean up  
		imageQR.set_data(NULL, 0);
		//waitKey(); Med dette fjernet kan vi bruge Esc

        // Display the image from camera
        cv::imshow("camera", image);
//.........这里部分代码省略.........

// --------------------------------------------------------------------------
// 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;
    }

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

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

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

        // Orientation
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();
        printf("ardrone.roll  = %3.2f [deg]\n", roll  * RAD_TO_DEG);
        printf("ardrone.pitch = %3.2f [deg]\n", pitch * RAD_TO_DEG);
        printf("ardrone.yaw   = %3.2f [deg]\n", yaw   * RAD_TO_DEG);

        // Altitude
        double altitude = ardrone.getAltitude();
        printf("ardrone.altitude = %3.2f [m]\n", altitude);

        // Velocity
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        printf("ardrone.vx = %3.2f [m/s]\n", vx);
        printf("ardrone.vy = %3.2f [m/s]\n", vy);
        printf("ardrone.vz = %3.2f [m/s]\n", vz);

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

        // 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;
        ardrone.move3D(x, y, z, r);

        // 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;
}

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");
	//　
//.........这里部分代码省略.........

// --------------------------------------------------------------------------
// 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;
//.........这里部分代码省略.........

// --------------------------------------------------------------------------
// 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("  Q - ARDRONE_ANIM_PHI_M30_DEG\n");
    printf("  A - ARDRONE_ANIM_PHI_30_DEG\n");
    printf("  Z - ARDRONE_ANIM_THETA_M30_DEG\n");
    printf("  W - ARDRONE_ANIM_THETA_30_DEG\n");
    printf("  S - ARDRONE_ANIM_THETA_20DEG_YAW_200DEG\n");
    printf("  X - ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG\n");
    printf("  E - ARDRONE_ANIM_TURNAROUND\n");
    printf("  D - ARDRONE_ANIM_TURNAROUND_GODOWN\n");
    printf("  C - ARDRONE_ANIM_YAW_SHAKE\n");
    printf("  R - ARDRONE_ANIM_YAW_DANCE\n");
    printf("  F - ARDRONE_ANIM_PHI_DANCE\n");
    printf("  V - ARDRONE_ANIM_THETA_DANCE\n");
    printf("  T - ARDRONE_ANIM_VZ_DANCE\n");
    printf("  G - ARDRONE_ANIM_WAVE\n");
    printf("  B - ARDRONE_ANIM_PHI_THETA_MIXED\n");
    printf("  Y - ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED\n");
    printf("  H - ARDRONE_ANIM_FLIP_AHEAD\n");
    printf("  N - ARDRONE_ANIM_FLIP_BEHIND\n");
    printf("  U - ARDRONE_ANIM_FLIP_LEFT\n");
    printf("  J - ARDRONE_ANIM_FLIP_RIGHT\n");

    // Main loop
    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();
        }

        // Flight animations
        if (key == 'q') ardrone.setAnimation(ARDRONE_ANIM_PHI_M30_DEG,             1000);
        if (key == 'a') ardrone.setAnimation(ARDRONE_ANIM_PHI_30_DEG,              1000);
        if (key == 'z') ardrone.setAnimation(ARDRONE_ANIM_THETA_M30_DEG,           1000);
        if (key == 'w') ardrone.setAnimation(ARDRONE_ANIM_THETA_30_DEG,            1000);
        if (key == 's') ardrone.setAnimation(ARDRONE_ANIM_THETA_20DEG_YAW_200DEG,  1000);
        if (key == 'x') ardrone.setAnimation(ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG, 1000);
        if (key == 'e') ardrone.setAnimation(ARDRONE_ANIM_TURNAROUND,              5000);
        if (key == 'd') ardrone.setAnimation(ARDRONE_ANIM_TURNAROUND_GODOWN,       5000);
        if (key == 'c') ardrone.setAnimation(ARDRONE_ANIM_YAW_SHAKE,               2000);
        if (key == 'r') ardrone.setAnimation(ARDRONE_ANIM_YAW_DANCE,               5000);
        if (key == 'f') ardrone.setAnimation(ARDRONE_ANIM_PHI_DANCE,               5000);
        if (key == 'v') ardrone.setAnimation(ARDRONE_ANIM_THETA_DANCE,             5000);
        if (key == 't') ardrone.setAnimation(ARDRONE_ANIM_VZ_DANCE,                5000);
        if (key == 'g') ardrone.setAnimation(ARDRONE_ANIM_WAVE,                    5000);
        if (key == 'b') ardrone.setAnimation(ARDRONE_ANIM_PHI_THETA_MIXED,         5000);
        if (key == 'y') ardrone.setAnimation(ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED,  5000);
        if (key == 'h') ardrone.setAnimation(ARDRONE_ANIM_FLIP_AHEAD,                15);
        if (key == 'n') ardrone.setAnimation(ARDRONE_ANIM_FLIP_BEHIND,               15);
        if (key == 'u') ardrone.setAnimation(ARDRONE_ANIM_FLIP_LEFT,                 15);
        if (key == 'j') ardrone.setAnimation(ARDRONE_ANIM_FLIP_RIGHT,                15);

        // Display the image
        cvShowImage("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;

    // 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;
//.........这里部分代码省略.........

// --------------------------------------------------------------------------
// 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,
//.........这里部分代码省略.........