void ESCController::initialize(int controlled_esc[4], int ESCSanity[4], int frequency, int min, int max) { for (int i=0;i<4;i++) { ESC[i] = controlled_esc[i]; gpioSetPWMfrequency(ESC[i], frequency); gpioSetPWMrange(ESC[i], 1000000/frequency); sanity[i] = ESCSanity[i]; } outMin = min; outMax = max; }
void mygpioSetup() { if (gpioGetMode(18) != PI_OUTPUT) { (gpioSetMode(18, PI_OUTPUT)); } if (gpioGetMode(17) != PI_OUTPUT) { (gpioSetMode(17, PI_OUTPUT)); } if (gpioGetPWMrange(18) != 2000) { (gpioSetPWMrange(18, 2000)); } if (gpioGetPWMrange(17) != 2000) { (gpioSetPWMrange(17, 2000)); } if (gpioGetPWMfrequency(18) != 8000) { (gpioSetPWMfrequency(18, 8000)); } if (gpioGetPWMfrequency(17) != 8000) { (gpioSetPWMfrequency(17, 8000)); } }
int main(int argc, char* argv[]) { int counter; wchar_t auxstr[20]; printf("start!\n"); printf("PUCK MINH: %d\n",minH); printf("PUCK MAXH: %d\n",maxH); printf("PUCK MINS: %d\n",minS); printf("PUCK MAXS: %d\n",maxS); printf("PUCK MINV: %d\n",minV); printf("PUCK MAXV: %d\n",maxV); printf("ROBOT MINH: %d\n",RminH); printf("ROBOT MAXH: %d\n",RmaxH); printf("ROBOT MINS: %d\n",RminS); printf("ROBOT MAXS: %d\n",RmaxS); printf("ROBOT MINV: %d\n",RminV); printf("ROBOT MAXV: %d\n",RmaxV); printf("FPS: %d\n",fps); //pwm initialize if(gpioInitialise() < 0) return 1; // 読み込み画像ファイル名 char imgfile[] = "camera/photodir/capmallet1.png"; char imgfile2[] = "camera/photodir/capmallet2.png"; // 画像の読み込み img = cvLoadImage(imgfile, CV_LOAD_IMAGE_ANYCOLOR | CV_LOAD_IMAGE_ANYDEPTH); img2 = cvLoadImage(imgfile2, CV_LOAD_IMAGE_ANYCOLOR | CV_LOAD_IMAGE_ANYDEPTH); // 画像の表示用ウィンドウ生成 cvNamedWindow("circle_sample", CV_WINDOW_AUTOSIZE); cvNamedWindow("circle_sample2", CV_WINDOW_AUTOSIZE); //cvNamedWindow("cv_ColorExtraction"); // Init font cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, 0.4,0.4,0,1); IplImage* dst_img_mallett = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); IplImage* dst_img_pack = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); IplImage* dst_img2_mallett = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); IplImage* dst_img2_pack = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); //白抽出0,255,0,15,240,255 //黒抽出0, 255, 0, 50, 0, 100 //青検出0, 255, 50, 200, 100, 180 //cv_ColorExtraction(img, dst_img_mallett, CV_BGR2HSV, 0, 255, 50, 200, 100, 180); cv_ColorExtraction(img, dst_img_pack, CV_BGR2HSV, 0, 255, 0, 50, 0, 100); cv_ColorExtraction(img2, dst_img2_mallett, CV_BGR2HSV, 0, 255, 50, 200, 100, 180); cv_ColorExtraction(img2, dst_img2_pack, CV_BGR2HSV, 0, 255, 0, 50, 0, 100); //CvMoments moment_mallett; CvMoments moment_pack; CvMoments moment2_mallett; CvMoments moment2_pack; //cvSetImageCOI(dst_img_mallett, 1); cvSetImageCOI(dst_img_pack, 1); cvSetImageCOI(dst_img2_mallett, 1); cvSetImageCOI(dst_img2_pack, 1); //cvMoments(dst_img_mallett, &moment_mallett, 0); cvMoments(dst_img_pack, &moment_pack, 0); cvMoments(dst_img2_mallett, &moment2_mallett, 0); cvMoments(dst_img2_pack, &moment2_pack, 0); //座標計算 double m00_before = cvGetSpatialMoment(&moment_pack, 0, 0); double m10_before = cvGetSpatialMoment(&moment_pack, 1, 0); double m01_before = cvGetSpatialMoment(&moment_pack, 0, 1); double m00_after = cvGetSpatialMoment(&moment2_pack, 0, 0); double m10_after = cvGetSpatialMoment(&moment2_pack, 1, 0); double m01_after = cvGetSpatialMoment(&moment2_pack, 0, 1); double gX_before = m10_before/m00_before; double gY_before = m01_before/m00_before; double gX_after = m10_after/m00_after; double gY_after = m01_after/m00_after; double m00_mallett = cvGetSpatialMoment(&moment2_mallett, 0, 0); double m10_mallett = cvGetSpatialMoment(&moment2_mallett, 1, 0); double m01_mallett = cvGetSpatialMoment(&moment2_mallett, 0, 1); double gX_now_mallett = m10_mallett/m00_mallett; double gY_now_mallett = m01_mallett/m00_mallett; cvCircle(img2, cvPoint((int)gX_before, (int)gY_before), 50, CV_RGB(0,0,255), 6, 8, 0); cvLine(img2, cvPoint((int)gX_before, (int)gY_before), cvPoint((int)gX_after, (int)gY_after), cvScalar(0,255,0), 2); int target_destanceY = 480 - 30;//Y座標の距離を一定にしている。ディフェンスライン。 //パックの移動は直線のため、一次関数の計算を使って、その後の軌跡を予測する。 double a_inclination = (gY_after - gY_before) / (gX_after - gX_before); double b_intercept = gY_after - a_inclination * gX_after; printf("gX_after: %f\n",gX_after); printf("gY_after: %f\n",gY_after); printf("gX_before: %f\n",gX_before); printf("gY_before: %f\n",gY_before); printf("a_inclination: %f\n",a_inclination); printf("b_intercept: %f\n",b_intercept); int target_coordinateX = (int)((target_destanceY - b_intercept) / a_inclination); printf("target_coordinateX: %d\n",target_coordinateX); cvLine(img2, cvPoint((int)gX_after, (int)gY_after), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2); cvLine(img2, cvPoint(640, target_destanceY), cvPoint(0, target_destanceY), cvScalar(255,255,0), 2); cvLine(img2, cvPoint((int)gX_now_mallett, (int)gY_now_mallett), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2); cvPutText (img2, to_c_char((int)gX_now_mallett), cvPoint(460,30), &font, cvScalar(220,50,50)); cvPutText (img2, to_c_char((int)target_coordinateX), cvPoint(560,30), &font, cvScalar(50,220,220)); int amount_movement = gX_now_mallett - target_coordinateX; //2枚の画像比較1回で移動できる量の計算 int max_amount_movement = CAM_PIX_WIDTH * 0.54 / 1; //CAM_PIX_WIDTH:640, 比較にかかる時間:0.27*2, 端までの移動時間:1s int target_direction; if(amount_movement > 0){ if(max_amount_movement < amount_movement){ amount_movement = max_amount_movement; } target_direction = 0;//時計回り } else if(amount_movement < 0){ amount_movement = -amount_movement;//正の数にする if(max_amount_movement < amount_movement){ amount_movement = max_amount_movement; } target_direction = 1;//反時計回り } //pwm output double set_time_millis= 270 * amount_movement / max_amount_movement;//0.27ms*(0~1) gpioSetMode(18, PI_OUTPUT); gpioSetMode(19, PI_OUTPUT); gpioPWM(18, 128); gpioWrite(19, target_direction); int closest_frequency = gpioSetPWMfrequency(18, 2000); printf("setting_frequency: %d\n", closest_frequency); gpioSetTimerFunc(0, (int)set_time_millis, pwmReset); // 指定したウィンドウ内に画像を表示する cvShowImage("circle_sample", img); cvShowImage("circle_sample2", img2); while(1) { if(cv::waitKey(30) >= 0) { break; } } gpioTerminate(); //Clean up used images cvReleaseImage(&img); // cvReleaseImage (&dst_img); cvDestroyAllWindows() ; return 0; }
int main(int argc, char* argv[]) { CvMemStorage *contStorage = cvCreateMemStorage(0); CvSeq *contours; CvTreeNodeIterator polyIterator; int found = 0; int i; CvPoint poly_point; int fps=30; // ポリライン近似 CvMemStorage *polyStorage = cvCreateMemStorage(0); CvSeq *polys, *poly; // OpenCV variables CvFont font; printf("start!\n"); //pwm initialize if(gpioInitialise() < 0) return -1; //pigpio CW/CCW pin setup //X:18, Y1:14, Y2:15 gpioSetMode(18, PI_OUTPUT); gpioSetMode(14, PI_OUTPUT); gpioSetMode(15, PI_OUTPUT); //pigpio pulse setup //X:25, Y1:23, Y2:24 gpioSetMode(25, PI_OUTPUT); gpioSetMode(23, PI_OUTPUT); gpioSetMode(24, PI_OUTPUT); //limit-switch setup gpioSetMode(5, PI_INPUT); gpioWrite(5, 0); gpioSetMode(6, PI_INPUT); gpioWrite(6, 0); gpioSetMode(7, PI_INPUT); gpioWrite(7, 0); gpioSetMode(8, PI_INPUT); gpioWrite(8, 0); gpioSetMode(13, PI_INPUT); gpioSetMode(19, PI_INPUT); gpioSetMode(26, PI_INPUT); gpioSetMode(21, PI_INPUT); CvCapture* capture_robot_side = cvCaptureFromCAM(0); CvCapture* capture_human_side = cvCaptureFromCAM(1); if(capture_robot_side == NULL){ std::cout << "Robot Side Camera Capture FAILED" << std::endl; return -1; } if(capture_human_side ==NULL){ std::cout << "Human Side Camera Capture FAILED" << std::endl; return -1; } // size設定 cvSetCaptureProperty(capture_robot_side,CV_CAP_PROP_FRAME_WIDTH,CAM_PIX_WIDTH); cvSetCaptureProperty(capture_robot_side,CV_CAP_PROP_FRAME_HEIGHT,CAM_PIX_HEIGHT); cvSetCaptureProperty(capture_human_side,CV_CAP_PROP_FRAME_WIDTH,CAM_PIX_WIDTH); cvSetCaptureProperty(capture_human_side,CV_CAP_PROP_FRAME_HEIGHT,CAM_PIX_HEIGHT); //fps設定 cvSetCaptureProperty(capture_robot_side,CV_CAP_PROP_FPS,fps); cvSetCaptureProperty(capture_human_side,CV_CAP_PROP_FPS,fps); // 画像の表示用ウィンドウ生成 //cvNamedWindow("Previous Image", CV_WINDOW_AUTOSIZE); // cvNamedWindow("Now Image", CV_WINDOW_AUTOSIZE); // cvNamedWindow("pack", CV_WINDOW_AUTOSIZE); // cvNamedWindow("mallet", CV_WINDOW_AUTOSIZE); // cvNamedWindow ("Poly", CV_WINDOW_AUTOSIZE); //Create trackbar to change brightness int iSliderValue1 = 50; cvCreateTrackbar("Brightness", "Now Image", &iSliderValue1, 100); //Create trackbar to change contrast int iSliderValue2 = 50; cvCreateTrackbar("Contrast", "Now Image", &iSliderValue2, 100); //pack threthold 0, 50, 120, 220, 100, 220 int iSliderValuePack1 = 54; //80; cvCreateTrackbar("minH", "pack", &iSliderValuePack1, 255); int iSliderValuePack2 = 84;//106; cvCreateTrackbar("maxH", "pack", &iSliderValuePack2, 255); int iSliderValuePack3 = 100;//219; cvCreateTrackbar("minS", "pack", &iSliderValuePack3, 255); int iSliderValuePack4 = 255;//175; cvCreateTrackbar("maxS", "pack", &iSliderValuePack4, 255); int iSliderValuePack5 = 0;//29; cvCreateTrackbar("minV", "pack", &iSliderValuePack5, 255); int iSliderValuePack6 = 255;//203; cvCreateTrackbar("maxV", "pack", &iSliderValuePack6, 255); //mallet threthold 0, 255, 100, 255, 140, 200 int iSliderValuemallet1 = 106; cvCreateTrackbar("minH", "mallet", &iSliderValuemallet1, 255); int iSliderValuemallet2 = 135; cvCreateTrackbar("maxH", "mallet", &iSliderValuemallet2, 255); int iSliderValuemallet3 = 218;//140 cvCreateTrackbar("minS", "mallet", &iSliderValuemallet3, 255); int iSliderValuemallet4 = 255; cvCreateTrackbar("maxS", "mallet", &iSliderValuemallet4, 255); int iSliderValuemallet5 = 0; cvCreateTrackbar("minV", "mallet", &iSliderValuemallet5, 255); int iSliderValuemallet6 = 105; cvCreateTrackbar("maxV", "mallet", &iSliderValuemallet6, 255); // 画像ファイルポインタの宣言 IplImage* img_robot_side = cvQueryFrame(capture_robot_side); IplImage* img_human_side = cvQueryFrame(capture_human_side); IplImage* img_all_round = cvCreateImage(cvSize(CAM_PIX_WIDTH, CAM_PIX_2HEIGHT), IPL_DEPTH_8U, 3); IplImage* tracking_img = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3); IplImage* img_all_round2 = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3); IplImage* show_img = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3); cv::Mat mat_frame1; cv::Mat mat_frame2; cv::Mat dst_img_v; cv::Mat dst_bright_cont; int iBrightness = iSliderValue1 - 50; double dContrast = iSliderValue2 / 50.0; IplImage* dst_img_frame = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3); IplImage* grayscale_img = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 1); IplImage* poly_tmp = cvCreateImage( cvGetSize( img_all_round), IPL_DEPTH_8U, 1); IplImage* poly_dst = cvCreateImage( cvGetSize( img_all_round), IPL_DEPTH_8U, 3); IplImage* poly_gray = cvCreateImage( cvGetSize(img_all_round),IPL_DEPTH_8U,1); int rotate_times = 0; //IplImage* -> Mat mat_frame1 = cv::cvarrToMat(img_robot_side); mat_frame2 = cv::cvarrToMat(img_human_side); //上下左右を反転。本番環境では、mat_frame1を反転させる cv::flip(mat_frame1, mat_frame1, 0); //水平軸で反転(垂直反転) cv::flip(mat_frame1, mat_frame1, 1); //垂直軸で反転(水平反転) vconcat(mat_frame2, mat_frame1, dst_img_v); dst_img_v.convertTo(dst_bright_cont, -1, dContrast, iBrightness); //1枚にした画像をコンバート //画像の膨張と縮小 // cv::Mat close_img; // cv::Mat element(3,3,CV_8U, cv::Scalar::all(255)); // cv::morphologyEx(dst_img_v, close_img, cv::MORPH_CLOSE, element, cv::Point(-1,-1), 3); // cv::imshow("morphologyEx", dst_img_v); // dst_img_v.convertTo(dst_bright_cont, -1, dContrast, iBrightness); //1枚にした画像をコンバート //明るさ調整した結果を変換(Mat->IplImage*)して渡す。その後解放。 *img_all_round = dst_bright_cont; cv_ColorExtraction(img_all_round, dst_img_frame, CV_BGR2HSV, 0, 54, 77, 255, 0, 255); cvCvtColor(dst_img_frame, grayscale_img, CV_BGR2GRAY); cv_Labelling(grayscale_img, tracking_img); cvCvtColor(tracking_img, poly_gray, CV_BGR2GRAY); cvCopy( poly_gray, poly_tmp); cvCvtColor( poly_gray, poly_dst, CV_GRAY2BGR); //画像の膨張と縮小 //cvMorphologyEx(tracking_img, tracking_img,) // 輪郭抽出 found = cvFindContours( poly_tmp, contStorage, &contours, sizeof( CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE); // ポリライン近似 polys = cvApproxPoly( contours, sizeof( CvContour), polyStorage, CV_POLY_APPROX_DP, 8, 10); cvInitTreeNodeIterator( &polyIterator, ( void*)polys, 10); poly = (CvSeq *)cvNextTreeNode( &polyIterator); printf("sort before by X\n"); for( i=0; i<poly->total; i++) { poly_point = *( CvPoint*)cvGetSeqElem( poly, i); // cvCircle( poly_dst, poly_point, 1, CV_RGB(255, 0 , 255), -1); // cvCircle( poly_dst, poly_point, 8, CV_RGB(255, 0 , 255)); std::cout << "x:" << poly_point.x << ", y:" << poly_point.y << std::endl; } printf("Poly FindTotal:%d\n",poly->total); //枠の座標決定 //左上 の 壁サイド側 upper_left_f //左上 の ゴール寄り upper_left_g //右上 の 壁サイド側 upper_right_f //右上 の ゴール寄り upper_right_g //左下 の 壁サイド側 lower_left_f //左下 の ゴール寄り lower_left_g //右下 の 壁サイド側 lower_right_f //右下 の ゴール寄り lower_right_g CvPoint upper_left_f, upper_left_g, upper_right_f, upper_right_g, lower_left_f, lower_left_g, lower_right_f, lower_right_g, robot_goal_left, robot_goal_right; CvPoint frame_points[8]; // if(poly->total == 8){ // for( i=0; i<8; i++){ // poly_point = *( CvPoint*)cvGetSeqElem( poly, i); // frame_points[i] = poly_point; // } // qsort(frame_points, 8, sizeof(CvPoint), compare_cvpoint); // printf("sort after by X\n"); // for( i=0; i<8; i++){ // std::cout << "x:" << frame_points[i].x << ", y:" << frame_points[i].y << std::endl; // } // if(frame_points[0].y < frame_points[1].y){ // upper_left_f = frame_points[0]; // lower_left_f = frame_points[1]; // } // else{ // upper_left_f = frame_points[1]; // lower_left_f = frame_points[0]; // } // if(frame_points[2].y < frame_points[3].y){ // upper_left_g = frame_points[2]; // lower_left_g = frame_points[3]; // } // else{ // upper_left_g = frame_points[3]; // lower_left_g = frame_points[2]; // } // if(frame_points[4].y < frame_points[5].y){ // upper_right_g = frame_points[4]; // lower_right_g = frame_points[5]; // } // else{ // upper_right_g = frame_points[5]; // lower_right_g = frame_points[4]; // } // if(frame_points[6].y < frame_points[7].y){ // upper_right_f = frame_points[6]; // lower_right_f = frame_points[7]; // } // else{ // upper_right_f = frame_points[7]; // lower_right_f = frame_points[6]; // } // } // else{ printf("Frame is not 8 Point\n"); upper_left_f = cvPoint(26, 29); upper_right_f = cvPoint(136, 29); lower_left_f = cvPoint(26, 220); lower_right_f = cvPoint(136, 220); upper_left_g = cvPoint(38, 22); upper_right_g = cvPoint(125, 22); lower_left_g = cvPoint(38, 226); lower_right_g = cvPoint(125, 226); robot_goal_left = cvPoint(60, 226); robot_goal_right = cvPoint(93, 226); // cvCopy(img_all_round, show_img); // cvLine(show_img, upper_left_f, upper_right_f, CV_RGB( 255, 255, 0 )); // cvLine(show_img, lower_left_f, lower_right_f, CV_RGB( 255, 255, 0 )); // cvLine(show_img, upper_right_f, lower_right_f, CV_RGB( 255, 255, 0 )); // cvLine(show_img, upper_left_f, lower_left_f, CV_RGB( 255, 255, 0 )); // // cvLine(show_img, upper_left_g, upper_right_g, CV_RGB( 0, 255, 0 )); // cvLine(show_img, lower_left_g, lower_right_g, CV_RGB( 0, 255, 0 )); // cvLine(show_img, upper_right_g, lower_right_g, CV_RGB( 0, 255, 0 )); // cvLine(show_img, upper_left_g, lower_left_g, CV_RGB( 0, 255, 0 )); //while(1){ //cvShowImage("Now Image", show_img); //cvShowImage ("Poly", poly_dst); //if(cv::waitKey(1) >= 0) { //break; //} //} //return -1; // } printf("upper_left_fX:%d, Y:%d\n",upper_left_f.x, upper_left_f.y); printf("upper_left_gX:%d, Y:%d\n",upper_left_g.x, upper_left_g.y); printf("upper_right_fX:%d,Y:%d\n", upper_right_f.x, upper_right_f.y); printf("upper_right_gX:%d, Y:%d\n" , upper_right_g.x, upper_right_g.y); printf("lower_left_fX:%d, Y:%d\n", lower_left_f.x, lower_left_f.y); printf("lower_left_gX:%d, Y:%d\n", lower_left_g.x, lower_left_g.y); printf("lower_right_fX:%d, Y:%d\n", lower_right_f.x, lower_right_f.y); printf("lower_right_gX:%d, Y:%d\n", lower_right_g.x, lower_right_g.y); printf("robot_goal_left:%d, Y:%d\n", robot_goal_left.x, robot_goal_left.y); printf("robot_goal_right:%d, Y:%d\n", robot_goal_right.x, robot_goal_right.y); cvReleaseImage(&dst_img_frame); cvReleaseImage(&grayscale_img); cvReleaseImage(&poly_tmp); cvReleaseImage(&poly_gray); cvReleaseMemStorage(&contStorage); cvReleaseMemStorage(&polyStorage); //return 1; // Init font cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, 0.4,0.4,0,1); bool is_pushed_decision_button = 1;//もう一方のラズパイ信号にする while(1){ //決定ボタンが押されたらスタート if(gpioRead(8)==0 && is_pushed_decision_button==1){ cvCopy(img_all_round, img_all_round2); cvCopy(img_all_round, show_img); img_robot_side = cvQueryFrame(capture_robot_side); img_human_side = cvQueryFrame(capture_human_side); //IplImage* -> Mat mat_frame1 = cv::cvarrToMat(img_robot_side); mat_frame2 = cv::cvarrToMat(img_human_side); //上下左右を反転。本番環境では、mat_frame1を反転させる cv::flip(mat_frame1, mat_frame1, 0); //水平軸で反転(垂直反転) cv::flip(mat_frame1, mat_frame1, 1); //垂直軸で反転(水平反転) vconcat(mat_frame2, mat_frame1, dst_img_v); iBrightness = iSliderValue1 - 50; dContrast = iSliderValue2 / 50.0; dst_img_v.convertTo(dst_bright_cont, -1, dContrast, iBrightness); //1枚にした画像をコンバート //明るさ調整した結果を変換(Mat->IplImage*)して渡す。その後解放。 *img_all_round = dst_bright_cont; mat_frame1.release(); mat_frame2.release(); dst_img_v.release(); IplImage* dst_img_mallet = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3); IplImage* dst_img_pack = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3); IplImage* dst_img2_mallet = cvCreateImage(cvGetSize(img_all_round2), IPL_DEPTH_8U, 3); IplImage* dst_img2_pack = cvCreateImage(cvGetSize(img_all_round2), IPL_DEPTH_8U, 3); cv_ColorExtraction(img_all_round, dst_img_pack, CV_BGR2HSV, iSliderValuePack1, iSliderValuePack2, iSliderValuePack3, iSliderValuePack4, iSliderValuePack5, iSliderValuePack6); cv_ColorExtraction(img_all_round, dst_img_mallet, CV_BGR2HSV, iSliderValuemallet1, iSliderValuemallet2, iSliderValuemallet3, iSliderValuemallet4, iSliderValuemallet5, iSliderValuemallet6); cv_ColorExtraction(img_all_round2, dst_img2_pack, CV_BGR2HSV, iSliderValuePack1, iSliderValuePack2, iSliderValuePack3, iSliderValuePack4, iSliderValuePack5, iSliderValuePack6); //CvMoments moment_mallet; CvMoments moment_pack; CvMoments moment_mallet; CvMoments moment2_pack; //cvSetImageCOI(dst_img_mallet, 1); cvSetImageCOI(dst_img_pack, 1); cvSetImageCOI(dst_img_mallet, 1); cvSetImageCOI(dst_img2_pack, 1); //cvMoments(dst_img_mallet, &moment_mallet, 0); cvMoments(dst_img_pack, &moment_pack, 0); cvMoments(dst_img_mallet, &moment_mallet, 0); cvMoments(dst_img2_pack, &moment2_pack, 0); //座標計算 double m00_before = cvGetSpatialMoment(&moment2_pack, 0, 0); double m10_before = cvGetSpatialMoment(&moment2_pack, 1, 0); double m01_before = cvGetSpatialMoment(&moment2_pack, 0, 1); double m00_after = cvGetSpatialMoment(&moment_pack, 0, 0); double m10_after = cvGetSpatialMoment(&moment_pack, 1, 0); double m01_after = cvGetSpatialMoment(&moment_pack, 0, 1); double gX_before = m10_before/m00_before; double gY_before = m01_before/m00_before; double gX_after = m10_after/m00_after; double gY_after = m01_after/m00_after; double m00_mallet = cvGetSpatialMoment(&moment_mallet, 0, 0); double m10_mallet = cvGetSpatialMoment(&moment_mallet, 1, 0); double m01_mallet = cvGetSpatialMoment(&moment_mallet, 0, 1); double gX_now_mallet = m10_mallet/m00_mallet; double gY_now_mallet = m01_mallet/m00_mallet; int target_direction = -1; //目標とする向き 時計回り=1、 反時計回り=0 //円の大きさは全体の1/10で描画 // cvCircle(show_img, cvPoint(gX_before, gY_before), CAM_PIX_HEIGHT/10, CV_RGB(0,0,255), 6, 8, 0); // cvCircle(show_img, cvPoint(gX_now_mallet, gY_now_mallet), CAM_PIX_HEIGHT/10, CV_RGB(0,0,255), 6, 8, 0); // cvLine(show_img, cvPoint(gX_before, gY_before), cvPoint(gX_after, gY_after), cvScalar(0,255,0), 2); // cvLine(show_img, robot_goal_left, robot_goal_right, cvScalar(0,255,255), 2); printf("gX_after: %f\n",gX_after); printf("gY_after: %f\n",gY_after); printf("gX_before: %f\n",gX_before); printf("gY_before: %f\n",gY_before); printf("gX_now_mallet: %f\n",gX_now_mallet); printf("gY_now_mallet: %f\n",gY_now_mallet); int target_destanceY = CAM_PIX_2HEIGHT - 30; //Y座標の距離を一定にしている。ディフェンスライン。 //パックの移動は直線のため、一次関数の計算を使って、その後の軌跡を予測する。 double a_inclination; double b_intercept; int closest_frequency; int target_coordinateX; int origin_coordinateY; int target_coordinateY; double center_line = (lower_right_f.x + lower_right_g.x + lower_left_f.x + lower_left_g.x)/4; int left_frame = (upper_left_f.x + lower_left_f.x)/2; int right_frame = (upper_right_f.x + lower_right_f.x)/2; if(robot_goal_right.x < gX_now_mallet){ gpioPWM(25, 128); closest_frequency = gpioSetPWMfrequency(25, 1500); target_direction = 0;//反時計回り } else if(gX_now_mallet < robot_goal_left.x){ gpioPWM(25, 128); closest_frequency = gpioSetPWMfrequency(25, 1500); target_direction = 1;//時計回り } else{ //pwm output for rotate //台の揺れを想定してマージンをとる if(abs(gX_after - gX_before) <= 1 && abs(gY_after - gY_before) <= 1){//パックが動いてない場合一時停止 gpioPWM(25, 0); closest_frequency = gpioSetPWMfrequency(25, 0); a_inclination = 0; b_intercept=0; } else{ a_inclination = (gY_after - gY_before) / (gX_after - gX_before); b_intercept = gY_after - a_inclination * gX_after; //一次関数で目標X座標の計算 if(a_inclination){ target_coordinateX = (int)((target_destanceY - b_intercept) / a_inclination); } else{ target_coordinateX = center_line; } origin_coordinateY = a_inclination * left_frame + b_intercept; int rebound_max = 5; int rebound_num = 0; while(target_coordinateX < left_frame || right_frame < target_coordinateX){ if(target_coordinateX < left_frame){ //左側の枠での跳ね返り後の軌跡。左枠側平均 target_coordinateX = 2 * left_frame - target_coordinateX; b_intercept -= 2 * ((-a_inclination) * left_frame); a_inclination = -a_inclination; origin_coordinateY = a_inclination * left_frame + b_intercept; if(target_coordinateX < right_frame){ } else{ //左側の枠から右側の枠に当たるときのY座標 target_coordinateY = a_inclination * right_frame + b_intercept; } } else if(right_frame < target_coordinateX){ //右側の枠での跳ね返り後の軌跡。右枠側平均 target_coordinateX = 2 * right_frame - target_coordinateX; b_intercept += 2 * (a_inclination * right_frame); a_inclination= -a_inclination; //cvLine(show_img, cvPoint(right_frame, b_intercept), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2); origin_coordinateY = a_inclination * right_frame + b_intercept; if(left_frame < target_coordinateX){ } else{ //右側の枠から左側の枠に当たるときのY座標 target_coordinateY = a_inclination * left_frame + b_intercept; } } rebound_num++; if(rebound_max < rebound_num){ //跳ね返りが多すぎる時は、中央を指定 target_coordinateX = (lower_right_f.x + lower_right_g.x + lower_left_f.x + lower_left_g.x)/4; break; } } if(target_coordinateX < center_line && center_line < gX_now_mallet){ target_direction = 0; gpioPWM(25, 128); closest_frequency = gpioSetPWMfrequency(25, 1000); } else if(center_line < target_coordinateX && gX_now_mallet < center_line){ target_direction = 1; gpioPWM(25, 128); closest_frequency = gpioSetPWMfrequency(25, 1000); } else{ gpioPWM(25, 0); closest_frequency = gpioSetPWMfrequency(25, 0); } } printf("a_inclination: %f\n",a_inclination); printf("b_intercept: %f\n",b_intercept); } if(target_direction != -1){ gpioWrite(18, target_direction); } //pwm output for rotate //台の揺れを想定してマージンをとる /*if(abs(gX_after - gX_before) <= 1){//パックが動いてない場合一時停止 gpioPWM(25, 0); closest_frequency = gpioSetPWMfrequency(25, 0); a_inclination = 0; b_intercept=0; } else if(gY_after-1 < gY_before ){ //packが離れていく時、台の中央に戻る a_inclination = 0; b_intercept=0; //目標値は中央。台のロボット側(4点)からを計算 double center_line = (lower_right_f.x + lower_right_g.x + lower_left_f.x + lower_left_g.x)/4; if(center_line + 3 < gX_now_mallet){ //+1 マージン gpioPWM(25, 128); closest_frequency = gpioSetPWMfrequency(25, 1500); target_direction = 0;//反時計回り } else if(gX_now_mallet < center_line-3){ //-1 マージン gpioPWM(25, 128); closest_frequency = gpioSetPWMfrequency(25, 1500); target_direction = 1;//時計回り } else{ gpioPWM(25, 0); closest_frequency = gpioSetPWMfrequency(25, 0); } } else{ gpioPWM(25, 128); closest_frequency = gpioSetPWMfrequency(25, 1500); a_inclination = (gY_after - gY_before) / (gX_after - gX_before); b_intercept = gY_after - a_inclination * gX_after; //一次関数で目標X座標の計算 if(a_inclination){ target_coordinateX = (int)((target_destanceY - b_intercept) / a_inclination); } else{ target_coordinateX = 0; } } printf("a_inclination: %f\n",a_inclination); printf("b_intercept: %f\n",b_intercept); int left_frame = (upper_left_f.x + lower_left_f.x)/2; int right_frame = (upper_right_f.x + lower_right_f.x)/2; origin_coordinateY = a_inclination * left_frame + b_intercept; if(target_coordinateX < left_frame){ cvLine(show_img, cvPoint((int)gX_after, (int)gY_after), cvPoint(left_frame, origin_coordinateY), cvScalar(0,255,255), 2); } else if(right_frame < target_coordinateX){ cvLine(show_img, cvPoint((int)gX_after, (int)gY_after), cvPoint(right_frame, origin_coordinateY), cvScalar(0,255,255), 2); } else{ cvLine(show_img, cvPoint((int)gX_after, (int)gY_after), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2); } int rebound_max = 5; int rebound_num = 0; while(target_coordinateX < left_frame || right_frame < target_coordinateX){ if(target_coordinateX < left_frame){ //左側の枠での跳ね返り後の軌跡。左枠側平均 target_coordinateX = 2 * left_frame - target_coordinateX; b_intercept -= 2 * ((-a_inclination) * left_frame); a_inclination = -a_inclination; origin_coordinateY = a_inclination * left_frame + b_intercept; if(target_coordinateX < right_frame){ cvLine(show_img, cvPoint(left_frame, origin_coordinateY), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2); } else{ //左側の枠から右側の枠に当たるときのY座標 target_coordinateY = a_inclination * right_frame + b_intercept; cvLine(show_img, cvPoint(left_frame, origin_coordinateY), cvPoint(right_frame, target_coordinateY), cvScalar(0,255,255), 2); } } else if(right_frame < target_coordinateX){ //右側の枠での跳ね返り後の軌跡。右枠側平均 target_coordinateX = 2 * right_frame - target_coordinateX; b_intercept += 2 * (a_inclination * right_frame); a_inclination= -a_inclination; //cvLine(show_img, cvPoint(right_frame, b_intercept), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2); origin_coordinateY = a_inclination * right_frame + b_intercept; if(left_frame < target_coordinateX){ cvLine(show_img, cvPoint(right_frame, origin_coordinateY), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2); } else{ //右側の枠から左側の枠に当たるときのY座標 target_coordinateY = a_inclination * left_frame + b_intercept; cvLine(show_img, cvPoint(right_frame, origin_coordinateY), cvPoint(left_frame, target_coordinateY), cvScalar(0,255,255), 2); } } rebound_num++; if(rebound_max < rebound_num){ //跳ね返りが多すぎる時は、中央を指定 target_coordinateX = (lower_right_f.x + lower_right_g.x + lower_left_f.x + lower_left_g.x)/4; break; } } printf("target_coordinateX: %d\n",target_coordinateX); //防御ラインの描画 cvLine(show_img, cvPoint(CAM_PIX_WIDTH, target_destanceY), cvPoint(0, target_destanceY), cvScalar(255,255,0), 2); //マレットの動きの描画 cvLine(show_img, cvPoint((int)gX_now_mallet, (int)gY_now_mallet), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2); //cvPutText (show_img, to_c_char((int)gX_now_mallet), cvPoint(460,30), &font, cvScalar(220,50,50)); //cvPutText (show_img, to_c_char((int)target_coordinateX), cvPoint(560,30), &font, cvScalar(50,220,220)); int amount_movement = target_coordinateX - gX_now_mallet; //reacted limit-switch and target_direction rotate // if(gpioRead(6) == 1){//X軸右 // gpioPWM(25, 128); // closest_frequency = gpioSetPWMfrequency(25, 1500); // target_direction = 0;//反時計回り // printf("X軸右リミット!反時計回り\n"); // } // else if(gpioRead(26) == 1){//X軸左 gpioPWM(25, 128); closest_frequency = gpioSetPWMfrequency(25, 1500); target_direction = 1;//時計回り printf("X軸左リミット!時計回り\n"); } else if(gpioRead(5) == 1){//Y軸右上 gpioPWM(23, 128); gpioSetPWMfrequency(23, 1500); gpioWrite(14, 0); printf("Y軸右上リミット!時計回り\n"); } else if(gpioRead(13) == 1){//Y軸右下 gpioPWM(23, 128); gpioSetPWMfrequency(23, 1500); gpioWrite(14, 1); printf("Y軸右下リミット!反時計回り\n"); } else if(gpioRead(19) == 1){//Y軸左下 gpioPWM(24, 128); gpioSetPWMfrequency(24, 1500); gpioWrite(15, 0); printf("Y軸左下リミット!時計回り\n"); } else if(gpioRead(21) == 1){//Y軸左上 gpioPWM(24, 0); gpioSetPWMfrequency(24, 1500); gpioWrite(15, 1); printf("Y軸左上リミット!反時計回り\n"); } else{ //Y軸固定のため gpioSetPWMfrequency(23, 0); gpioSetPWMfrequency(24, 0); if(amount_movement > 0){ target_direction = 1;//時計回り } else if(amount_movement < 0){ target_direction = 0;//反時計回り } } if(target_direction != -1){ gpioWrite(18, target_direction); } else{ gpioPWM(24, 0); gpioSetPWMfrequency(24, 0); } printf("setting_frequency: %d\n", closest_frequency);*/ // 指定したウィンドウ内に画像を表示する //cvShowImage("Previous Image", img_all_round2); // cvShowImage("Now Image", show_img); // cvShowImage("pack", dst_img_pack); // cvShowImage("mallet", dst_img_mallet); // cvShowImage ("Poly", poly_dst); cvReleaseImage (&dst_img_mallet); cvReleaseImage (&dst_img_pack); cvReleaseImage (&dst_img2_mallet); cvReleaseImage (&dst_img2_pack); if(cv::waitKey(1) >= 0) { break; } } else{ //リセット信号が来た場合 is_pushed_decision_button = 0; } } gpioTerminate(); cvDestroyAllWindows(); //Clean up used CvCapture* cvReleaseCapture(&capture_robot_side); cvReleaseCapture(&capture_human_side); //Clean up used images cvReleaseImage(&poly_dst); cvReleaseImage(&tracking_img); cvReleaseImage(&img_all_round); cvReleaseImage(&img_human_side); cvReleaseImage(&img_all_round2); cvReleaseImage(&show_img); cvReleaseImage(&img_robot_side); return 0; }
int main(int argc, char* argv[]) { int counter; wchar_t auxstr[20]; printf("start!\n"); printf("PUCK MINH: %d\n",minH); printf("PUCK MAXH: %d\n",maxH); printf("PUCK MINS: %d\n",minS); printf("PUCK MAXS: %d\n",maxS); printf("PUCK MINV: %d\n",minV); printf("PUCK MAXV: %d\n",maxV); printf("ROBOT MINH: %d\n",RminH); printf("ROBOT MAXH: %d\n",RmaxH); printf("ROBOT MINS: %d\n",RminS); printf("ROBOT MAXS: %d\n",RmaxS); printf("ROBOT MINV: %d\n",RminV); printf("ROBOT MAXV: %d\n",RmaxV); printf("FPS: %d\n",fps); // 読み込み画像ファイル名 char imgfile[] = "camera/photodir/capmallet1.png"; char imgfile2[] = "camera/photodir/capmallet2.png"; // 画像の読み込み img = cvLoadImage(imgfile, CV_LOAD_IMAGE_ANYCOLOR | CV_LOAD_IMAGE_ANYDEPTH); img2 = cvLoadImage(imgfile2, CV_LOAD_IMAGE_ANYCOLOR | CV_LOAD_IMAGE_ANYDEPTH); // 画像の表示用ウィンドウ生成 cvNamedWindow("circle_sample", CV_WINDOW_AUTOSIZE); cvNamedWindow("circle_sample2", CV_WINDOW_AUTOSIZE); //cvNamedWindow("cv_ColorExtraction"); // Init font cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, 0.4,0.4,0,1); IplImage* dst_img_mallett = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); IplImage* dst_img_pack = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); IplImage* dst_img2_mallett = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); IplImage* dst_img2_pack = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); //白抽出0,255,0,15,240,255 //黒抽出0, 255, 0, 50, 0, 100 //青検出0, 255, 50, 200, 100, 180 //cv_ColorExtraction(img, dst_img_mallett, CV_BGR2HSV, 0, 255, 50, 200, 100, 180); cv_ColorExtraction(img, dst_img_pack, CV_BGR2HSV, 0, 255, 0, 50, 0, 100); cv_ColorExtraction(img2, dst_img2_mallett, CV_BGR2HSV, 0, 255, 50, 200, 100, 180); cv_ColorExtraction(img2, dst_img2_pack, CV_BGR2HSV, 0, 255, 0, 50, 0, 100); //CvMoments moment_mallett; CvMoments moment_pack; CvMoments moment2_mallett; CvMoments moment2_pack; //cvSetImageCOI(dst_img_mallett, 1); cvSetImageCOI(dst_img_pack, 1); cvSetImageCOI(dst_img2_mallett, 1); cvSetImageCOI(dst_img2_pack, 1); //cvMoments(dst_img_mallett, &moment_mallett, 0); cvMoments(dst_img_pack, &moment_pack, 0); cvMoments(dst_img2_mallett, &moment2_mallett, 0); cvMoments(dst_img2_pack, &moment2_pack, 0); //座標計算 double m00_before = cvGetSpatialMoment(&moment_pack, 0, 0); double m10_before = cvGetSpatialMoment(&moment_pack, 1, 0); double m01_before = cvGetSpatialMoment(&moment_pack, 0, 1); double m00_after = cvGetSpatialMoment(&moment2_pack, 0, 0); double m10_after = cvGetSpatialMoment(&moment2_pack, 1, 0); double m01_after = cvGetSpatialMoment(&moment2_pack, 0, 1); double gX_before = m10_before/m00_before; double gY_before = m01_before/m00_before; double gX_after = m10_after/m00_after; double gY_after = m01_after/m00_after; double m00_mallett = cvGetSpatialMoment(&moment2_mallett, 0, 0); double m10_mallett = cvGetSpatialMoment(&moment2_mallett, 1, 0); double m01_mallett = cvGetSpatialMoment(&moment2_mallett, 0, 1); double gX_now_mallett = m10_mallett/m00_mallett; double gY_now_mallett = m01_mallett/m00_mallett; cvCircle(img2, cvPoint((int)gX_before, (int)gY_before), 50, CV_RGB(0,0,255), 6, 8, 0); cvLine(img2, cvPoint((int)gX_before, (int)gY_before), cvPoint((int)gX_after, (int)gY_after), cvScalar(0,255,0), 2); int target_destanceY = 480 - 30;//Y座標の距離を一定にしている。ディフェンスライン。 //パックの移動は直線のため、一次関数の計算を使って、その後の軌跡を予測する。 double a_inclination = (gY_after - gY_before) / (gX_after - gX_before); double b_intercept = gY_after - a_inclination * gX_after; printf("gX_after: %f\n",gX_after); printf("gY_after: %f\n",gY_after); printf("gX_before: %f\n",gX_before); printf("gY_before: %f\n",gY_before); printf("a_inclination: %f\n",a_inclination); printf("b_intercept: %f\n",b_intercept); int target_coordinateX = (int)((target_destanceY - b_intercept) / a_inclination); printf("target_coordinateX: %d\n",target_coordinateX); cvLine(img2, cvPoint((int)gX_after, (int)gY_after), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2); cvLine(img2, cvPoint(640, target_destanceY), cvPoint(0, target_destanceY), cvScalar(255,255,0), 2); cvLine(img2, cvPoint((int)gX_now_mallett, (int)gY_now_mallett), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2); cvPutText (img2, to_c_char((int)gX_now_mallett), cvPoint(460,30), &font, cvScalar(220,50,50)); cvPutText (img2, to_c_char((int)target_coordinateX), cvPoint(560,30), &font, cvScalar(50,220,220)); // 指定したウィンドウ内に画像を表示する cvShowImage("circle_sample", img); cvShowImage("circle_sample2", img2); //pwm output test if(gpioInitialise() < 0) return 1; gpioSetMode(18, PI_OUTPUT); gpioPWM(18, 128); gpioSetPWMfrequency(18, 1000); /*if(wiringPiSetupGpio()==-1){ printf("cannotsetup gpio.\n" ); return 1; } pinMode(18,PWM_OUTPUT); pwmSetMode(PWM_MODE_MS); pwmSetClock(64); pwmSetRange(100); pwmWrite(18,50);*/ while(1) { if(cv::waitKey(30) >= 0) { break; } } //Clean up used images cvReleaseImage(&img); // cvReleaseImage (&dst_img); cvDestroyAllWindows() ; return 0; }
/* * Class: com_diozero_pigpioj_PigpioGpio * Method: setPWMFrequency * Signature: (II)I */ JNIEXPORT jint JNICALL Java_com_diozero_pigpioj_PigpioGpio_setPWMFrequency (JNIEnv* env, jclass clz, jint gpio, jint frequency) { return gpioSetPWMfrequency(gpio, frequency); }
void zero_crossing(int gpio, int level, unsigned int tick) { //static unsigned int adc_chan = 0; //unsigned int temp; switch (level) { case 1: ac_present = 1; current_zero_tick = tick; if (current_zero_tick > (prev_zero_tick + 9900)) { gpioWrite(PUMP_1_GPIO, 1); // Set PUMP_1_GPIO high. gpioWrite(PUMP_2_GPIO, 1); // Set PUMP_2_GPIO high. prev_zero_tick = current_zero_tick; zero_tick = current_zero_tick; zero_crossing_count ++; //printf("gpio %d became %d at %ud\n", gpio, level, tick); //printf("%u\n",tick); #if 0 //pump 2 if (pump2_pwm > 90) gpioWrite(PUMP_2_GPIO, 0); else if (pump2_pwm < 10) gpioWrite(PUMP_2_GPIO, 1); else { gpioPWM(PUMP_2_GPIO, 0); gpioSetPWMfrequency(PUMP_2_GPIO, 250); //Dummy frequency gpioSetPWMfrequency(PUMP_2_GPIO, 100); gpioPWM(PUMP_2_GPIO, 100 - pump2_pwm); //gpioSetTimerFunc(0 + (zero_crossing_count % 2), 2000 - (pump2_pwm/10), start_pwm_pump2_timer); } #endif } break; case PI_TIMEOUT: ac_present = 0; printf("ac_present = 0\n"); if (pump1_pwm > 0) { gpioWrite(PUMP_1_GPIO, 0); } else { gpioWrite(PUMP_1_GPIO, 1); } if (pump2_pwm > 0) { gpioWrite(PUMP_2_GPIO, 0); } else { gpioWrite(PUMP_2_GPIO, 1); } break; default: break; } }