trinhdh commented 9 years ago

Does the library support PID controller ?

Thanks

puku0x commented 9 years ago

PID controller ?

It's easy to implement, I have an example.

#include "ardrone/ardrone.h"

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

    // Switch to vertical camera
    ardrone.setCamera(1);

    // Thresholds
    int minH = 0, maxH = 179;
    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, 179);
    cv::createTrackbar("H min", "binalized", &minH, 179);
    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);

    // Marker
    //cv::Point marker(binalized.cols / 2, binalized.rows / 2);
    cv::Point marker(0, 0);

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

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

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

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

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

        // 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 (Closing)
        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 (int i = 0; i < (int)contours.size(); i++) {
            double area = fabs(cv::contourArea(contours[i]));
            if (area > max_area) {
                contour_index = i;
                max_area = area;
            }
        }

        // A marker detected
        if (contour_index >= 0) {
            // Moments
            cv::Moments moments = cv::moments(contours[contour_index], true);
            marker.y = (int)(moments.m01 / moments.m00);
            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));
            //cv::drawContours(image, contours, contour_index, cv::Scalar(0,255,0));
        }

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

        // Move using keyboard
        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;

        // Switch tracking ON/OFF
        static int track = 0;
        if (key == 't') track = !track;
        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_AA);

        // Marker tracking
        if (track) {
            // PID gains
            const double kp = 0.001;
            const double ki = 0.000;
            const double kd = 0.000;

            // Errors
            double error_x = (binalized.rows / 2 - marker.y);   // Error front/back
            double error_y = (binalized.cols / 2 - marker.x);   // Error left/right

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

            // Integral terms
            static double integral_x = 0.0, integral_y = 0.0;
            if (dt > 0.1) {
                // Reset
                integral_x = 0.0;
                integral_y = 0.0;
            }
            integral_x += error_x * dt;
            integral_y += error_y * dt;

            // Derivative terms
            static double previous_error_x = 0.0, previous_error_y = 0.0;
            if (dt > 0.1) {
                // Reset
                previous_error_x = 0.0;
                previous_error_y = 0.0;
            }
            double derivative_x = (error_x - previous_error_x) / dt;
            double derivative_y = (error_y - previous_error_y) / dt;
            previous_error_x = error_x;
            previous_error_y = error_y;

            // Command velocities
            vx = kp * error_x + ki * integral_x + kd * derivative_x;
            vy = kp * error_y + ki * integral_y + kd * derivative_y;
            vz = 0.0;
            vr = 0.0;
            std::cout << "(vx, vy)" << "(" << vx << "," << vy << ")" << std::endl;
        }

        // Move
        ardrone.move3D(vx, vy, vz, vr);

        // Display the image
        cv::imshow("camera", image);
    }

    // See you
    ardrone.close();

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

    return 0;
}

Please note that you should tune PID gains manually.

trinhdh commented 9 years ago

Thank you. It worked perfectly.

On Wed, Nov 5, 2014 at 3:06 AM, puku0x notifications@github.com wrote:

PID controller ?

It's easy to implement, I have an example.

include "ardrone/ardrone.h"

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

// Switch to vertical camera
ardrone.setCamera(1);

// Thresholds
int minH = 0, maxH = 179;
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, 179);
cv::createTrackbar("H min", "binalized", &minH, 179);
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);

// Marker
cv::Point marker(binalized.cols / 2, binalized.rows / 2);

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

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

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

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

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

    // 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 (Closing)
    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 (int i = 0; i < (int)contours.size(); i++) {
        double area = fabs(cv::contourArea(contours[i]));
        if (area > max_area) {
            contour_index = i;
            max_area = area;
        }
    }

    // A marker detected
    if (contour_index >= 0) {
        // Moments
        cv::Moments moments = cv::moments(contours[contour_index], true);
        marker.y = (int)(moments.m01 / moments.m00);
        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));
        //cv::drawContours(image, contours, contour_index, cv::Scalar(0,255,0));
    }

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

    // Move using keyboard
    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;

    // Switch tracking ON/OFF
    static int track = 0;
    if (key == 't') track = !track;
    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_AA);

    // Marker tracking
    if (track) {
        // PID gains
        const double kp = 0.001;
        const double ki = 0.000;
        const double kd = 0.000;

        // Errors
        double error_x = (binalized.rows / 2 - marker.y);   // Error front/back
        double error_y = (binalized.cols / 2 - marker.x);   // Error left/right

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

        // Integral terms
        static double integral_x = 0.0, integral_y = 0.0;
        if (dt > 0.1) {
            // Reset
            integral_x = 0.0;
            integral_y = 0.0;
        }
        integral_x += error_x * dt;
        integral_y += error_y * dt;

        // Derivative terms
        static double previous_error_x = 0.0, previous_error_y = 0.0;
        if (dt > 0.1) {
            // Reset
            previous_error_x = 0.0;
            previous_error_y = 0.0;
        }
        double derivative_x = (error_x - previous_error_x) / dt;
        double derivative_y = (error_y - previous_error_y) / dt;
        previous_error_x = error_x;
        previous_error_y = error_y;

        // Command velocities
        vx = kp * error_x + ki * integral_x + kd * derivative_x;
        vy = kp * error_y + ki * integral_y + kd * derivative_y;
        vz = 0.0;
        vr = 0.0;
        std::cout << "(vx, vy)" << "(" << vx << "," << vy << ")" << std::endl;
    }

    // Move
    ardrone.move3D(vx, vy, vz, vr);

    // Display the image
    cv::imshow("camera", image);
}

// See you
ardrone.close();

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

return 0;

}

Please note that you should tune PID gains manually.

— Reply to this email directly or view it on GitHub https://github.com/puku0x/cvdrone/issues/20#issuecomment-61778098.

johneyoh commented 8 years ago

// Marker cv::Point marker(binalized.cols / 2, binalized.rows / 2);

why binalized not defined!??

puku0x commented 8 years ago

Sorry, it is my mistake. Please fix it like this.

cv::Point marker(0, 0);

kyawthuyalwin commented 8 years ago

hay ,,,, friends,... could you describe your Kp , Ki and Kd tuned values for #20...?

puku0x / cvdrone

PID controller #20

include "ardrone/ardrone.h"