구차니의 잡동사니 모음

void pwmSetMode (int mode)

{

  if ((wiringPiMode == WPI_MODE_PINS) || (wiringPiMode == WPI_MODE_PHYS) || (wiringPiMode == WPI_MODE_GPIO))

  {

    if (mode == PWM_MODE_MS)

      *(pwm + PWM_CONTROL) = PWM0_ENABLE | PWM1_ENABLE | PWM0_MS_MODE | PWM1_MS_MODE ;

    else

      *(pwm + PWM_CONTROL) = PWM0_ENABLE | PWM1_ENABLE ;

  }

}

void pwmSetRange (unsigned int range)

{

  if ((wiringPiMode == WPI_MODE_PINS) || (wiringPiMode == WPI_MODE_PHYS) || (wiringPiMode == WPI_MODE_GPIO))

  {

    *(pwm + PWM0_RANGE) = range ; delayMicroseconds (10) ;

    *(pwm + PWM1_RANGE) = range ; delayMicroseconds (10) ;

  }

}

#include <opencv2/core/core.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>

#include <wiringPi.h>

#define MIN_POS 30

#define MAX_POS 110

using namespace cv;

using namespace std;

bool bMouse = true;

Point mpos(160,120);

void CallBackFunc(int event, int x, int y, int flags, void* userdata)

{

        switch(event)

        {

        default:

                break;

        case EVENT_LBUTTONDOWN:

                cout << "EVENT_LBUTTONDOWN" << endl;

                cout << "pos (" << x << "," << y << ")" << endl;

                mpos.x = x;

                mpos.y = y;

                bMouse = true;

                break;

        case EVENT_LBUTTONUP:

                cout << "EVENT_LBUTTONUP" << endl;

                break;

        case EVENT_RBUTTONDOWN:

                cout << "EVENT_RBUTTONDOWN" << endl;

                bMouse = true;

                break;

        }

}

void init_wiringpi()

{

        if(wiringPiSetup() == -1) exit(0);

        pinMode(1, PWM_OUTPUT);

        pwmSetMode(PWM_MODE_MS);

        pwmSetRange(1024);

        pwmSetClock(400);

        pinMode(23, PWM_OUTPUT);

        pwmSetMode(PWM_MODE_MS);

        pwmSetRange(1024);

        pwmSetClock(400);

        pwmWrite(1,     MIN_POS + (MAX_POS - MIN_POS) / 2);

        pwmWrite(23,    MIN_POS + (MAX_POS - MIN_POS) / 2);

}

void setPWM_1(int val)

{

        pwmWrite(23, val);

}

void setPWM_2(int val)

{

        pwmWrite(1, val);

}

void on_trackbar_1(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_1(val);

        cout << "pos 1 [" << pos << ":" << val << "]" << endl;

}

void on_trackbar_2(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_2(val);

        cout << "pos 2 [" << pos << ":" << val << "]" << endl;

}

void on_trackbar_3(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

//      setPWM_1(val);

//      setPWM_2(val);

        setTrackbarPos("track 1", "cam 1", pos);

        setTrackbarPos("track 2", "cam 2", pos);

        cout << "pos 3 [" << pos << ":" << val << "]" << endl;

}

int main(int argc, char** argv)

{

        // opencv 2 style

        VideoCapture cap(0);

        int pos[2];

        init_wiringpi();

        if(!cap.isOpened())

        {

                cout << "No camera detected" << endl;

                return -1;

        }

        else

        {

                cout << "In capture ..." << endl;

                cap.set(CV_CAP_PROP_FRAME_WIDTH, 320);

                cap.set(CV_CAP_PROP_FRAME_HEIGHT, 240);

        }

/*

        VideoCapture cap2(1);

        if(!cap2.isOpened())

        {

                cout << "No camera detected" << endl;

                return -1;

        }

        else

        {

                cout << "In capture ..." << endl;

                cap2.set(CV_CAP_PROP_FRAME_WIDTH, 320);

                cap2.set(CV_CAP_PROP_FRAME_HEIGHT, 240);

        }

*/

        namedWindow("cam 1", WINDOW_AUTOSIZE );

//      namedWindow("cam 2", WINDOW_AUTOSIZE );

        namedWindow("control", WINDOW_AUTOSIZE );

        namedWindow("crop", WINDOW_AUTOSIZE );

        createTrackbar("track 1", "cam 1", &pos[0], 100, on_trackbar_1 );

//      createTrackbar("track 2", "cam 2", &pos[1], 100, on_trackbar_2 );

//      createTrackbar("track 3", "control", &pos[0], 100, on_trackbar_3 );

        setTrackbarPos("track 1", "cam 1", 50);

//      setTrackbarPos("track 2", "cam 2", 50);

//      setTrackbarPos("track 3", "control", 50);

        setMouseCallback("cam 1", CallBackFunc, NULL);

        Mat frame;

        Mat result;

        Mat tMat;

        Mat crop;

        double minVal;

        double maxVal;

        Point minLoc;

        Point maxLoc;

        Point matchLoc;

        for(;;)

        {

                Point pt[2];

                if(!cap.read(frame)) break;

                        pt[0].x = 150;

                        pt[0].y = 110;

                        pt[1].x = pt[0].x + 20;

                        pt[1].y = pt[0].y + 20;

                if(bMouse)

                {

                        tMat = frame.clone();

                        crop = Mat(tMat, Rect(mpos.x - 10, mpos.y - 10, 20, 20));

                        imshow("crop", crop);

                }

                matchTemplate( frame, crop, result, CV_TM_SQDIFF );

                normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );

                minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

                matchLoc = minLoc;

                cout << "matchLoc : " << matchLoc << endl;

                rectangle(frame, pt[0], pt[1], Scalar(0,0,0),2);

                rectangle(frame, Point(mpos.x - 10, mpos.y - 10), Point(mpos.x + 10, mpos.y + 10), Scalar(0,0,255),2);

                rectangle(frame, matchLoc, Point( matchLoc.x + 20 , matchLoc.y + 20 ), Scalar(0,255,0), 2);

#define SERVO_ANG       180

#define CAM_FOV         60

                if(bMouse)

                {

                        int tpos = getTrackbarPos("track 1", "cam 1");

//                      int value = tpos + (matchLoc.x - 160) * 3 * 2/ 100;

//                      int value = tpos + (100 / (SERVO_ANG / CAM_FOV)) * (matchLoc.x / 320) ;

                        int value = tpos + ((matchLoc.x - 160) * 60 / 3 /  320);

                        cout << "---- calc : " <<  value << endl;

                        setTrackbarPos("track 1", "cam 1", value);

                        bMouse = false;

                }

                imshow("cam 1", frame);

/*

                if(!cap2.read(frame)) break;

                rectangle(frame, pt[0], pt[1], Scalar(0,0,0),2);

                matchTemplate( frame, crop, result, CV_TM_SQDIFF );

                normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );

                minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

                matchLoc = minLoc;

                cout << "matchLoc : " << matchLoc << endl;

                rectangle(frame, matchLoc, Point( matchLoc.x + 20 , matchLoc.y + 20 ), Scalar(0,255,0), 2);

                        imshow("cam 2", frame);

*/

                if(waitKey(30) >= 0) break;

        }

        return 0;

} 

#include <opencv2/core/core.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>

#include <wiringPi.h>

#define MIN_POS 30

#define MAX_POS 110

using namespace cv;

using namespace std;

bool bMouse = true;

Point mpos(160,120);

void CallBackFunc(int event, int x, int y, int flags, void* userdata)

{

        switch(event)

        {

        default:

                break;

        case EVENT_LBUTTONDOWN:

                cout << "EVENT_LBUTTONDOWN" << endl;

                cout << "pos (" << x << "," << y << ")" << endl;

                mpos.x = x;

                mpos.y = y;

                bMouse = true;

                break;

        case EVENT_LBUTTONUP:

                cout << "EVENT_LBUTTONUP" << endl;

                break;

        case EVENT_RBUTTONDOWN:

                cout << "EVENT_RBUTTONDOWN" << endl;

                bMouse = true;

                break;

        }

}

void init_wiringpi()

{

        if(wiringPiSetup() == -1) exit(0);

        pinMode(1, PWM_OUTPUT);

        pwmSetMode(PWM_MODE_MS);

        pwmSetRange(1024);

        pwmSetClock(400);

        pinMode(23, PWM_OUTPUT);

        pwmSetMode(PWM_MODE_MS);

        pwmSetRange(1024);

        pwmSetClock(400);

        pwmWrite(1,     MIN_POS + (MAX_POS - MIN_POS) / 2);

        pwmWrite(23,    MIN_POS + (MAX_POS - MIN_POS) / 2);

}

void setPWM_1(int val)

{

        pwmWrite(23, val);

}

void setPWM_2(int val)

{

        pwmWrite(1, val);

}

void on_trackbar_1(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_1(val);

        cout << "pos 1 [" << pos << ":" << val << "]" << endl;

}

void on_trackbar_2(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_2(val);

        cout << "pos 2 [" << pos << ":" << val << "]" << endl;

}

void on_trackbar_3(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_1(val);

        setPWM_2(val);

        setTrackbarPos("track 1", "cam 1", pos);

        setTrackbarPos("track 2", "cam 2", pos);

        cout << "pos 3 [" << pos << ":" << val << "]" << endl;

}

int main(int argc, char** argv)

{

        // opencv 2 style

        VideoCapture cap(0);

        int pos[2];

        init_wiringpi();

        if(!cap.isOpened())

        {

                cout << "No camera detected" << endl;

                return -1;

        }

        else

        {

                cout << "In capture ..." << endl;

                cap.set(CV_CAP_PROP_FRAME_WIDTH, 320);

                cap.set(CV_CAP_PROP_FRAME_HEIGHT, 240);

        }

        VideoCapture cap2(1);

        if(!cap2.isOpened())

        {

                cout << "No camera detected" << endl;

                return -1;

        }

        else

        {

                cout << "In capture ..." << endl;

                cap2.set(CV_CAP_PROP_FRAME_WIDTH, 320);

                cap2.set(CV_CAP_PROP_FRAME_HEIGHT, 240);

        }

        namedWindow("cam 1", WINDOW_AUTOSIZE );

        namedWindow("cam 2", WINDOW_AUTOSIZE );

        namedWindow("control", WINDOW_AUTOSIZE );

        namedWindow("crop", WINDOW_AUTOSIZE );

        createTrackbar("track 1", "cam 1", &pos[0], 100, on_trackbar_1 );

        createTrackbar("track 2", "cam 2", &pos[1], 100, on_trackbar_2 );

        createTrackbar("track 3", "control", &pos[0], 100, on_trackbar_3 );

        setTrackbarPos("track 1", "cam 1", 50);

        setTrackbarPos("track 2", "cam 2", 50);

        setTrackbarPos("track 3", "control", 50);

        setMouseCallback("cam 1", CallBackFunc, NULL);

        Mat frame;

        Mat result;

        Mat tMat;

        Mat crop;

        double minVal;

        double maxVal;

        Point minLoc;

        Point maxLoc;

        Point matchLoc;

        for(;;)

        {

                Point pt[2];

                if(!cap.read(frame)) break;

                        pt[0].x = 150;

                        pt[0].y = 110;

                        pt[1].x = pt[0].x + 20;

                        pt[1].y = pt[0].y + 20;

                if(bMouse)

                {

                        tMat = frame.clone();

                        crop = Mat(tMat, Rect(mpos.x - 10, mpos.y - 10, 20, 20));

                        imshow("crop", crop);

                }

                matchTemplate( frame, crop, result, CV_TM_SQDIFF );

                normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );

                minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

                matchLoc = minLoc;

                cout << "matchLoc : " << matchLoc << endl;

                rectangle(frame, pt[0], pt[1], Scalar(0,0,0),2);

                rectangle(frame, Point(mpos.x - 10, mpos.y - 10), Point(mpos.x + 10, mpos.y + 10), Scalar(0,0,255),2);

                rectangle(frame, matchLoc, Point( matchLoc.x + 20 , matchLoc.y + 20 ), Scalar(0,255,0), 2);

#define SERVO_ANG       180

#define CAM_FOV         60

                if(bMouse)

                {

                        int tpos = getTrackbarPos("track 1", "cam 1");

                        int value = tpos + ((matchLoc.x - 160) * 60 / 3 /  320);

                        cout << "---- calc : " <<  value << endl;

                        setTrackbarPos("track 1", "cam 1", value);

                }

                imshow("cam 1", frame);

                if(!cap2.read(frame)) break;

                rectangle(frame, pt[0], pt[1], Scalar(0,0,0),2);

                matchTemplate( frame, crop, result, CV_TM_SQDIFF );

                normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );

                minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

                matchLoc = minLoc;

                cout << "matchLoc : " << matchLoc << endl;

                rectangle(frame, matchLoc, Point( matchLoc.x + 20 , matchLoc.y + 20 ), Scalar(0,255,0), 2);

                if(bMouse)

                {

                        int tpos = getTrackbarPos("track 2", "cam 2");

                        int value = tpos + ((matchLoc.x - 160) * 60 / 3 /  320);

                        cout << "---- calc : " <<  value << endl;

                        setTrackbarPos("track 2", "cam 2", value);

                        bMouse = false;

                }

                imshow("cam 2", frame);

                if(waitKey(30) >= 0) break;

        }

        return 0;

}