PID controller
trinhdh opened this issue · 5 comments
trinhdh commented
Does the library support PID controller ?
Thanks
puku0x commented
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
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
#20 (comment).
johneyoh commented
// Marker
cv::Point marker(binalized.cols / 2, binalized.rows / 2);
why binalized not defined!??
puku0x commented
Sorry, it is my mistake. Please fix it like this.
cv::Point marker(0, 0);
kyawthuyalwin commented
hay ,,,, friends,... could you describe your Kp , Ki and Kd tuned values for #20...?