//タイマー制御用関数。時間が経ったらリセット
void pwmReset(void){
gpioWrite(18, 0);
//gpioSetPWMfrequency(18, 0);
gpioSetTimerFunc(0, 6000, NULL);
}
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[])
{
char str[256];
if (argc > 1) micros = atoi(argv[1]);
if (argc > 2) millis = atoi(argv[2]);
putTTY("\033c"); /* clear console */
gpioCfgBufferSize(millis);
gpioCfgClock(micros, PI_CLOCK_PCM, PI_CLOCK_PLLD);
/* before using the library you must call gpioInitialise */
version = gpioInitialise();
if (version >= 0)
{
/* initialise pins, only gpio numbers are supported */
gpioSetMode(SERVO1, PI_OUTPUT);
gpioSetMode(SERVO2, PI_OUTPUT);
gpioSetMode(SERVO3, PI_OUTPUT);
gpioSetMode(LASER, PI_OUTPUT);
gpioSetMode(LED1, PI_OUTPUT);
gpioSetMode(LED2, PI_OUTPUT);
gpioSetMode(LED3, PI_OUTPUT);
gpioSetMode(MOTOR_A_IN1, PI_OUTPUT);
gpioSetMode(MOTOR_A_IN2, PI_OUTPUT);
gpioSetMode(MOTOR_B_IN1, PI_OUTPUT);
gpioSetMode(MOTOR_B_IN2, PI_OUTPUT);
gpioSetMode(SONAR_TRIGGER, PI_OUTPUT);
gpioWrite (SONAR_TRIGGER, PI_OFF);
gpioSetMode(SONAR_ECHO, PI_INPUT);
gpioSetMode(LAUNCHPAD, PI_INPUT);
gpioSetMode(LDR, PI_INPUT);
/* update i2c fifty times a second, timer #0 */
gpioSetTimerFunc(0, 20, i2cTick);
//gpioSetTimerFunc(0, 1000, servoTick);
/* update LEDs and laser once a second, timer #1 */
gpioSetTimerFunc(1, 1000, LEDlaserTick);
/* update motors every three seconds, timer #2 */
gpioSetTimerFunc(2, 3000, motorTick);
/* update sonar/LDR 10 times a second, timer #3 */
gpioSetTimerFunc(3, 100, sonarLDRtick);
/* an attachecd TI launchpad is transmitting high pulses of
15, 35, 55, 75, ..., 975, 995 microseconds repeating with 50
microseconds off between each pulse */
gpioSetAlertFunc(LAUNCHPAD, launchpadAlert);
/* monitor sonar echos */
gpioSetAlertFunc(SONAR_ECHO, sonarAlert);
/* monitor LDR level changes */
gpioSetAlertFunc(LDR, LDRalert);
while (1)
{
sleep(1);
sprintf(str, "TI pulses %8ld", launchpadPulses);
putTTYstr(9, 1, str);
sprintf(str, "+/-5 %8ld", launchpad5);
putTTYstr(10, 6, str);
sprintf(str, "+/-10 %8ld", launchpad10);
putTTYstr(11, 5, str);
sprintf(str, "+/-15 %8ld", launchpad15);
putTTYstr(12, 5, str);
sprintf(str, "Others %8ld (last %d) ",
launchpadOutRange, launchpadErr);
putTTYstr(13, 4, str);
}
}
gpioTerminate();
return 0;
}
