void motorTick(void)
{
static int gpio_in1[2]={MOTOR_A_IN1, MOTOR_B_IN1};
static int gpio_in2[2]={MOTOR_A_IN2, MOTOR_B_IN2};
static int speed [2]={ 80, 80};
static int inc [2]={ -50, 50};
int i;
char str[256];
for (i=0; i<2; i++)
{
speed[i]+=inc[i];
if (speed[i]<0)
{
gpioPWM(gpio_in1[i], -speed[i]);
gpioPWM(gpio_in2[i], 0);
if (speed[i] < -205) inc[i] = -inc[i];
sprintf(str, "MOT%d IN1=%3d IN2=%3d", i+1, -speed[i], 0);
}
else
{
gpioPWM(gpio_in2[i], speed[i]);
gpioPWM(gpio_in1[i], 0);
if (speed[i] > 205) inc[i] = -inc[i];
sprintf(str, "MOT%d IN1=%3d IN2=%3d", i+1, 0, speed[i]);
}
if (i) putTTYstr(7, 1, str); else putTTYstr(5, 1, str);
}
}
//################################################BUTTON##########################
static UA_StatusCode
readIntegerB(void *handle, const UA_NodeId nodeid, UA_Boolean sourceTimeStamp,
const UA_NumericRange *range, UA_DataValue *dataValue) {
// Let LED BLink
gpioPWM(17,255);
usleep( 1000 );
gpioPWM(17,0);
// Read Digital value of GPIO 22 and set as Node Value
handle = &ButtonValue;
dataValue->hasValue = true;
UA_Variant_setScalarCopy(&dataValue->value, (UA_UInt32*)handle, &UA_TYPES[UA_TYPES_INT32]);
// we know the nodeid is a string
UA_LOG_INFO(logger, UA_LOGCATEGORY_USERLAND, "Node read %.*s",
nodeid.identifier.string.length, nodeid.identifier.string.data);
UA_LOG_INFO(logger, UA_LOGCATEGORY_USERLAND, "read value %i", *(UA_UInt32*)handle);
return UA_STATUSCODE_GOOD;
}
void ESCController::YPRT(int yprt[4])
{
int i;
int ESCOutput[4];
ESCOutput[0] = outMin+yprt[3] + yprt[0] - yprt[1] + yprt[2];
ESCOutput[1] = outMin+yprt[3] - yprt[0] + yprt[1] + yprt[2];
ESCOutput[2] = outMin+yprt[3] + yprt[0] + yprt[1] - yprt[2];
ESCOutput[3] = outMin+yprt[3] - yprt[0] - yprt[1] - yprt[2];
// Limit to range
int min,max;
int diff;
max = *std::max_element(ESCOutput, ESCOutput+4);
if (max > outMax) {
diff = outMax - max;
for (i=0;i<4;i++) {
ESCOutput[i] += diff;
}
for (i=0;i<4;i++) {
if (ESCOutput[i] < outMin) {
ESCOutput[i] = outMin;
}
}
}
min = *std::min_element(ESCOutput, ESCOutput+4);
if (min < outMin) {
diff = outMin - min;
for (i=0;i<4;i++) {
ESCOutput[i] += diff;
}
for (i=0;i<4;i++) {
if (ESCOutput[i] > outMax) {
ESCOutput[i] = outMax;
}
}
}
// Sanity checker
for (i=0;i<4;i++) {
if (ESCOutput[i] - oldESC[i] > sanity[i]) {
ESCOutput[i] = oldESC[i] + sanity[i];
}
if (ESCOutput[i] - oldESC[i] < -sanity[i]) {
ESCOutput[i] = oldESC[i] - sanity[i];
}
oldESC[i] = ESCOutput[i];
}
// Output
for (i=0;i<4;i++) {
gpioPWM(ESC[i], ESCOutput[i]);
}
}
int main() {
int ythrust;
gpioInitialise();
mygpioSetup();
printf("gpio setup");
while (1) {
ythrust = 2000;
gpioPWM(18, ythrust);
printf("gpio 18 at 2000");
sleep(5);
ythrust = 1500;
gpioPWM(18, ythrust);
printf("gpio 18 at 1500");
sleep(5);
ythrust = 1000;
gpioPWM(18, ythrust);
printf("gpio 18 at 1000");
sleep(5);
}
gpioTerminate();
return 0;
}
int set_PWM(int chan, int value)
{
int d_chan;
if (chan == 0) {d_chan = 4;}
if (chan == 1) {d_chan = 17;}
if (chan == 2) {d_chan = 27;}
if (chan == 3) {d_chan = 22;}
if (chan == 4) {d_chan = 6;}
if (chan == 5) {d_chan = 13;}
if (chan == 6) {d_chan = 19;}
if (chan == 7) {d_chan = 26;}
gpioPWM(d_chan, value);
return 1;
}
// sets motor speed and direction
int set_motor(int motor,int speed)
{
if ((motor>2)||(motor<0))
{
printf("Wrong motor number\n");
return -1;
}
if ((speed>255)||(speed<-255))
{
printf("Speed should be in range -255..255\n");
return -3;
}
// set PWM for motor1
if (motor == 1)
{
if (speed >= 0)
{
gpioPWM(12,speed);
gpioPWM(16,0);
}
if ( speed <0 )
{
gpioPWM(16,0-speed);
gpioPWM(12,0);
}
}
// set PWM for motor 2
if (motor == 2)
{
if (speed>=0)
{
gpioPWM(20,speed);
gpioPWM(21,0);
}
if ( speed < 0)
{
gpioPWM(21,0-speed);
gpioPWM(20,0);
}
}
return 0;
}
void sLED (int red,int blue,int green,int mode)
{
if(mode == 1) {
gpioPWM(RED,red);
gpioPWM(BLUE,blue);
gpioPWM(GREEN,green);
}
else if(mode == 0) {
MODE = 0;
gpioPWM(RED,red);
gpioPWM(BLUE,blue);
gpioPWM(GREEN,green);
}
}
void LEDlaserTick(void)
{
static int gpio[LEDS]={LED1, LED2, LED3, LASER};
static int pos [LEDS]={ 0, 3, 6, 9};
static int inc [LEDS]={ 1, 1, 1, 1};
static int vals[] = {0, 1, 2, 4, 8, 16, 32, 64, 128, 249};
int i;
for (i=0; i<LEDS; i++)
{
gpioPWM(gpio[i], vals[pos[i]]);
pos[i] += inc[i];
if ( (pos[i]>=(sizeof(vals)/4)) || (pos[i]<0) )
{
inc[i] = -inc[i];
pos[i] += inc[i];
}
}
}
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;
}
void *pressureSensor_task(void *arg)
{
printf("pressure: Task Started %s\n", (char *)arg);
ads1015_t ads;
ads1015_init(&ads, GENERIC);
float voltage = 0;
int scaledVoltage = 0;
//float pressure = 0;
int prev_value = 0;
int index = 0;
float avg = 0;
int delta = 0;
int window[5];
for(int i = 0; i < 5; i++)
{
// Initialize to middle value
window[i] = 128;
}
while(1)
{
// read the pulse sensor signal
sem_wait(&mutex_adc);
voltage = ads1015_getDataFromChannel(&ads, PRESSURE_SENSOR_ADC_CHANNEL);
usleep(400);
voltage = ads1015_getDataFromChannel(&ads, PRESSURE_SENSOR_ADC_CHANNEL);
sem_post(&mutex_adc);
//printf("d= %f\n", voltage);
if (voltage > 0)
{
// scale the voltage to PWM values
scaledVoltage = map((int)(voltage * 1000), 1500, 4096, 0, 255);
if (scaledVoltage > 255) scaledVoltage = 255;
// guard against negative values for PWM
if (scaledVoltage < 0)
{
scaledVoltage = prev_value;
}
else
{
prev_value = scaledVoltage;
}
// Set the LED brightness value
sem_wait(&mutex_gpio);
gpioPWM(PRESSURE_SENSOR_GPIO_PIN, scaledVoltage);
sem_post(&mutex_gpio);
// determine delta
avg = 0;
for(int i = 0; i < 5; i++)
{
avg = avg + window[i];
}
avg = avg / 5;
delta = (scaledVoltage - avg);
//fprintf(stderr,"d= %d\n", delta );
//fprintf(stderr,"d= %d\n", scaledVoltage );
// report to main thread
DeltaPressure = delta;
Pressure = scaledVoltage;
// Put current value into the averaging window
window[index++] = scaledVoltage;
if (index >= 5) index = 0;
}
// sleep for 230 ms
usleep(230000);
}
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); //１枚にした画像をコンバート
//画像の膨張と縮小
// 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); //１枚にした画像をコンバート
//明るさ調整した結果を変換(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); //１枚にした画像をコンバート
//明るさ調整した結果を変換(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: setPWMDutyCycle
* Signature: (II)I
*/
JNIEXPORT jint JNICALL Java_com_diozero_pigpioj_PigpioGpio_setPWMDutyCycle
(JNIEnv* env, jclass clz, jint gpio, jint dutyCycle) {
return gpioPWM(gpio, dutyCycle);
}
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;
}
}
void ESCController::allSetTo(int width)
{
for (int i=0;i<4;i++) {
gpioPWM(ESC[i], width);
}
}
