From 243a2d19d8416dc0c66c47d2fea426403c92bf76 Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 13 Feb 2017 19:03:57 -0200 Subject: [PATCH 01/20] Adicionado controle de PID e bug do unitag arrumado --- cc/camcap.hpp | 28 +-- cc/controlGUI.hpp | 196 ++++++++++++++++++ .../capture-gui/V4LInterface-aux.cpp | 2 +- .../capture-gui/V4LInterface-events.cpp | 22 +- 4 files changed, 224 insertions(+), 24 deletions(-) diff --git a/cc/camcap.hpp b/cc/camcap.hpp index 8c6042a3..1cf048e3 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -438,8 +438,8 @@ class CamCap: parallel_tracking(image); if(!v.HSV_calib_event_flag) { - //robot_creation_unitag(); - robot_creation(); + robot_creation_unitag(); + //robot_creation(); if (!v.draw_info_flag) { @@ -683,7 +683,7 @@ class CamCap: robot.push_back(r); distanceRef1 = 999999999.0; distanceRef2 = 999999999.0; - for(int i = 0; i < 3; i++) { + for(int i = 0; i < 1; i++) { for(int k = 0; k < Team_Sec[i].size(); k++) { distance = calcDistance(Team_Main[j],Team_Sec[i][k]); @@ -708,17 +708,21 @@ class CamCap: robot[l].position = Team_Main[j]; //cout<<"3.1"<Team_Sec_area[index2[0]][index2[1]]) { - robot[l].secundary = Team_Sec[index1[0]][index1[1]]; - robot[l].ternary = Team_Sec[index2[0]][index2[1]]; - - //cout<<"Area 1 = Secundary"<<" Area 2 = Ternary"<Team_Sec_area[index2[0]][index2[1]]) { + robot[l].secundary = Team_Sec[index1[0]][index1[1]]; + robot[l].ternary = Team_Sec[index2[0]][index2[1]]; + + //cout<<"Area 1 = Secundary"<<" Area 2 = Ternary"<pack_start(bt_HSV_left, false, true, 10); bt_HSV_left.set_label(" < "); bt_HSV_left.set_alignment(.5, .5); - HSV_label.set_text("Principal"); + HSV_label.set_text("Main"); //HSV_label.set_alignment(0, .5); hbox3->pack_start(HSV_label, false, true, 10); hbox3->pack_start(bt_HSV_right, false, true, 10); diff --git a/pack-capture-gui/capture-gui/V4LInterface-events.cpp b/pack-capture-gui/capture-gui/V4LInterface-events.cpp index 1e677e77..08a12820 100644 --- a/pack-capture-gui/capture-gui/V4LInterface-events.cpp +++ b/pack-capture-gui/capture-gui/V4LInterface-events.cpp @@ -676,7 +676,7 @@ void V4LInterface::__event_bt_right_HSV_calib_clicked() { HScale_Amin.set_value(Amin[Img_id]); switch(Img_id) { case 0: - HSV_label.set_text("Primaria"); + HSV_label.set_text("Main"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -687,7 +687,7 @@ void V4LInterface::__event_bt_right_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 1: - HSV_label.set_text("Secundaria 1"); + HSV_label.set_text("Secondary 1"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -698,7 +698,7 @@ void V4LInterface::__event_bt_right_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 2: - HSV_label.set_text("Secundaria 2"); + HSV_label.set_text("Secondary 2"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -709,7 +709,7 @@ void V4LInterface::__event_bt_right_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 3: - HSV_label.set_text("Secundaria 3"); + HSV_label.set_text("Secondary 3"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -720,7 +720,7 @@ void V4LInterface::__event_bt_right_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 4: - HSV_label.set_text("Bola"); + HSV_label.set_text("Ball"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -731,7 +731,7 @@ void V4LInterface::__event_bt_right_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 5: - HSV_label.set_text("Adversario"); + HSV_label.set_text("Opponent"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -762,7 +762,7 @@ void V4LInterface::__event_bt_left_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 1: - HSV_label.set_text("Secundaria 1"); + HSV_label.set_text("Secondary 1"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -773,7 +773,7 @@ void V4LInterface::__event_bt_left_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 2: - HSV_label.set_text("Secundaria 2"); + HSV_label.set_text("Secondary 2"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -784,7 +784,7 @@ void V4LInterface::__event_bt_left_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 3: - HSV_label.set_text("Secundaria 3"); + HSV_label.set_text("Secondary 3"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -795,7 +795,7 @@ void V4LInterface::__event_bt_left_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 4: - HSV_label.set_text("Bola"); + HSV_label.set_text("Ball"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); @@ -806,7 +806,7 @@ void V4LInterface::__event_bt_left_HSV_calib_clicked() { HScale_Vmax.set_value(V[Img_id][1]); break; case 5: - HSV_label.set_text("Adversario"); + HSV_label.set_text("Opponent"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]); From a51bc0df1c5d5833aa0058497d7619344d639829 Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 13 Feb 2017 19:56:28 -0200 Subject: [PATCH 02/20] to_string() removido. Codigo repetido eliminado. --- .../capture-gui/V4LInterface-aux.cpp | 36 ++++-- .../capture-gui/V4LInterface-events.cpp | 110 ++---------------- pack-capture-gui/capture-gui/V4LInterface.hpp | 3 - 3 files changed, 37 insertions(+), 112 deletions(-) diff --git a/pack-capture-gui/capture-gui/V4LInterface-aux.cpp b/pack-capture-gui/capture-gui/V4LInterface-aux.cpp index ea89c873..b5ba3ca9 100644 --- a/pack-capture-gui/capture-gui/V4LInterface-aux.cpp +++ b/pack-capture-gui/capture-gui/V4LInterface-aux.cpp @@ -944,7 +944,10 @@ namespace capture robots_speed_vbox[1].pack_start(robots_speed_progressBar[0], false, true, 0); robots_speed_progressBar[0].set_halign(Gtk::ALIGN_CENTER); robots_speed_progressBar[0].set_valign(Gtk::ALIGN_CENTER); - //robots_speed_progressBar[0].set_text(std::to_string(robot_list[0].V).substr(0,3)); + std::ostringstream strs0; + strs0 << robot_list[0].V; + std::string str0 = strs0.str(); + robots_speed_progressBar[0].set_text(str0.substr(0,3)); robots_speed_progressBar[0].set_show_text(true); robots_speed_progressBar[0].set_fraction( (double) robot_list[0].V); robots_speed_vbox[0].pack_start(robots_speed_hbox[1], false, true, 0); @@ -963,7 +966,10 @@ namespace capture robots_speed_vbox[2].pack_start(robots_speed_progressBar[1], false, true, 0); robots_speed_progressBar[1].set_halign(Gtk::ALIGN_CENTER); robots_speed_progressBar[1].set_valign(Gtk::ALIGN_CENTER); - //robots_speed_progressBar[1].set_text(std::to_string(robot_list[1].V).substr(0,3)); + std::ostringstream strs1; + strs1 << robot_list[1].V; + std::string str1 = strs1.str(); + robots_speed_progressBar[1].set_text(str1.substr(0,3)); robots_speed_progressBar[1].set_show_text(true); robots_speed_progressBar[1].set_fraction( (double) robot_list[1].V); robots_speed_vbox[0].pack_start(robots_speed_hbox[2], false, true, 0); @@ -982,7 +988,10 @@ namespace capture robots_speed_vbox[3].pack_start(robots_speed_progressBar[2], false, true, 0); robots_speed_progressBar[2].set_halign(Gtk::ALIGN_CENTER); robots_speed_progressBar[2].set_valign(Gtk::ALIGN_CENTER); - //robots_speed_progressBar[2].set_text(std::to_string(robot_list[2].V).substr(0,3)); + std::ostringstream strs2; + strs2 << robot_list[2].V; + std::string str2 = strs2.str(); + robots_speed_progressBar[2].set_text(str2.substr(0,3)); robots_speed_progressBar[2].set_show_text(true); robots_speed_progressBar[2].set_fraction( (double) robot_list[2].V); robots_speed_vbox[0].pack_start(robots_speed_hbox[3], false, true, 0); @@ -1001,12 +1010,23 @@ namespace capture } void V4LInterface::update_speed_progressBars(){ + std::ostringstream strs0, strs1, strs2; + std::string str0, str1, str2; + robots_speed_progressBar[0].set_fraction( (double) robot_list[0].V/6); - //robots_speed_progressBar[0].set_text(std::to_string(robot_list[0].V).substr(0,3)); + strs0 << (double) robot_list[0].V; + str0 = strs0.str(); + robots_speed_progressBar[0].set_text(str0.substr(0,4)); + robots_speed_progressBar[1].set_fraction( (double) robot_list[1].V/6); - //robots_speed_progressBar[1].set_text(std::to_string(robot_list[1].V).substr(0,3)); + strs1 << (double) robot_list[1].V; + str1 = strs1.str(); + robots_speed_progressBar[1].set_text(str1.substr(0,4)); + robots_speed_progressBar[2].set_fraction( (double) robot_list[2].V/6); - //robots_speed_progressBar[2].set_text(std::to_string(robot_list[2].V).substr(0,3)); + strs2 << (double) robot_list[2].V; + str2 = strs2.str(); + robots_speed_progressBar[2].set_text(str2.substr(0,4)); } void V4LInterface::createFunctionsFrame(){ @@ -1271,8 +1291,8 @@ namespace capture cb_frame_interval_signal = cb_frame_interval.signal_changed().connect(sigc::mem_fun(*this, &V4LInterface::__event_cb_frame_interval_changed)); draw_info_checkbox.signal_clicked().connect(sigc::mem_fun(*this, &capture::V4LInterface::event_draw_info_checkbox_signal_clicked)); - robots_save_bt.signal_clicked().connect(sigc::mem_fun(*this, &capture::V4LInterface::event_robots_save_bt_signal_clicked)); - robots_load_bt.signal_clicked().connect(sigc::mem_fun(*this, &capture::V4LInterface::event_robots_load_bt_signal_clicked)); + robots_save_bt.signal_clicked().connect(sigc::mem_fun(*this, &capture::V4LInterface::__event_bt_save_robots_info_clicked)); + robots_load_bt.signal_clicked().connect(sigc::mem_fun(*this, &capture::V4LInterface::__event_bt_load_robots_info_clicked)); } } diff --git a/pack-capture-gui/capture-gui/V4LInterface-events.cpp b/pack-capture-gui/capture-gui/V4LInterface-events.cpp index 08a12820..7f05b409 100644 --- a/pack-capture-gui/capture-gui/V4LInterface-events.cpp +++ b/pack-capture-gui/capture-gui/V4LInterface-events.cpp @@ -9,6 +9,7 @@ #include "../../cc/filechooser.hpp" #include #include +#include #define DEFAULT_STR " - " @@ -131,11 +132,15 @@ void V4LInterface::__event_bt_load_robots_info_clicked() } getline(txtFile, linha); - robots_speed_hscale[i].set_value(atof(linha.c_str())); - robot_list[i].vmax = (float) robots_speed_hscale[i].get_value(); + double value = atof(linha.c_str()); + robots_speed_hscale[i].set_value(value); + robot_list[i].vmax = (float) value; - robots_speed_progressBar[i].set_fraction( robots_speed_hscale[i].get_value()/6); - //robots_speed_progressBar[i].set_text(std::to_string(robots_speed_hscale[i].get_value()).substr(0,3)); + robots_speed_progressBar[i].set_fraction(robots_speed_hscale[i].get_value()/6); + std::ostringstream strs; + strs << robots_speed_hscale[i].get_value(); + std::string str = strs.str(); + robots_speed_progressBar[i].set_text(str.substr(0,4)); } @@ -1202,103 +1207,6 @@ void V4LInterface::event_draw_info_checkbox_signal_clicked(){ } - void V4LInterface::event_robots_save_bt_signal_clicked(){ - std::ofstream txtFile; - - - if (quick_save_flag) - { - txtFile.open("INFO_quicksave.txt"); - } - else - { - std::cout<<"saving robots info"< Date: Wed, 15 Feb 2017 19:15:05 -0200 Subject: [PATCH 03/20] =?UTF-8?q?Corre=C3=A7=C3=B5es=20nas=20constantes=20?= =?UTF-8?q?da=20Estrat=C3=A9gia=20(Bruno).=20Estrat=C3=A9gia=20agora=20fic?= =?UTF-8?q?a=20dentro=20do=20strategyGUI.hpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cc/Strategy.hpp | 1558 ++++++++++++++++++++++---------------------- cc/camcap.hpp | 52 +- cc/strategyGUI.hpp | 14 +- 3 files changed, 815 insertions(+), 809 deletions(-) diff --git a/cc/Strategy.hpp b/cc/Strategy.hpp index 480d8e92..aa23da53 100644 --- a/cc/Strategy.hpp +++ b/cc/Strategy.hpp @@ -1,9 +1,9 @@ - #ifndef STRATEGY_HPP_ +#ifndef STRATEGY_HPP_ #define STRATEGY_HPP_ #define PI 3.14159265453 #include "opencv2/opencv.hpp" #include "LS.cpp" -#include "../pack-capture-gui/capture-gui/Robot.hpp" +#include "Robot.hpp" #include "SerialW.hpp" #include #define PREDICAO_NUM_SAMPLES 15 @@ -26,787 +26,789 @@ class Strategy { - public: - Robot Goalkeeper; - Robot Attack; - Robot Defense; - Robot Opponent; - cv::Point Ball; - cv::Point FutureBall; - int distBall; - LS LS_ball_x; - LS LS_ball_y; - cv::Point Ball_Est; - cv::Point target = cv::Point(-1,-1); - cv::Point targetlock = cv::Point(-1,-1); - int height, width; - int COMPRIMENTO_CAMPO ; - int LARGURA_CAMPO ; - int TAMANHO_GOL ; - int TAMANHO_AREA ; - bool GOAL_DANGER_ZONE = false; - bool ADV_NA_AREA = false; - int BANHEIRA ; - int DIVISAO_AREAS ; - int OFFSET_BANHEIRA ; - int MEIO_GOL_X ; - int MEIO_GOL_Y ; - int MAX_GOL_Y ; - int MIN_GOL_Y ; - int LINHA_ZAGA ; - int LIMITE_AREA_X ; - // Parametros para atacante sem bola - int OFFSET_RATIO ; - int CONE_RATIO ; - int COUNT_DECISION ; - int MAX_DECISION_COUNT; - int TARGET_LOCK_COUNT; - int MAX_TARGET_LOCK_COUNT; - // Parametros do Defensor na defesa - int DESLOCAMENTO_ZAGA_ATAQUE ; - - Strategy() // TUDO EM METROS - { - LS_ball_x.init(PREDICAO_NUM_SAMPLES,1); - LS_ball_y.init(PREDICAO_NUM_SAMPLES,1); - } - - void set_constants(int w, int h) { - width = w; - height = h; - COMPRIMENTO_CAMPO = round(1.50*float(width)/1.70); - LARGURA_CAMPO = height; - TAMANHO_GOL = round(0.35*float(height)/1.30); - TAMANHO_AREA = 247; - LIMITE_AREA_X = 113; - - BANHEIRA = round((0.50*COMPRIMENTO_CAMPO))+round(0.16*float(width)/1.70); - DIVISAO_AREAS = round((0.50*COMPRIMENTO_CAMPO)); - OFFSET_BANHEIRA= round(0.20*float(width)/1.70); - MEIO_GOL_X = round(COMPRIMENTO_CAMPO+round(0.1*float(width)/1.70)); - MEIO_GOL_Y = round(LARGURA_CAMPO/2); - MAX_GOL_Y = round(MEIO_GOL_Y + TAMANHO_GOL/2); - MIN_GOL_Y = round(MEIO_GOL_Y - TAMANHO_GOL/2); - LINHA_ZAGA = round(0.30*COMPRIMENTO_CAMPO); - - // Parametros para atacante sem bola - OFFSET_RATIO = round(0.12*float(width)/1.70); - CONE_RATIO = round(0.8*float(width)/1.70); - MAX_DECISION_COUNT = 10; - COUNT_DECISION = MAX_DECISION_COUNT; - TARGET_LOCK_COUNT = 10; - MAX_TARGET_LOCK_COUNT = 10; - // Parametros do Defensor na defesa - DESLOCAMENTO_ZAGA_ATAQUE = round(1.3*float(width)/1.70); - - } - - - cv::Point get_atk_target(cv::Point robot, double orientation) { // Estratégia de ataque clássico (Antigo Ojuara) - - Attack.previous_status = Attack.status; - - distBall = sqrt(pow(robot.x - Ball.x, 2) + pow(robot.y - Ball.y, 2)); - // cout<<"Status - "< DIVISAO_AREAS) { //Bola no ataque? - Attack.fixedPos=false; - //cout<<"Bola no Ataque "<<"|"; - if(distBall < round(0.08*float(width)/1.70) ) { - //Posse de bola?????????? SIM - - //Definicao de acao para acelerar em direcao ao gol - if (robot.y > MAX_GOL_Y && orientation >= 0) { - orientation = orientation - double(PI); - } - if (robot.y < MIN_GOL_Y && orientation < 0) { - orientation = orientation + double(PI); - } - - if (robot.x >= COMPRIMENTO_CAMPO - LIMITE_AREA_X && (robot.y < MIN_GOL_Y || robot.y > MAX_GOL_Y) ) { - // Posse de Bola? Retangulo lateral? - - //cout<<"Retangulo Lateral - "; - - // robo esta nos retangulos laterais(com aprox 20cm a partir do fundo do gol) do gol adversario? - - target.x = Ball.x; //Vai na bola - target.y = Ball.y; - Attack.status = 3; - } - else if (Attack.previous_status == 3||COUNT_DECISION atan2(float(MIN_GOL_Y - robot.y),float(COMPRIMENTO_CAMPO - robot.x)) && - orientation < atan2(float(MAX_GOL_Y - robot.y) , float(COMPRIMENTO_CAMPO - robot.x)) && robot.x < Ball.x) { - //Posse de bola? && Orientação robô + bola voltada para o lado adversário? - - // cout<<"Angulos "< MIN_GOL_Y && robot.x < (COMPRIMENTO_CAMPO - 0.1*float(width)/1.70)) { - - Attack.status = 2; - - if ( robot.y < MEIO_GOL_Y) { - target.x = MEIO_GOL_X; - target.y = MAX_GOL_Y; - // cout<<"Centro 1 "<= MEIO_GOL_Y) { - target.x = MEIO_GOL_X; - target.y = MIN_GOL_Y; - // cout<<"Centro 2"< Ball_Est.x+round(0.04*float(width)/1.70)) { // Atacante a frente da bola? - // cout<<"Atk na frente da bola - "; - target.x = Ball_Est.x-round(0.16*float(width)/1.70); //Coisas fuzzy que ninguem entende a partir daqui----------------------------| - - // Ataque adiantado, da a volta na bola - float diff = (float(Ball_Est.y) - float(robot.y))/float(LARGURA_CAMPO); - - float offset = pow(cos(PI*diff/2),6)*float(OFFSET_RATIO)*cos(2*PI*diff); - - float mixrb = abs(diff)*float(Ball_Est.y) + (1-abs(diff))*float(robot.y); - - - - if(diff>0) - target.y = round(mixrb - offset); - else - target.y = round(mixrb + offset); - - // Não permite que o target seja colocado além dos limites do campo - if(target.y<0) - target.y = round(mixrb + offset); - else if(target.y>LARGURA_CAMPO) - target.y = round(mixrb - offset); - - - } - else { - // cout<<"Atk atras da bola - "; - // Ataque recuado - if(robot.x < COMPRIMENTO_CAMPO- round(0.2*float(width)/1.70)) { - // cout<<"Fora da area - "; - - if (distBall < 100 ) { - Attack.status = 1; - // cout<<"Status - "< LARGURA_CAMPO) { - // cout<<"Alvo fora - "; - target.x = Ball_Est.x; - target.y = Ball_Est.y; - } - } else { - // cout<<"Na area - "; - target.x = Ball_Est.x; - target.y = Ball_Est.y; // Fim das coisas fuzzy queninguem entende----------------------------| - } - } - } - } - else { - //cout<<"Bola na Defesa - "; - Attack.fixedPos=true; - //Bola na defesa - target.x = BANHEIRA; - if(Ball.y < LARGURA_CAMPO/2) - target.y = Ball.y + OFFSET_BANHEIRA; - else - target.y = Ball.y - OFFSET_BANHEIRA; - // cout< MAX_GOL_Y) target.y = MIN_GOL_Y; - if ( target.y < MIN_GOL_Y) target.y = MAX_GOL_Y; - } - else { // Sem a bola - //cout<<"Sem Posse de Bola - "; - if (Ball.x < LIMITE_AREA_X&&(Ball.y < LARGURA_CAMPO/2-TAMANHO_AREA/2 || Ball.y > LARGURA_CAMPO/2+TAMANHO_AREA/2)) { - //Bola na linha de fundo - // cout<<"Bola na linha de fundo - "; - if(robot.x>Ball.x+17) { - //Bola atras - //cout<<"Bola atras - "; - if(Ball.yLARGURA_CAMPO/2-TAMANHO_AREA/2) { - target.x = LIMITE_AREA_X; - } else { - target.x = Ball_Est.x; - } - - target.y = LARGURA_CAMPO/2-TAMANHO_AREA/2 - 17; - //cout<<"Fundo superior - "; - } else { - // lINHA DE FUNDO INFERIOR - if(robot.y Ball.x + round(0.04*float(width)/1.70)) { - target.x = Ball_Est.x-round(0.16*float(width)/1.70); //Coisas fuzzy que ninguem entende a partir daqui----------------------------| - //cout<<" Defesa adiantado, da a volta na bola"; - float diff = float(Ball_Est.y - robot.y)/float(LARGURA_CAMPO); - float offset = pow(cos(PI*diff/2),6)*float(OFFSET_RATIO)*cos(2*PI*diff); - float mixrb = abs(diff)*float(Ball_Est.y) + (1-abs(diff))*float(robot.y); - - if(diff>0) - target.y = round(mixrb - offset); - else - target.y = round(mixrb + offset); - - // Não permite que o target seja colocado além dos limites do campo - if(target.y<0) - target.y = round(mixrb + offset); - else if(target.y>LARGURA_CAMPO) - target.y = round(mixrb - offset); - - } - else { - // Ataque recuado - //cout<<"Robo atras - "; -// Cap.Bruno if(robot.x < COMPRIMENTO_CAMPO-round(0.2*float(width)/1.70)) { - //cout<<"Dentro linha area - "; - // Dentro da area - float phi = atan(float(MEIO_GOL_Y - Ball.y))/float(MEIO_GOL_X - Ball.x); // Angulo entre o gol e a bola - float theta = atan(float(MEIO_GOL_Y - robot.y))/float(MEIO_GOL_X - robot.x); // Angulo entre o gol e o atacante - target.x = Ball_Est.x - round(CONE_RATIO*cos(phi)*2*(abs(phi-theta))/PI); - target.y = Ball_Est.y - round(CONE_RATIO*sin(phi)*2*(abs(phi-theta))/PI); - if(target.y<0 || target.y > LARGURA_CAMPO) { - target.x = Ball_Est.x; - target.y = Ball_Est.y; - } -/* Cap.Bruno } else { - //cout<<"fora linha area - "; - target.x = Ball_Est.x; - target.y = Ball_Est.y; // Fim das coisas fuzzy que ninguem entende----------------------------| - } */ - } - } - - - } - else { - // cout<<"Bola no Ataque - "; - // Bola no ataque - target.x = LINHA_ZAGA; - Defense.fixedPos=true; - if (ZAGA_CENTRALIZADA) { - //cout<<"Entrou no if porra "< LARGURA_CAMPO/2-TAMANHO_AREA/2 && target.y < LARGURA_CAMPO/2+TAMANHO_AREA/2)) { - target.x = LINHA_ZAGA; - Defense.fixedPos=true; - target.y = Ball.y; - // cout<<"Nao deixa area - "; - // Não permite que o alvo esteja dentro da área - } - - - - - //cout< MAX_GOL_Y && orientation >= 0) { - orientation = orientation - double(PI); - } - if (robot.y < MIN_GOL_Y && orientation < 0) { - orientation = orientation + double(PI); - } - - if (robot.x <= LIMITE_AREA_X && (robot.y < MIN_GOL_Y || robot.y > MAX_GOL_Y) ) { - // Posse de Bola? Retangulo lateral? - - cout<<"Retangulo Lateral"<<"|"; - - // robo esta nos retangulos laterais(com aprox 20cm a partir do fundo do gol) do gol adversario? - - target.x = Ball.x; //Vai na bola - target.y = Ball.y; - Opponent.status = 3; - } - else if (Opponent.previous_status == 3||COUNT_DECISION atan2(float(MIN_GOL_Y - robot.y),float(0-robot.x)) && - orientation < atan2(float(MAX_GOL_Y - robot.y),float(0-robot.x)) && - robot.x > Ball.x) { - //Posse de bola? && Orientação robô + bola voltada para o lado adversário? - cout<<"Orientação Correta"<<"|"; - // cout<<"Angulos "< MIN_GOL_Y && - robot.x > (0.1*float(width)/1.70)) { - cout<<"Situação de Gol"<<"|"; - Opponent.status = 2; - - if ( robot.y < MEIO_GOL_Y) { - target.x = 0; - target.y = MAX_GOL_Y; - // cout<<"Centro 1 "<= MEIO_GOL_Y) { - target.x = 0; - target.y = MIN_GOL_Y; - // cout<<"Centro 2"< robot.x) - { - target.x = robot.x - 50; - } - else { - target.y = Ball.y; - target.x = Ball.x; - } - - - } - else { //Sem a bola - cout<<"Sem Posse de Bola "<<"|"; - if(robot.x < Ball_Est.x-round(0.04*float(width)/1.70)) { // Atacante a frente da bola? - cout<<"Atk na frente da bola"<<"|"; - target.x = Ball_Est.x+round(0.16*float(width)/1.70); //Coisas fuzzy que ninguem entende a partir daqui----------------------------| - - // Ataque adiantado, da a volta na bola - float diff = (float(Ball_Est.y) - float(robot.y))/float(LARGURA_CAMPO); - - float offset = pow(cos(PI*diff/2),6)*float(OFFSET_RATIO)*cos(2*PI*diff); - - float mixrb = abs(diff)*float(Ball_Est.y) + (1-abs(diff))*float(robot.y); - - - - if(diff>0) - target.y = round(mixrb - offset); - else - target.y = round(mixrb + offset); - - // Não permite que o target seja colocado além dos limites do campo - if(target.y<0) - target.y = round(mixrb + offset); - else if(target.y>LARGURA_CAMPO) - target.y = round(mixrb - offset); - - - } - else { - cout<<"Atk atras da bola"<<"|"; - // Ataque recuado - if(robot.x > round(0.2*float(width)/1.70)) { - cout<<"Fora da area"<<"|"; - - if (distBall < 100 ) { - Opponent.status = 1; - // cout<<"Status - "< LARGURA_CAMPO) { - // cout<<"Alvo fora - "; - target.x = Ball_Est.x; - target.y = Ball_Est.y; - } - } else { - cout<<"Na area"<<"|"; - target.x = Ball_Est.x; - target.y = Ball_Est.y; // Fim das coisas fuzzy queninguem entende----------------------------| - } - } - } - cout< MAX_GOL_Y) target.y = MIN_GOL_Y; - if ( target.y < MIN_GOL_Y) target.y = MAX_GOL_Y; - } - else { // Sem a bola - //cout<<"Sem Posse de Bola - "; - if ((Ball.x > COMPRIMENTO_CAMPO-LIMITE_AREA_X)&&(Ball.y < LARGURA_CAMPO/2-TAMANHO_AREA/2 || Ball.y > LARGURA_CAMPO/2+TAMANHO_AREA/2)) { - //Bola na linha de fundo - // cout<<"Bola na linha de fundo - "; - if(robot.xLARGURA_CAMPO/2-TAMANHO_AREA/2) { - target.x = COMPRIMENTO_CAMPO-LIMITE_AREA_X; - } else { - target.x = Ball_Est.x; - } - - target.y = LARGURA_CAMPO/2-TAMANHO_AREA/2 - 17; - //cout<<"Fundo superior - "; - } else { - // lINHA DE FUNDO INFERIOR - if(robot.y0) - target.y = round(mixrb - offset); - else - target.y = round(mixrb + offset); - - // Não permite que o target seja colocado além dos limites do campo - if(target.y<0) - target.y = round(mixrb + offset); - else if(target.y>LARGURA_CAMPO) - target.y = round(mixrb - offset); - - } - else { - // Ataque recuado - //cout<<"Robo atras - "; -// Cap.Bruno if(robot.x < COMPRIMENTO_CAMPO-round(0.2*float(width)/1.70)) { - //cout<<"Dentro linha area - "; - // Dentro da area - float phi = atan(float(MEIO_GOL_Y - Ball.y))/float(0 - Ball.x); // Angulo entre o gol e a bola - float theta = atan(float(MEIO_GOL_Y - robot.y))/float(0 - robot.x); // Angulo entre o gol e o atacante - target.x = Ball_Est.x + round(CONE_RATIO*cos(phi)*2*(abs(phi-theta))/PI); - target.y = Ball_Est.y + round(CONE_RATIO*sin(phi)*2*(abs(phi-theta))/PI); - if(target.y<0 || target.y > LARGURA_CAMPO) { - target.x = Ball_Est.x; - target.y = Ball_Est.y; - } -/* Cap.Bruno } else { - //cout<<"fora linha area - "; - target.x = Ball_Est.x; - target.y = Ball_Est.y; // Fim das coisas fuzzy que ninguem entende----------------------------| - } */ - } - } - - - } - - - /*if(target.x >COMPRIMENTO_CAMPO-LIMITE_AREA_X && (target.y > LARGURA_CAMPO/2-TAMANHO_AREA/2 && target.y < LARGURA_CAMPO/2+TAMANHO_AREA/2)) { - target.x = COMPRIMENTO_CAMPO-LINHA_ZAGA; - Opponent.fixedPos=true; - target.y = Ball.y; - // cout<<"Nao deixa area - "; - // Não permite que o alvo esteja dentro da área - } */ - - return target; +public: + Robot Goalkeeper; + Robot Attack; + Robot Defense; + Robot Opponent; + cv::Point Ball; + cv::Point FutureBall; + int distBall; + LS LS_ball_x; + LS LS_ball_y; + cv::Point Ball_Est; + cv::Point target = cv::Point(-1,-1); + cv::Point targetlock = cv::Point(-1,-1); + int height, width; + int COMPRIMENTO_CAMPO_TOTAL; + int COMPRIMENTO_PISTA; + int LARGURA_CAMPO ; + int TAMANHO_GOL ; + int TAMANHO_AREA ; + bool GOAL_DANGER_ZONE = false; + bool ADV_NA_AREA = false; + int BANHEIRA ; + int DIVISAO_AREAS ; + int OFFSET_BANHEIRA ; + int MEIO_GOL_X ; + int MEIO_GOL_Y ; + int MAX_GOL_Y ; + int MIN_GOL_Y ; + int LINHA_ZAGA ; + int LIMITE_AREA_X ; + // Parametros para atacante sem bola + int OFFSET_RATIO ; + int CONE_RATIO ; + int COUNT_DECISION ; + int MAX_DECISION_COUNT; + int TARGET_LOCK_COUNT; + int MAX_TARGET_LOCK_COUNT; + // Parametros do Defensor na defesa + int DESLOCAMENTO_ZAGA_ATAQUE ; + + Strategy() // TUDO EM METROS + { + LS_ball_x.init(PREDICAO_NUM_SAMPLES,1); + LS_ball_y.init(PREDICAO_NUM_SAMPLES,1); + } + + void set_constants(int w, int h) { + width = w; + height = h; + COMPRIMENTO_PISTA = round(1.50*float(width)/1.70); // ATENÇÃO !!!!! Valor sem referência, medida absoluto + COMPRIMENTO_CAMPO_TOTAL = width; // Do fund de um gol ao fundo do outro gol + + LARGURA_CAMPO = height; + TAMANHO_GOL = round(0.35*float(height)/1.30); + TAMANHO_AREA = 247; + LIMITE_AREA_X = 113; + + BANHEIRA = round((0.50*COMPRIMENTO_CAMPO_TOTAL))+round(0.16*float(width)/1.70); + DIVISAO_AREAS = round((0.50*COMPRIMENTO_CAMPO_TOTAL)); + OFFSET_BANHEIRA= round(0.20*float(width)/1.70); + MEIO_GOL_X = round(COMPRIMENTO_CAMPO_TOTAL-round(0.05*float(width)/1.70)); + MEIO_GOL_Y = round(LARGURA_CAMPO/2); + MAX_GOL_Y = round(MEIO_GOL_Y + TAMANHO_GOL/2); + MIN_GOL_Y = round(MEIO_GOL_Y - TAMANHO_GOL/2); + LINHA_ZAGA = round(0.30*COMPRIMENTO_CAMPO_TOTAL); + + // Parametros para atacante sem bola + OFFSET_RATIO = round(0.12*float(width)/1.70); + CONE_RATIO = round(0.8*float(width)/1.70); + MAX_DECISION_COUNT = 10; + COUNT_DECISION = MAX_DECISION_COUNT; + TARGET_LOCK_COUNT = 10; + MAX_TARGET_LOCK_COUNT = 10; + // Parametros do Defensor na defesa + DESLOCAMENTO_ZAGA_ATAQUE = round(1.3*float(width)/1.70); + + } + + + cv::Point get_atk_target(cv::Point robot, double orientation) { // Estratégia de ataque clássico (Antigo Ojuara) + + Attack.previous_status = Attack.status; + + distBall = sqrt(pow(robot.x - Ball.x, 2) + pow(robot.y - Ball.y, 2)); + // cout<<"Status - "< DIVISAO_AREAS) { //Bola no ataque? + Attack.fixedPos=false; + //cout<<"Bola no Ataque "<<"|"; + if(distBall < round(0.08*float(width)/1.70) ) { + //Posse de bola?????????? SIM + + //Definicao de acao para acelerar em direcao ao gol + if (robot.y > MAX_GOL_Y && orientation >= 0) { + orientation = orientation - double(PI); + } + if (robot.y < MIN_GOL_Y && orientation < 0) { + orientation = orientation + double(PI); + } + + if (robot.x >= COMPRIMENTO_CAMPO_TOTAL - LIMITE_AREA_X && (robot.y < MIN_GOL_Y || robot.y > MAX_GOL_Y) ) { + // Posse de Bola? Retangulo lateral? + + //cout<<"Retangulo Lateral - "; + + // robo esta nos retangulos laterais(com aprox 20cm a partir do fundo do gol) do gol adversario? + + target.x = Ball.x; //Vai na bola + target.y = Ball.y; + Attack.status = 3; + } + else if (Attack.previous_status == 3||COUNT_DECISION atan2(float(MIN_GOL_Y - robot.y),float((COMPRIMENTO_CAMPO_TOTAL-0.1*float(width)/1.70) - robot.x)) && + orientation < atan2(float(MAX_GOL_Y - robot.y) , float((COMPRIMENTO_CAMPO_TOTAL - 0.1*float(width)/1.70) - robot.x)) && robot.x < Ball.x) { + //Posse de bola? && Orientação robô + bola voltada para o lado adversário? + + // cout<<"Angulos "< MIN_GOL_Y && robot.x < (COMPRIMENTO_CAMPO_TOTAL - LIMITE_AREA_X)) { + + Attack.status = 2; + + if ( robot.y < MEIO_GOL_Y) { + target.x = MEIO_GOL_X; + target.y = MAX_GOL_Y; + // cout<<"Centro 1 "<= MEIO_GOL_Y) { + target.x = MEIO_GOL_X; + target.y = MIN_GOL_Y; + // cout<<"Centro 2"< Ball_Est.x+round(0.04*float(width)/1.70)) { // Atacante a frente da bola? + // cout<<"Atk na frente da bola - "; + target.x = Ball_Est.x-round(0.16*float(width)/1.70); //Coisas fuzzy que ninguem entende a partir daqui----------------------------| + + // Ataque adiantado, da a volta na bola + float diff = (float(Ball_Est.y) - float(robot.y))/float(LARGURA_CAMPO); + + float offset = pow(cos(PI*diff/2),6)*float(OFFSET_RATIO)*cos(2*PI*diff); + + float mixrb = abs(diff)*float(Ball_Est.y) + (1-abs(diff))*float(robot.y); + + + + if(diff>0) + target.y = round(mixrb - offset); + else + target.y = round(mixrb + offset); + + // Não permite que o target seja colocado além dos limites do campo + if(target.y<0) + target.y = round(mixrb + offset); + else if(target.y>LARGURA_CAMPO) + target.y = round(mixrb - offset); + + + } + else { + // cout<<"Atk atras da bola - "; + // Ataque recuado + if(robot.x < COMPRIMENTO_CAMPO_TOTAL - LIMITE_AREA_X) { + // cout<<"Fora da area - "; + + if (distBall < 100 ) { + Attack.status = 1; + // cout<<"Status - "< LARGURA_CAMPO) { + // cout<<"Alvo fora - "; + target.x = Ball_Est.x; + target.y = Ball_Est.y; + } + } else { + // cout<<"Na area - "; + target.x = Ball_Est.x; + target.y = Ball_Est.y; // Fim das coisas fuzzy queninguem entende----------------------------| + } + } + } + } + else { + //cout<<"Bola na Defesa - "; + Attack.fixedPos=true; + //Bola na defesa + target.x = BANHEIRA; + if(Ball.y < LARGURA_CAMPO/2) + target.y = Ball.y + OFFSET_BANHEIRA; + else + target.y = Ball.y - OFFSET_BANHEIRA; + // cout< MAX_GOL_Y) target.y = MIN_GOL_Y; + if ( target.y < MIN_GOL_Y) target.y = MAX_GOL_Y; + } + else { // Sem a bola + //cout<<"Sem Posse de Bola - "; + if (Ball.x < LIMITE_AREA_X&&(Ball.y < LARGURA_CAMPO/2-TAMANHO_AREA/2 || Ball.y > LARGURA_CAMPO/2+TAMANHO_AREA/2)) { + //Bola na linha de fundo + // cout<<"Bola na linha de fundo - "; + if(robot.x>Ball.x+17) { + //Bola atras + //cout<<"Bola atras - "; + if(Ball.yLARGURA_CAMPO/2-TAMANHO_AREA/2) { + target.x = LIMITE_AREA_X; + } else { + target.x = Ball_Est.x; + } + + target.y = LARGURA_CAMPO/2-TAMANHO_AREA/2 - 17; + //cout<<"Fundo superior - "; + } else { + // lINHA DE FUNDO INFERIOR + if(robot.y Ball.x + round(0.04*float(width)/1.70)) { + target.x = Ball_Est.x-round(0.16*float(width)/1.70); //Coisas fuzzy que ninguem entende a partir daqui----------------------------| + //cout<<" Defesa adiantado, da a volta na bola"; + float diff = float(Ball_Est.y - robot.y)/float(LARGURA_CAMPO); + float offset = pow(cos(PI*diff/2),6)*float(OFFSET_RATIO)*cos(2*PI*diff); + float mixrb = abs(diff)*float(Ball_Est.y) + (1-abs(diff))*float(robot.y); + + if(diff>0) + target.y = round(mixrb - offset); + else + target.y = round(mixrb + offset); + + // Não permite que o target seja colocado além dos limites do campo + if(target.y<0) + target.y = round(mixrb + offset); + else if(target.y>LARGURA_CAMPO) + target.y = round(mixrb - offset); + + } + else { + // Ataque recuado + // Cap.Bruno if(robot.x < COMPRIMENTO_CAMPO-round(0.2*float(width)/1.70)) { + //cout<<"Dentro linha area - "; + // Dentro da area + float phi = atan(float(MEIO_GOL_Y - Ball.y))/float(MEIO_GOL_X - Ball.x); // Angulo entre o gol e a bola + float theta = atan(float(MEIO_GOL_Y - robot.y))/float(MEIO_GOL_X - robot.x); // Angulo entre o gol e o atacante + target.x = Ball_Est.x - round(CONE_RATIO*cos(phi)*2*(abs(phi-theta))/PI); + target.y = Ball_Est.y - round(CONE_RATIO*sin(phi)*2*(abs(phi-theta))/PI); + if(target.y<0 || target.y > LARGURA_CAMPO) { + target.x = Ball_Est.x; + target.y = Ball_Est.y; + } + /* Cap.Bruno } else { + //cout<<"fora linha area - "; + target.x = Ball_Est.x; + target.y = Ball_Est.y; // Fim das coisas fuzzy que ninguem entende----------------------------| + } */ + } + } + + + } + else { + // cout<<"Bola no Ataque - "; + // Bola no ataque + target.x = LINHA_ZAGA; + Defense.fixedPos=true; + if (ZAGA_CENTRALIZADA) { + //cout<<"Entrou no if porra "< LARGURA_CAMPO/2-TAMANHO_AREA/2 && target.y < LARGURA_CAMPO/2+TAMANHO_AREA/2)) { + target.x = LINHA_ZAGA; + Defense.fixedPos=true; + target.y = Ball.y; + // cout<<"Nao deixa area - "; + // Não permite que o alvo esteja dentro da área + } + + + + + //cout< MAX_GOL_Y && orientation >= 0) { + orientation = orientation - double(PI); } + if (robot.y < MIN_GOL_Y && orientation < 0) { + orientation = orientation + double(PI); + } + + if (robot.x <= LIMITE_AREA_X && (robot.y < MIN_GOL_Y || robot.y > MAX_GOL_Y) ) { + // Posse de Bola? Retangulo lateral? + + cout<<"Retangulo Lateral"<<"|"; + // robo esta nos retangulos laterais(com aprox 20cm a partir do fundo do gol) do gol adversario? + target.x = Ball.x; //Vai na bola + target.y = Ball.y; + Opponent.status = 3; + } + else if (Opponent.previous_status == 3||COUNT_DECISION atan2(float(MIN_GOL_Y - robot.y),float(0-robot.x)) && + orientation < atan2(float(MAX_GOL_Y - robot.y),float(0-robot.x)) && + robot.x > Ball.x) { + //Posse de bola? && Orientação robô + bola voltada para o lado adversário? + cout<<"Orientação Correta"<<"|"; + // cout<<"Angulos "< MIN_GOL_Y && + robot.x > (0.1*float(width)/1.70)) { + cout<<"Situação de Gol"<<"|"; + Opponent.status = 2; + + if ( robot.y < MEIO_GOL_Y) { + target.x = 0; + target.y = MAX_GOL_Y; + // cout<<"Centro 1 "<= MEIO_GOL_Y) { + target.x = 0; + target.y = MIN_GOL_Y; + // cout<<"Centro 2"< robot.x) + { + target.x = robot.x - 50; + } + else { + target.y = Ball.y; + target.x = Ball.x; + } + + + } + else { //Sem a bola + cout<<"Sem Posse de Bola "<<"|"; + if(robot.x < Ball_Est.x-round(0.04*float(width)/1.70)) { // Atacante a frente da bola? + cout<<"Atk na frente da bola"<<"|"; + target.x = Ball_Est.x+round(0.16*float(width)/1.70); //Coisas fuzzy que ninguem entende a partir daqui----------------------------| + + // Ataque adiantado, da a volta na bola + float diff = (float(Ball_Est.y) - float(robot.y))/float(LARGURA_CAMPO); + + float offset = pow(cos(PI*diff/2),6)*float(OFFSET_RATIO)*cos(2*PI*diff); + + float mixrb = abs(diff)*float(Ball_Est.y) + (1-abs(diff))*float(robot.y); + + + + if(diff>0) + target.y = round(mixrb - offset); + else + target.y = round(mixrb + offset); + + // Não permite que o target seja colocado além dos limites do campo + if(target.y<0) + target.y = round(mixrb + offset); + else if(target.y>LARGURA_CAMPO) + target.y = round(mixrb - offset); + + + } + else { + cout<<"Atk atras da bola"<<"|"; + // Ataque recuado + if(robot.x > round(0.2*float(width)/1.70)) { + cout<<"Fora da area"<<"|"; + + if (distBall < 100 ) { + Opponent.status = 1; + // cout<<"Status - "< LARGURA_CAMPO) { + // cout<<"Alvo fora - "; + target.x = Ball_Est.x; + target.y = Ball_Est.y; + } + } else { + cout<<"Na area"<<"|"; + target.x = Ball_Est.x; + target.y = Ball_Est.y; // Fim das coisas fuzzy queninguem entende----------------------------| + } + } + } + cout< MAX_GOL_Y) target.y = MIN_GOL_Y; + if ( target.y < MIN_GOL_Y) target.y = MAX_GOL_Y; + } + else { // Sem a bola + //cout<<"Sem Posse de Bola - "; + if ((Ball.x > COMPRIMENTO_CAMPO_TOTAL-LIMITE_AREA_X)&&(Ball.y < LARGURA_CAMPO/2-TAMANHO_AREA/2 || Ball.y > LARGURA_CAMPO/2+TAMANHO_AREA/2)) { + //Bola na linha de fundo + // cout<<"Bola na linha de fundo - "; + if(robot.xLARGURA_CAMPO/2-TAMANHO_AREA/2) { + target.x = COMPRIMENTO_CAMPO_TOTAL-LIMITE_AREA_X; + } else { + target.x = Ball_Est.x; + } + + target.y = LARGURA_CAMPO/2-TAMANHO_AREA/2 - 17; + //cout<<"Fundo superior - "; + } else { + // lINHA DE FUNDO INFERIOR + if(robot.y0) + target.y = round(mixrb - offset); + else + target.y = round(mixrb + offset); + + // Não permite que o target seja colocado além dos limites do campo + if(target.y<0) + target.y = round(mixrb + offset); + else if(target.y>LARGURA_CAMPO) + target.y = round(mixrb - offset); + + } + else { + // Ataque recuado + //cout<<"Robo atras - "; + // Cap.Bruno if(robot.x < COMPRIMENTO_CAMPO-round(0.2*float(width)/1.70)) { + //cout<<"Dentro linha area - "; + // Dentro da area + float phi = atan(float(MEIO_GOL_Y - Ball.y))/float(0 - Ball.x); // Angulo entre o gol e a bola + float theta = atan(float(MEIO_GOL_Y - robot.y))/float(0 - robot.x); // Angulo entre o gol e o atacante + target.x = Ball_Est.x + round(CONE_RATIO*cos(phi)*2*(abs(phi-theta))/PI); + target.y = Ball_Est.y + round(CONE_RATIO*sin(phi)*2*(abs(phi-theta))/PI); + if(target.y<0 || target.y > LARGURA_CAMPO) { + target.x = Ball_Est.x; + target.y = Ball_Est.y; + } + /* Cap.Bruno } else { + //cout<<"fora linha area - "; + target.x = Ball_Est.x; + target.y = Ball_Est.y; // Fim das coisas fuzzy que ninguem entende----------------------------| + } */ + } + } + + +} + + +/*if(target.x >COMPRIMENTO_CAMPO-LIMITE_AREA_X && (target.y > LARGURA_CAMPO/2-TAMANHO_AREA/2 && target.y < LARGURA_CAMPO/2+TAMANHO_AREA/2)) { +target.x = COMPRIMENTO_CAMPO-LINHA_ZAGA; +Opponent.fixedPos=true; +target.y = Ball.y; +// cout<<"Nao deixa area - "; +// Não permite que o alvo esteja dentro da área +} */ + +return target; + +} + + + +cv::Point get_Ball_Est() { + return Ball_Est; +} +void set_Ball_Est(cv::Point b) { + Ball_Est = b; +} +void set_Ball(cv::Point b) { + Ball = b; + //LS_ball_x.addValue(Ball.x); + //LS_ball_y.addValue(Ball.y); + + // Ball_Est.x = LS_ball_x.estimate(5); + //Ball_Est.y = LS_ball_y.estimate(5); + +} + + +cv::Point get_gk_target(vector< cv::Point > Adv_Main) { + Goalkeeper.fixedPos=true; + Goalkeeper.target.x = 60; + if (Ball.x < DIVISAO_AREAS) + { // Bola na defesa + + Goalkeeper.target.y = Ball_Est.y; + // ir na projeção da bola na linha de fundo + + if(Ball.x MAX_GOL_Y) + Goalkeeper.target.y = MAX_GOL_Y; + else if (Goalkeeper.target.y < MIN_GOL_Y) + Goalkeeper.target.y = MIN_GOL_Y; + + + + + /* if( Ball.xLARGURA_CAMPO/2-TAMANHO_AREA/2 && Ball.yLARGURA_CAMPO/2-TAMANHO_AREA/2&&Adv_Main[i].yLARGURA_CAMPO/2+TAMANHO_AREA/2)) +{ - cv::Point get_Ball_Est() { - return Ball_Est; - } - void set_Ball_Est(cv::Point b) { - Ball_Est = b; - } - void set_Ball(cv::Point b) { - Ball = b; - LS_ball_x.addValue(Ball.x); - LS_ball_y.addValue(Ball.y); - - Ball_Est.x = LS_ball_x.estimate(5); - Ball_Est.y = LS_ball_y.estimate(5); - - } - - - cv::Point get_gk_target(vector< cv::Point > Adv_Main) { - Goalkeeper.fixedPos=true; - Goalkeeper.target.x = 60; - if (Ball.x < DIVISAO_AREAS) - { // Bola na defesa - - Goalkeeper.target.y = Ball_Est.y; - // ir na projeção da bola na linha de fundo - - if(Ball.x MAX_GOL_Y) - Goalkeeper.target.y = MAX_GOL_Y; - else if (Goalkeeper.target.y < MIN_GOL_Y) - Goalkeeper.target.y = MIN_GOL_Y; - - - - - /* if( Ball.xLARGURA_CAMPO/2-TAMANHO_AREA/2 && Ball.yLARGURA_CAMPO/2-TAMANHO_AREA/2&&Adv_Main[i].yLARGURA_CAMPO/2+TAMANHO_AREA/2)) - { - - //cout<<"bola nas laterais da area"< LARGURA_CAMPO/2) - Goalkeeper.target.y = 354; - else if (Ball.y < LARGURA_CAMPO/2) - Goalkeeper.target.y = 135; - } - } - else - { // Bola no ataque - Goalkeeper.target.y = Ball_Est.y; - // ir na projeção da bola na linha de fundo - - if (Goalkeeper.target.y > MAX_GOL_Y) - Goalkeeper.target.y = MAX_GOL_Y; - else if (Goalkeeper.target.y < MIN_GOL_Y) - Goalkeeper.target.y = MIN_GOL_Y; - - } - //cout<<"GK TARGET "< LARGURA_CAMPO/2) + Goalkeeper.target.y = 354; + else if (Ball.y < LARGURA_CAMPO/2) + Goalkeeper.target.y = 135; +} +} +else +{ // Bola no ataque + Goalkeeper.target.y = Ball_Est.y; + // ir na projeção da bola na linha de fundo + + if (Goalkeeper.target.y > MAX_GOL_Y) + Goalkeeper.target.y = MAX_GOL_Y; + else if (Goalkeeper.target.y < MIN_GOL_Y) + Goalkeeper.target.y = MIN_GOL_Y; + +} +//cout<<"GK TARGET "< #include #include "CPUTimer.cpp" -#include "Strategy.hpp" + boost::mutex io_mutex; boost::thread_group threshold_threads; struct ImageGray; @@ -39,7 +39,7 @@ class CamCap: public Gtk::HBox { public: - Strategy strats; + cv::Mat image_copy; cv::Point Ball_Est; @@ -192,13 +192,13 @@ class CamCap: { width = gdk_screen_get_width(screen)/2; height = gdk_screen_get_height(screen)/2; - strats.set_constants(width,height); + strategy.strats.set_constants(width,height); } else {*/ width = v.vcap.format_dest.fmt.pix.width; height = v.vcap.format_dest.fmt.pix.height; - strats.set_constants(width,height); + strategy.strats.set_constants(width,height); //} @@ -468,8 +468,8 @@ class CamCap: center.x = state.at(0); center.y = state.at(1); circle(image,center, 5, cv::Scalar(0,0,255), 2); */ - // strats.set_Ball(Ball); - //strats.set_Ball_Est(center); + // strategy.strats.set_Ball(Ball); + //strategy.strats.set_Ball_Est(center); for(int i=0; i +#include "Strategy.hpp" class StrategyGUI: public Gtk::VBox { @@ -42,7 +43,7 @@ class StrategyGUI: public Gtk::VBox Gtk::VBox info_text_hbox; Gtk::Image strategy_img; - + Gtk::Button select_button; Gtk::Label strategy_title_label; @@ -50,8 +51,11 @@ class StrategyGUI: public Gtk::VBox Gtk::TextView strategy_description_view; Glib::RefPtr strategy_description_text; + Strategy strats; + StrategyGUI() { + createSelectionFrame(); createMenuFrame(); createInfoTextFrame(); @@ -75,7 +79,7 @@ class StrategyGUI: public Gtk::VBox menu_fm.add(menu_vbox); - + menu_hbox[0].pack_start(m_TreeView, false, true, 5); menu_hbox[0].set_halign(Gtk::ALIGN_CENTER); m_TreeView.set_size_request(450,200); @@ -109,7 +113,7 @@ class StrategyGUI: public Gtk::VBox m_ScrolledWindow.set_min_content_width(450); strategy_description_text = Gtk::TextBuffer::create(); strategy_description_text->set_text("This is the text from TextBuffer #1.\n\n\n\n"); - strategy_description_view.set_buffer(strategy_description_text); + strategy_description_view.set_buffer(strategy_description_text); } void createInfoImageFrame() @@ -123,7 +127,7 @@ class StrategyGUI: public Gtk::VBox ~StrategyGUI() {} - + @@ -132,4 +136,4 @@ class StrategyGUI: public Gtk::VBox }; -#endif /* STRATEGYGUI_HPP_ */ \ No newline at end of file +#endif /* STRATEGYGUI_HPP_ */ From fac16093f1ec8460e45c89463fada59b6ea9f6a7 Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 15 Feb 2017 23:25:36 -0200 Subject: [PATCH 04/20] =?UTF-8?q?Adicionado=20os=20indicadores=20de=20cons?= =?UTF-8?q?tantes=20da=20estrat=C3=A9gia=20na=20imagem=20da=20c=C3=A2mera?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cc/Strategy.hpp | 2 +- cc/camcap.hpp | 56 +++++++++ cc/strategyGUI.hpp | 291 ++++++++++++++++++++++++++++++++++++++++++++- 3 files changed, 344 insertions(+), 5 deletions(-) diff --git a/cc/Strategy.hpp b/cc/Strategy.hpp index aa23da53..5b5ac66e 100644 --- a/cc/Strategy.hpp +++ b/cc/Strategy.hpp @@ -3,7 +3,7 @@ #define PI 3.14159265453 #include "opencv2/opencv.hpp" #include "LS.cpp" -#include "Robot.hpp" +#include "../pack-capture-gui/capture-gui/Robot.hpp" #include "SerialW.hpp" #include #define PREDICAO_NUM_SAMPLES 15 diff --git a/cc/camcap.hpp b/cc/camcap.hpp index 656888d2..ef27c965 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -441,6 +441,8 @@ class CamCap: robot_creation_unitag(); //robot_creation(); + drawStrategyConstants(image, w, h); + if (!v.draw_info_flag) { circle(image,v.robot_list[0].position, 15, cv::Scalar(255,255,0), 2); @@ -663,6 +665,60 @@ class CamCap: } + void drawStrategyConstants(cv::Mat image, int w, int h) + { + if (strategy.get_deslocamentoZagaAtaque_flag()) + line(image,cv::Point(strategy.strats.DESLOCAMENTO_ZAGA_ATAQUE, 0),cv::Point(strategy.strats.DESLOCAMENTO_ZAGA_ATAQUE, h),cv::Scalar(255,255,0), 2); + + if (strategy.get_goalSize_flag()) + line(image,cv::Point(strategy.strats.TAMANHO_GOL, h/2-strategy.strats.TAMANHO_GOL/2),cv::Point(strategy.strats.TAMANHO_GOL, h/2+strategy.strats.TAMANHO_GOL/2),cv::Scalar(255,255,0), 2); + + if (strategy.get_fieldWwidth_flag()) + line(image,cv::Point(0, h/2),cv::Point(strategy.strats.LARGURA_CAMPO, h/2),cv::Scalar(255,255,0), 2); + + if (strategy.get_totalFieldLength_flag()) + line(image,cv::Point(w/2, 0),cv::Point(w/2, strategy.strats.COMPRIMENTO_CAMPO_TOTAL),cv::Scalar(255,255,0), 2); + + if (strategy.get_fieldLength_flag()) + line(image,cv::Point(strategy.strats.TAMANHO_GOL, h/2),cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL-strategy.strats.TAMANHO_GOL, h/2),cv::Scalar(255,255,0), 2); + + if (strategy.get_coneRatio_flag()) + line(image,cv::Point(strategy.strats.CONE_RATIO, 0),cv::Point(strategy.strats.CONE_RATIO, h),cv::Scalar(255,255,0), 2); + + if (strategy.get_offsetRatio_flag()) + line(image,cv::Point(strategy.strats.OFFSET_RATIO, 0),cv::Point(strategy.strats.OFFSET_RATIO, h),cv::Scalar(255,255,0), 2); + + if (strategy.get_defenseLine_flag()) + line(image,cv::Point(strategy.strats.LINHA_ZAGA, 0),cv::Point(strategy.strats.LINHA_ZAGA, h),cv::Scalar(255,255,0), 2); + + if (strategy.get_goalMax_flag()) + putText(image, "X", cv::Point(w-strategy.strats.COMPRIMENTO_PISTA, strategy.strats.MEIO_GOL_Y+strategy.strats.TAMANHO_GOL/2),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); + + if (strategy.get_goalMin_flag()) + putText(image, "X", cv::Point(w-strategy.strats.COMPRIMENTO_PISTA, strategy.strats.MEIO_GOL_Y-strategy.strats.TAMANHO_GOL/2),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); + + if (strategy.get_goalCenterY_flag()) + putText(image, "X", cv::Point(strategy.strats.MEIO_GOL_X, strategy.strats.MEIO_GOL_Y),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); + + if (strategy.get_goalCenterX_flag()) + putText(image, "X", cv::Point(strategy.strats.MEIO_GOL_X, strategy.strats.MEIO_GOL_Y),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); + + if (strategy.get_banheira_flag()) + line(image,cv::Point(strategy.strats.BANHEIRA, 0),cv::Point(strategy.strats.BANHEIRA, h),cv::Scalar(255,255,0), 2); + + if (strategy.get_banheiraOffset_flag()) + line(image,cv::Point(strategy.strats.OFFSET_BANHEIRA, 0),cv::Point(strategy.strats.OFFSET_BANHEIRA, h),cv::Scalar(255,255,0), 2); + + if (strategy.get_areasDivision_flag()) + line(image,cv::Point(strategy.strats.DIVISAO_AREAS, 0),cv::Point(strategy.strats.DIVISAO_AREAS, h),cv::Scalar(255,255,0), 2); + + if (strategy.get_areaSize_flag()) + line(image,cv::Point(strategy.strats.TAMANHO_AREA, 0),cv::Point(strategy.strats.TAMANHO_AREA, h),cv::Scalar(255,255,0), 2); + + if (strategy.get_areaLimitX_flag()) + line(image,cv::Point(strategy.strats.LIMITE_AREA_X, 0),cv::Point(strategy.strats.LIMITE_AREA_X, h),cv::Scalar(255,255,0), 2); + } + void robot_creation_unitag() { vector robot; diff --git a/cc/strategyGUI.hpp b/cc/strategyGUI.hpp index a7ea7df6..9460d41a 100644 --- a/cc/strategyGUI.hpp +++ b/cc/strategyGUI.hpp @@ -32,6 +32,7 @@ class StrategyGUI: public Gtk::VBox Gtk::Frame selection_fm; Gtk::Frame info_text_fm; Gtk::Frame info_img_fm; + Gtk::Frame constants_fm; Gtk::TreeView m_TreeView; Glib::RefPtr m_refTreeModel; @@ -53,13 +54,294 @@ class StrategyGUI: public Gtk::VBox Strategy strats; +protected: + Gtk::Grid constants_grid; + Gtk::CheckButton fieldLength_bt; + Gtk::CheckButton totalFieldLength_bt; + Gtk::CheckButton fieldWwidth_bt; + Gtk::CheckButton goalSize_bt; + Gtk::CheckButton areaSize_bt; + Gtk::CheckButton areaLimitX_bt; + Gtk::CheckButton banheira_bt; + Gtk::CheckButton areasDivision_bt; + Gtk::CheckButton banheiraOffset_bt; + Gtk::CheckButton goalCenterX_bt; + Gtk::CheckButton goalCenterY_bt; + Gtk::CheckButton goalMin_bt; + Gtk::CheckButton goalMax_bt; + Gtk::CheckButton defenseLine_bt; + Gtk::CheckButton offsetRatio_bt; + Gtk::CheckButton coneRatio_bt; + Gtk::CheckButton deslocamentoZagaAtaque_bt; + +private: + bool deslocamentoZagaAtaque_flag = false; + bool goalSize_flag = false; + bool fieldWwidth_flag = false; + bool totalFieldLength_flag = false; + bool fieldLength_flag = false; + bool coneRatio_flag = false; + bool offsetRatio_flag = false; + bool defenseLine_flag = false; + bool goalMax_flag = false; + bool goalMin_flag = false; + bool goalCenterY_flag = false; + bool goalCenterX_flag = false; + bool banheiraOffset_flag = false; + bool areasDivision_flag = false; + bool banheira_flag = false; + bool areaLimitX_flag = false; + bool areaSize_flag = false; + +public: + + bool get_deslocamentoZagaAtaque_flag() + { + return deslocamentoZagaAtaque_flag; + } + + bool get_goalSize_flag() + { + return goalSize_flag; + } + + bool get_fieldWwidth_flag() + { + return fieldWwidth_flag; + } + + bool get_totalFieldLength_flag() + { + return totalFieldLength_flag; + } + + bool get_fieldLength_flag() + { + return fieldLength_flag; + } + + bool get_coneRatio_flag() + { + return coneRatio_flag; + } + + bool get_offsetRatio_flag() + { + return offsetRatio_flag; + } + + bool get_defenseLine_flag() + { + return defenseLine_flag; + } + + bool get_goalMax_flag() + { + return goalMax_flag; + } + + bool get_goalMin_flag() + { + return goalMin_flag; + } + + bool get_goalCenterY_flag() + { + return goalCenterY_flag; + } + + bool get_goalCenterX_flag() + { + return goalCenterX_flag; + } + + bool get_banheiraOffset_flag() + { + return banheiraOffset_flag; + } + + bool get_areasDivision_flag() + { + return areasDivision_flag; + } + + bool get_banheira_flag() + { + return banheira_flag; + } + + bool get_areaSize_flag() + { + return areaSize_flag; + } + + bool get_areaLimitX_flag() + { + return areaLimitX_flag; + } + StrategyGUI() { + createCheckConstantsFrame(); + //createSelectionFrame(); + //createMenuFrame(); + //createInfoTextFrame(); + //createInfoImageFrame(); + } + + void createCheckConstantsFrame() + { + pack_start(constants_fm, false, true, 5); + constants_fm.add(constants_grid); + constants_fm.set_label("Draw Constants"); + + fieldLength_bt.set_label("Field Length"); + totalFieldLength_bt.set_label("Total Field Length"); + fieldWwidth_bt.set_label("Field Width"); + goalSize_bt.set_label("Goal Size"); + areaSize_bt.set_label("Area Size"); + areaLimitX_bt.set_label("Area Limit X"); + banheira_bt.set_label("Banheira"); + areasDivision_bt.set_label("Areas Division"); + banheiraOffset_bt.set_label("Banheira Offset"); + goalCenterX_bt.set_label("Goal Center X"); + goalCenterY_bt.set_label("Goal Center Y"); + goalMin_bt.set_label("Goal Min."); + goalMax_bt.set_label("Goal Max."); + defenseLine_bt.set_label("Defense Line"); + offsetRatio_bt.set_label("Offset Ratio"); + coneRatio_bt.set_label("Cone Ratio"); + deslocamentoZagaAtaque_bt.set_label("Deslocamento Zaga Ataque"); + + constants_grid.set_border_width(10); + constants_grid.set_column_spacing(5); + + constants_grid.add(fieldLength_bt); + constants_grid.add(totalFieldLength_bt); + constants_grid.add(fieldWwidth_bt); + + constants_grid.attach_next_to(goalSize_bt, fieldLength_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach_next_to(areaSize_bt, totalFieldLength_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach_next_to(areaLimitX_bt, fieldWwidth_bt, Gtk::POS_BOTTOM, 1, 1); + + constants_grid.attach_next_to(banheira_bt, goalSize_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach_next_to(areasDivision_bt, areaSize_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach_next_to(banheiraOffset_bt, areaLimitX_bt, Gtk::POS_BOTTOM, 1, 1); + + constants_grid.attach_next_to(goalCenterX_bt, banheira_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach_next_to(goalCenterY_bt, areasDivision_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach_next_to(goalMin_bt, banheiraOffset_bt, Gtk::POS_BOTTOM, 1, 1); + + constants_grid.attach_next_to(goalMax_bt, goalCenterX_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach_next_to(defenseLine_bt, goalCenterY_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach_next_to(offsetRatio_bt, goalMin_bt, Gtk::POS_BOTTOM, 1, 1); + + constants_grid.attach_next_to(coneRatio_bt, goalMax_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach_next_to(deslocamentoZagaAtaque_bt, defenseLine_bt, Gtk::POS_BOTTOM, 1, 1); + + fieldLength_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_fieldLength_bt_clicked)); + totalFieldLength_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_totalFieldLength_bt_clicked)); + fieldWwidth_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_fieldWwidth_bt_clicked)); + goalSize_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalSize_bt_clicked)); + areaSize_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_areaSize_bt_clicked)); + areaLimitX_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_areaLimitX_bt_clicked)); + banheira_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_banheira_bt_clicked)); + areasDivision_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_areasDivision_bt_clicked)); + banheiraOffset_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_banheiraOffset_bt_clicked)); + goalCenterX_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalCenterX_bt_clicked)); + goalCenterY_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalCenterY_bt_clicked)); + goalMin_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalMin_bt_clicked)); + goalMax_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalMax_bt_clicked)); + defenseLine_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_defenseLine_bt_clicked)); + offsetRatio_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_offsetRatio_bt_clicked)); + coneRatio_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_coneRatio_bt_clicked)); + deslocamentoZagaAtaque_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_deslocamentoZagaAtaque_bt_clicked)); + } + +private: + void _event_fieldLength_bt_clicked() + { + fieldLength_flag = !fieldLength_flag; + } + + void _event_totalFieldLength_bt_clicked() + { + totalFieldLength_flag = !totalFieldLength_flag; + } + + void _event_fieldWwidth_bt_clicked() + { + fieldWwidth_flag = !fieldWwidth_flag; + } + + void _event_goalSize_bt_clicked() + { + goalSize_flag = !goalSize_flag; + } + + void _event_areaSize_bt_clicked() + { + areaSize_flag = !areaSize_flag; + } + + void _event_areaLimitX_bt_clicked() + { + areaLimitX_flag = !areaLimitX_flag; + } + + void _event_banheira_bt_clicked() + { + banheira_flag = !banheira_flag; + } + + void _event_areasDivision_bt_clicked() + { + areasDivision_flag = !areasDivision_flag; + } + + void _event_banheiraOffset_bt_clicked() + { + banheiraOffset_flag = !banheiraOffset_flag; + } + + void _event_goalCenterX_bt_clicked() + { + goalCenterX_flag = !goalCenterX_flag; + } - createSelectionFrame(); - createMenuFrame(); - createInfoTextFrame(); - createInfoImageFrame(); + void _event_goalCenterY_bt_clicked() + { + goalCenterY_flag = !goalCenterY_flag; + } + + void _event_goalMin_bt_clicked() + { + goalMin_flag = !goalMin_flag; + } + + void _event_goalMax_bt_clicked() + { + goalMax_flag = !goalMax_flag; + } + + void _event_defenseLine_bt_clicked() + { + defenseLine_flag = !defenseLine_flag; + } + + void _event_offsetRatio_bt_clicked() + { + offsetRatio_flag = !offsetRatio_flag; + } + + void _event_coneRatio_bt_clicked() + { + coneRatio_flag = !coneRatio_flag; + } + + void _event_deslocamentoZagaAtaque_bt_clicked() + { + deslocamentoZagaAtaque_flag = !deslocamentoZagaAtaque_flag; } void createSelectionFrame() @@ -73,6 +355,7 @@ class StrategyGUI: public Gtk::VBox selection_hbox.pack_start(strategy_title_label, false, true, 5); } +public: void createMenuFrame() { pack_start(menu_fm, false, true, 5); From 750c6938d22052604aba4ffe34ba24594c56ab94 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 16 Feb 2017 20:50:19 -0200 Subject: [PATCH 05/20] =?UTF-8?q?Corre=C3=A7=C3=B5es=20no=20desenho=20das?= =?UTF-8?q?=20constantes=20da=20estrat=C3=A9gia?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cc/camcap.hpp | 7 ++---- cc/strategyGUI.hpp | 63 ++++++++++++++++++---------------------------- 2 files changed, 26 insertions(+), 44 deletions(-) diff --git a/cc/camcap.hpp b/cc/camcap.hpp index ef27c965..9f3fd303 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -680,7 +680,7 @@ class CamCap: line(image,cv::Point(w/2, 0),cv::Point(w/2, strategy.strats.COMPRIMENTO_CAMPO_TOTAL),cv::Scalar(255,255,0), 2); if (strategy.get_fieldLength_flag()) - line(image,cv::Point(strategy.strats.TAMANHO_GOL, h/2),cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL-strategy.strats.TAMANHO_GOL, h/2),cv::Scalar(255,255,0), 2); + line(image,cv::Point(round(0.10*float(h)/1.70), h/2),cv::Point(strategy.strats.COMPRIMENTO_PISTA, h/2),cv::Scalar(255,255,0), 2); if (strategy.get_coneRatio_flag()) line(image,cv::Point(strategy.strats.CONE_RATIO, 0),cv::Point(strategy.strats.CONE_RATIO, h),cv::Scalar(255,255,0), 2); @@ -697,10 +697,7 @@ class CamCap: if (strategy.get_goalMin_flag()) putText(image, "X", cv::Point(w-strategy.strats.COMPRIMENTO_PISTA, strategy.strats.MEIO_GOL_Y-strategy.strats.TAMANHO_GOL/2),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); - if (strategy.get_goalCenterY_flag()) - putText(image, "X", cv::Point(strategy.strats.MEIO_GOL_X, strategy.strats.MEIO_GOL_Y),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); - - if (strategy.get_goalCenterX_flag()) + if (strategy.get_goalCenter_flag()) putText(image, "X", cv::Point(strategy.strats.MEIO_GOL_X, strategy.strats.MEIO_GOL_Y),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); if (strategy.get_banheira_flag()) diff --git a/cc/strategyGUI.hpp b/cc/strategyGUI.hpp index 9460d41a..82401e56 100644 --- a/cc/strategyGUI.hpp +++ b/cc/strategyGUI.hpp @@ -65,8 +65,7 @@ class StrategyGUI: public Gtk::VBox Gtk::CheckButton banheira_bt; Gtk::CheckButton areasDivision_bt; Gtk::CheckButton banheiraOffset_bt; - Gtk::CheckButton goalCenterX_bt; - Gtk::CheckButton goalCenterY_bt; + Gtk::CheckButton goalCenter_bt; Gtk::CheckButton goalMin_bt; Gtk::CheckButton goalMax_bt; Gtk::CheckButton defenseLine_bt; @@ -85,8 +84,7 @@ class StrategyGUI: public Gtk::VBox bool defenseLine_flag = false; bool goalMax_flag = false; bool goalMin_flag = false; - bool goalCenterY_flag = false; - bool goalCenterX_flag = false; + bool goalCenter_flag = false; bool banheiraOffset_flag = false; bool areasDivision_flag = false; bool banheira_flag = false; @@ -145,14 +143,9 @@ class StrategyGUI: public Gtk::VBox return goalMin_flag; } - bool get_goalCenterY_flag() + bool get_goalCenter_flag() { - return goalCenterY_flag; - } - - bool get_goalCenterX_flag() - { - return goalCenterX_flag; + return goalCenter_flag; } bool get_banheiraOffset_flag() @@ -204,8 +197,7 @@ class StrategyGUI: public Gtk::VBox banheira_bt.set_label("Banheira"); areasDivision_bt.set_label("Areas Division"); banheiraOffset_bt.set_label("Banheira Offset"); - goalCenterX_bt.set_label("Goal Center X"); - goalCenterY_bt.set_label("Goal Center Y"); + goalCenter_bt.set_label("Goal Center"); goalMin_bt.set_label("Goal Min."); goalMax_bt.set_label("Goal Max."); defenseLine_bt.set_label("Defense Line"); @@ -216,28 +208,27 @@ class StrategyGUI: public Gtk::VBox constants_grid.set_border_width(10); constants_grid.set_column_spacing(5); - constants_grid.add(fieldLength_bt); - constants_grid.add(totalFieldLength_bt); - constants_grid.add(fieldWwidth_bt); + constants_grid.attach(fieldLength_bt, 0, 0, 1 , 1); + constants_grid.attach(totalFieldLength_bt, 1, 0, 1, 1); + constants_grid.attach(fieldWwidth_bt, 2, 0, 1, 1); - constants_grid.attach_next_to(goalSize_bt, fieldLength_bt, Gtk::POS_BOTTOM, 1, 1); - constants_grid.attach_next_to(areaSize_bt, totalFieldLength_bt, Gtk::POS_BOTTOM, 1, 1); - constants_grid.attach_next_to(areaLimitX_bt, fieldWwidth_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach(goalSize_bt, 0, 1, 1, 1); + constants_grid.attach(areaSize_bt, 1, 1, 1, 1); + constants_grid.attach(areaLimitX_bt, 2, 1, 1, 1); - constants_grid.attach_next_to(banheira_bt, goalSize_bt, Gtk::POS_BOTTOM, 1, 1); - constants_grid.attach_next_to(areasDivision_bt, areaSize_bt, Gtk::POS_BOTTOM, 1, 1); - constants_grid.attach_next_to(banheiraOffset_bt, areaLimitX_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach(banheira_bt, 0, 2, 1, 1); + constants_grid.attach(areasDivision_bt, 1, 2, 1, 1); + constants_grid.attach(banheiraOffset_bt, 2, 2, 1, 1); - constants_grid.attach_next_to(goalCenterX_bt, banheira_bt, Gtk::POS_BOTTOM, 1, 1); - constants_grid.attach_next_to(goalCenterY_bt, areasDivision_bt, Gtk::POS_BOTTOM, 1, 1); - constants_grid.attach_next_to(goalMin_bt, banheiraOffset_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach(goalCenter_bt, 0, 3, 1, 1); + constants_grid.attach(goalMin_bt, 1, 3, 1, 1); + constants_grid.attach(goalMax_bt, 2, 3, 1, 1); - constants_grid.attach_next_to(goalMax_bt, goalCenterX_bt, Gtk::POS_BOTTOM, 1, 1); - constants_grid.attach_next_to(defenseLine_bt, goalCenterY_bt, Gtk::POS_BOTTOM, 1, 1); - constants_grid.attach_next_to(offsetRatio_bt, goalMin_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach(defenseLine_bt, 0, 4, 1, 1); + constants_grid.attach(offsetRatio_bt, 1, 4, 1, 1); + constants_grid.attach(coneRatio_bt, 2, 4, 1, 1); - constants_grid.attach_next_to(coneRatio_bt, goalMax_bt, Gtk::POS_BOTTOM, 1, 1); - constants_grid.attach_next_to(deslocamentoZagaAtaque_bt, defenseLine_bt, Gtk::POS_BOTTOM, 1, 1); + constants_grid.attach(deslocamentoZagaAtaque_bt, 0, 5, 2, 1); fieldLength_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_fieldLength_bt_clicked)); totalFieldLength_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_totalFieldLength_bt_clicked)); @@ -248,8 +239,7 @@ class StrategyGUI: public Gtk::VBox banheira_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_banheira_bt_clicked)); areasDivision_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_areasDivision_bt_clicked)); banheiraOffset_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_banheiraOffset_bt_clicked)); - goalCenterX_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalCenterX_bt_clicked)); - goalCenterY_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalCenterY_bt_clicked)); + goalCenter_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalCenter_bt_clicked)); goalMin_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalMin_bt_clicked)); goalMax_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalMax_bt_clicked)); defenseLine_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_defenseLine_bt_clicked)); @@ -304,14 +294,9 @@ class StrategyGUI: public Gtk::VBox banheiraOffset_flag = !banheiraOffset_flag; } - void _event_goalCenterX_bt_clicked() - { - goalCenterX_flag = !goalCenterX_flag; - } - - void _event_goalCenterY_bt_clicked() + void _event_goalCenter_bt_clicked() { - goalCenterY_flag = !goalCenterY_flag; + goalCenter_flag = !goalCenter_flag; } void _event_goalMin_bt_clicked() From 18dd873c4ea348d9079595fc17f2808b16803557 Mon Sep 17 00:00:00 2001 From: pqvs Date: Fri, 17 Feb 2017 19:47:43 -0200 Subject: [PATCH 06/20] =?UTF-8?q?Vis=C3=A3o=20agora=20est=C3=A1=20em=20vis?= =?UTF-8?q?ion.hpp.=20Unitag=20melhorado=20(talvez=20arrumado,=20precisa?= =?UTF-8?q?=20de=20mais=20testes)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CAM_quicksave.txt | 22 +- cc/camcap.hpp | 824 ++---------------- cc/vision.cpp | 0 cc/vision.hpp | 470 ++++++++++ pack-capture-gui/capture-gui/ImageView2.hpp | 187 ---- .../capture-gui/V4LInterface-aux.cpp | 26 +- pack-capture-gui/capture-gui/V4LInterface.hpp | 4 + 7 files changed, 591 insertions(+), 942 deletions(-) delete mode 100644 cc/vision.cpp delete mode 100644 pack-capture-gui/capture-gui/ImageView2.hpp diff --git a/CAM_quicksave.txt b/CAM_quicksave.txt index 56298aa5..7431ea99 100644 --- a/CAM_quicksave.txt +++ b/CAM_quicksave.txt @@ -1,15 +1,13 @@ 9963776 -71 +82 9963777 48 9963778 38 -9963779 -50 9963788 0 -9963792 -50 +9963795 +22 9963800 2 9963802 @@ -18,5 +16,19 @@ 100 9963804 0 +10094849 +1 +10094850 +377 10094851 +0 +10094856 +0 +10094857 +0 +10094858 +85 +10094860 +0 +10094861 1 diff --git a/cc/camcap.hpp b/cc/camcap.hpp index 9f3fd303..17a52a7a 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -18,6 +18,7 @@ #include "strategyGUI.hpp" #include "controlGUI.hpp" +#include "vision.hpp" #include #include #include @@ -30,9 +31,7 @@ #include #include "CPUTimer.cpp" -boost::mutex io_mutex; -boost::thread_group threshold_threads; -struct ImageGray; +Vision *vision; class CamCap: @@ -46,25 +45,13 @@ class CamCap: StrategyGUI strategy; ControlGUI control; capture::V4LInterface v; + unsigned char * d; - unsigned char **threshold = NULL; + Gtk::Notebook notebook; int w, h; CPUTimer timer; - // Team_Main[INDEX] - Vector de cv::Point - // GUARDA A POSIÇÃO DAS TAGS PRIMÁRIAS DO TIME(.x e .y acessam a posição) - vector< cv::Point > Team_Main; - // Team_Sec[COLOR_INDEX][INDEX] - Vector de Vector de cv::Point - // CADA POSIÇÃO GUARDA UM VECTOR DE cv::Point PARA CADA COR SECUNDÁRIA DO TIME - vector> Team_Sec; - vector> Team_Sec_area; - // ADV_Main[INDEX] - Vector de cv::Point - // GUARDA A POSIÇÃO DAS TAGS PRIMÁRIAS DO ADVERSÁRIO(.x e .y acessam a posição) - vector< cv::Point > Adv_Main; - // Ball - cv::Point - // GUARDA A POSIÇÃO DA BOLA - cv::Point Ball; @@ -76,105 +63,32 @@ class CamCap: int width, height; int Selec_index=-1; bool fixed_ball[3]; - double threshouldAuto[3][2]; - - // VARIÁVEIS PARA A FRAME INFO - Gtk::Label *label; - - - - - - std::string function[3]; - - // KALMAN FILTER - int stateSize = 6; - int measSize = 4; - int contrSize = 0; - - unsigned int type = CV_32F; - - cv::KalmanFilter kfBall; - - cv::Mat state;// [x,y,v_x,v_y,w,h] - cv::Mat meas; // [z_x,z_y,z_w,z_h] - - double ticks = 0; - bool found = false; - int notFoundCount = 0; - - - // Função para retornar a posição de um robo - cv::Point getRobotPosition(int robot_list_index) { - return v.robot_list[robot_list_index].position; - } - - double meanCalc(vector v) { - int sum = std::accumulate(v.begin(), v.end(), 0.0); - double mean = sum / v.size(); - //cout<<"Media = "< v) { - int sum = std::accumulate(v.begin(), v.end(), 0.0); - double mean = sum / v.size(); - std::vector diff(v.size()); - std::transform(v.begin(), v.end(), diff.begin(), - std::bind2nd(std::minus(), mean)); - double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); - double stdev = std::sqrt(sq_sum / v.size()); - - return stdev; - } - - void autoThreshold(cv::Mat img, cv::Point p) { - - vector H, S, V; - - cv::Mat imagem = img.clone(); - - cv::cvtColor(imagem, imagem, cv::COLOR_RGB2HSV); - int alpha_slider = 6; - int horizontalOffset = alpha_slider, verticalOffset = alpha_slider, Hue, Sat, Val; - - //int threshould[3][2]; - - for ( int i = (p.x-horizontalOffset); i < (p.x+horizontalOffset); i++ ) { - for ( int j = (p.y-verticalOffset); j < (p.y+verticalOffset); j++ ) { - - //pixel = image.at(j,i); - //cout<<(int)pixel.val[0]<<"|"<<(int)pixel.val[1]<<"|"<<(int)pixel.val[2]<(j,i)[0]; - Sat = (int)imagem.at(j,i)[1]; - Val = (int)imagem.at(j,i)[2]; - //cout<< Hue <<"|"<< Sat <<"|"<< Val <robot_creation_unitag(); + //vision->robot_creation(); + // cout<<"AQUI"<getRobotListSize(); i++) + { + v.robot_list[i].position = vision->getRobotFromList(i).position; + v.robot_list[i].orientation = vision->getRobotFromList(i).orientation; + v.robot_list[i].secundary = vision->getRobotFromList(i).secundary; + } + // cout<<"AQUI"<getBallPosition().x; + v.ballY = vision->getBallPosition().y; + + v.robot_list[0].feedHist(v.robot_list[0].position); + v.robot_list[1].feedHist(v.robot_list[1].position); + v.robot_list[2].feedHist(v.robot_list[2].position); + + v.updateRobotLabels(); } bool start_signal(bool b) { + if (b) { cout << "Start Clicked!" << endl; @@ -184,8 +98,7 @@ class CamCap: free(data); data = 0; } - Ball.x=0; - Ball.y=0; + /*GdkScreen* screen = gdk_screen_get_default(); if (v.vcap.format_dest.fmt.pix.width > gdk_screen_get_width(screen)/2 || v.vcap.format_dest.fmt.pix.height > gdk_screen_get_height(screen)/2) @@ -209,15 +122,8 @@ class CamCap: v.robots_save_bt.set_state(Gtk::STATE_NORMAL); v.robots_load_bt.set_state(Gtk::STATE_NORMAL); - threshold = (unsigned char**) malloc(6 * sizeof(unsigned char *)); - for(int i = 0; i < 6; i++) - { - threshold[i] = (unsigned char*) malloc((3*(width*height + width) +3) * sizeof(unsigned char)); - if(threshold[i] == NULL) - { - cout<<"out of memory\n"<(0) = 1.0f; - kfBall.measurementMatrix.at(7) = 1.0f; - kfBall.measurementMatrix.at(16) = 1.0f; - kfBall.measurementMatrix.at(23) = 1.0f; - - /* // Process Noise Covariance Matrix Q - // [ Ex 0 0 0 0 0 ] - // [ 0 Ey 0 0 0 0 ] - // [ 0 0 Ev_x 0 0 0 ] - // [ 0 0 0 1 Ev_y 0 ] - // [ 0 0 0 0 1 Ew ] - // [ 0 0 0 0 0 Eh ] - //cv::setIdentity(kfBall.processNoiseCov, cv::Scalar(1e-2)); - * */ - - kfBall.processNoiseCov.at(0) = 1e-2; - kfBall.processNoiseCov.at(7) = 1e-2; - kfBall.processNoiseCov.at(14) = 2.0f; - kfBall.processNoiseCov.at(21) = 1.0f; - kfBall.processNoiseCov.at(28) = 1e-2; - kfBall.processNoiseCov.at(35) = 1e-2; - - // Measures Noise Covariance Matrix R - cv::setIdentity(kfBall.measurementNoiseCov, cv::Scalar(1e-1)); - return true; } @@ -310,6 +162,8 @@ class CamCap: w = v.iv.get_width(); h = v.iv.get_height(); + + cv::Mat image(h,w,CV_8UC3,d); if(v.iv.hold_warp) { @@ -320,8 +174,6 @@ class CamCap: v.iv.hold_warp=false; } - v.iv.auto_calib_flag = v.auto_calib_flag; // resolver - v.iv.PID_test_flag = control.PID_test_flag; v.iv.adjust_event_flag = v.adjust_event_flag; @@ -338,108 +190,19 @@ class CamCap: } - // RESOLVER - - if(v.auto_calib_flag) { - //cout<<"----------------------------------------"<setHSV(v.H,v.S,v.V,v.Amin); //TRACKING CAMERA - parallel_tracking(image); + + + vision->parallel_tracking(image); + + if(!v.HSV_calib_event_flag) { - robot_creation_unitag(); - //robot_creation(); + updateAllPositions(); drawStrategyConstants(image, w, h); @@ -457,23 +220,10 @@ class CamCap: line(image,v.robot_list[2].position,v.robot_list[2].secundary,cv::Scalar(255,255,0), 2); //line(image,v.robot_list[2].position,v.robot_list[2].ternary,cv::Scalar(100,255,0), 2); putText(image,"3",cv::Point(v.robot_list[2].position.x-5,v.robot_list[2].position.y-17),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0),2); - circle(image,Ball, 7, cv::Scalar(255,255,255), 2); - - //PREDIÇÃO DA BOLA - /* double precTick = ticks; - ticks = (double) cv::getTickCount(); - double dT = (ticks - precTick) / cv::getTickFrequency(); //seconds - kfBall.transitionMatrix.at(2) = dT; - kfBall.transitionMatrix.at(9) = dT; - state = kfBall.predict(); - cv::Point center; - center.x = state.at(0); - center.y = state.at(1); - circle(image,center, 5, cv::Scalar(0,0,255), 2); */ - // strategy.strats.set_Ball(Ball); - //strategy.strats.set_Ball_Est(center); - for(int i=0; igetBallPosition(), 7, cv::Scalar(255,255,255), 2); + + for(int i=0; iAdv_Main.size(); i++) + circle(image,vision->Adv_Main[i], 15, cv::Scalar(0,0,255), 2); } @@ -494,11 +244,11 @@ class CamCap: for(int i=0; igetBallPosition()); /* line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2),cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); @@ -510,13 +260,13 @@ class CamCap: */ if(v.start_game_flag) { Ball_Est=strategy.strats.get_Ball_Est(); - line(image,Ball,Ball_Est,cv::Scalar(255,140,0), 2); + line(image,vision->getBallPosition(),Ball_Est,cv::Scalar(255,140,0), 2); circle(image,Ball_Est, 7, cv::Scalar(255,140,0), 2); char buffer[3]; for(int i =0; i<3; i++) { switch (v.robot_list[i].role) { case 0: - v.robot_list[i].target = strategy.strats.get_gk_target(Adv_Main); + v.robot_list[i].target = strategy.strats.get_gk_target(vision->Adv_Main); v.robot_list[i].fixedPos = strategy.strats.Goalkeeper.fixedPos; if(strategy.strats.GOAL_DANGER_ZONE) { // cout<<"hist_wipe"<Adv_Main.size();j++){ + if ( sqrt(pow(vision->Adv_Main[j].x - v.robot_list[i].position.x, 2) + pow(vision->Adv_Main[j].y - v.robot_list[i].position.y, 2)) < 50) { v.robot_list[i].histWipe(); } }*/ @@ -537,8 +287,8 @@ class CamCap: v.robot_list[i].target = strategy.strats.get_def_target(v.robot_list[i].position); v.robot_list[i].fixedPos = strategy.strats.Defense.fixedPos; v.robot_list[i].status = strategy.strats.Defense.status; - /* for(int j=0;jAdv_Main.size();j++){ + if ( sqrt(pow(vision->Adv_Main[j].x - v.robot_list[i].position.x, 2) + pow(vision->Adv_Main[j].y - v.robot_list[i].position.y, 2)) < 50) { v.robot_list[i].spin = true; } }*/ @@ -579,9 +329,9 @@ class CamCap: cout<Adv_Main.size()<Adv_Main.size();i++){ + cout<Adv_Main[i].x<<" "<Adv_Main[i].y<<" "; } cout<threshold[v.Img_id][i]; } return true; } @@ -602,7 +352,7 @@ class CamCap: void send_vel_to_robots() { for(int i=0; igetBallPosition()); } else { v.robot_list[i].Vr = 0 ; v.robot_list[i].Vl = 0 ; @@ -627,12 +377,12 @@ class CamCap: } if(Selec_index>-1) { v.robot_list[Selec_index].histWipe(); - if(sqrt(pow((Ball.x-v.robot_list[Selec_index].target.x),2)+pow((Ball.y-v.robot_list[Selec_index].target.y),2))<=7) + if(sqrt(pow((vision->getBallPosition().x-v.robot_list[Selec_index].target.x),2)+pow((vision->getBallPosition().y-v.robot_list[Selec_index].target.y),2))<=7) fixed_ball[Selec_index]=true; if(fixed_ball[Selec_index]) - v.robot_list[Selec_index].target=Ball; + v.robot_list[Selec_index].target=vision->getBallPosition(); else v.robot_list[Selec_index].target = cv::Point(v.iv.tar_pos[0],v.iv.tar_pos[1]); } @@ -640,7 +390,7 @@ class CamCap: for(int i=0; igetBallPosition(); else { if(sqrt(pow((v.robot_list[i].position.x-v.robot_list[i].target.x),2)+ pow((v.robot_list[i].position.y-v.robot_list[i].target.y),2))<15) { @@ -652,7 +402,7 @@ class CamCap: v.robot_list[i].Vl = 0 ; } if(v.robot_list[i].target.x!=-1&&v.robot_list[i].target.y!=-1) { - v.robot_list[i].goTo(v.robot_list[i].target,Ball); + v.robot_list[i].goTo(v.robot_list[i].target,vision->getBallPosition()); } else { v.robot_list[i].Vr = 0 ; v.robot_list[i].Vl = 0 ; @@ -667,8 +417,23 @@ class CamCap: void drawStrategyConstants(cv::Mat image, int w, int h) { + if (strategy.get_deslocamentoZagaAtaque_flag()) - line(image,cv::Point(strategy.strats.DESLOCAMENTO_ZAGA_ATAQUE, 0),cv::Point(strategy.strats.DESLOCAMENTO_ZAGA_ATAQUE, h),cv::Scalar(255,255,0), 2); + { + line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2),cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2),cv::Point(0,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2),cv::Point(0,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + + line(image, cv::Point(strategy.strats.Ball.x, strategy.strats.Ball.y - 100),cv::Point(strategy.strats.Ball.x, strategy.strats.Ball.y + 100), cv::Scalar(255,0,0),2); + line(image, cv::Point(strategy.strats.Ball),cv::Point(strategy.strats.Ball.x - 100, strategy.strats.Ball.y), cv::Scalar(255,0,0),2); + + line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,0),cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,strategy.strats.MIN_GOL_Y), cv::Scalar(255,255,255),2); + line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,height),cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,strategy.strats.MAX_GOL_Y), cv::Scalar(255,255,255),2); + line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,strategy.strats.MAX_GOL_Y),cv::Point(width,strategy.strats.MAX_GOL_Y), cv::Scalar(255,255,255),2); + line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,strategy.strats.MIN_GOL_Y),cv::Point(width,strategy.strats.MIN_GOL_Y), cv::Scalar(255,255,255),2); + + line(image,cv::Point(strategy.strats.DESLOCAMENTO_ZAGA_ATAQUE, 0),cv::Point(strategy.strats.DESLOCAMENTO_ZAGA_ATAQUE, h),cv::Scalar(255,255,0), 2); + } if (strategy.get_goalSize_flag()) line(image,cv::Point(strategy.strats.TAMANHO_GOL, h/2-strategy.strats.TAMANHO_GOL/2),cv::Point(strategy.strats.TAMANHO_GOL, h/2+strategy.strats.TAMANHO_GOL/2),cv::Scalar(255,255,0), 2); @@ -716,426 +481,10 @@ class CamCap: line(image,cv::Point(strategy.strats.LIMITE_AREA_X, 0),cv::Point(strategy.strats.LIMITE_AREA_X, h),cv::Scalar(255,255,0), 2); } - void robot_creation_unitag() { - - vector robot; - Robot r; - double omax = -99999; - double omin = 99999; - //cout<<"1"<Team_Sec_area[index2[0]][index2[1]]) { - robot[l].secundary = Team_Sec[index1[0]][index1[1]]; - robot[l].ternary = Team_Sec[index2[0]][index2[1]]; - - //cout<<"Area 1 = Secundary"<<" Area 2 = Ternary"<o) { - omin = o; - v.robot_list[1].position = robot[i].position; // colocar em um vetor - - v.robot_list[1].secundary = robot[i].secundary; // colocar em um vetor - v.robot_list[1].orientation = robot[i].orientation; - - - min=i; - } - } - for(int i=0; iset_text(aux1.str()); - - stringstream aux2; - aux2 << "(" << round((v.robot_list[1].position.x))<< "," << round((v.robot_list[1].position.y))<< "," << round((v.robot_list[1].orientation*(180/PI))) << ")"; - v.robot2_pos_lb->set_text(aux2.str()); - - stringstream aux3; - aux3 << "(" << round((v.robot_list[2].position.x))<< "," << round((v.robot_list[2].position.y)) << "," << round((v.robot_list[2].orientation*(180/PI))) << ")"; - - v.robot3_pos_lb->set_text(aux3.str()); - stringstream aux4; - aux4 << "(" << round((Ball.x))<< "," << round((Ball.y)) << ")"; - v.ball_pos_lb->set_text(aux4.str()); - } - - void robot_creation() { - Robot robot; - cv::Point secundary; - int index[2]; - - - float distanceRef = 999999999.0; - float distance = 0; - for(int j = 0; j < Team_Main.size()&&j<=3; j++) { - for(int i = 0; i < 3; i++) { - for(int k = 0; k < Team_Sec[i].size(); k++) { - - distance = calcDistance(Team_Main[j],Team_Sec[i][k]); - if(distance < distanceRef) { - distanceRef = distance; - index[0] = i; - index[1] = k; - } - } - } - - robot.position = Team_Main[j]; - robot.secundary = Team_Sec[index[0]][index[1]]; - distanceRef = 999999999.0; - v.robot_list[index[0]].position = robot.position; // colocar em um vetor - v.robot_list[index[0]].feedHist(robot.position); - v.robot_list[index[0]].secundary = robot.secundary; // colocar em um vetor - calcOrientation(index[0]); - - - } - // Atualizar as labels de posição dos robos - - stringstream aux1; - aux1 << "(" << round((v.robot_list[0].position.x))<< "," << round((v.robot_list[0].position.y))<< "," << round(v.robot_list[0].orientation*(180/PI)) << ")"; - v.robot1_pos_lb->set_text(aux1.str()); - - stringstream aux2; - aux2 << "(" << round((v.robot_list[1].position.x))<< "," << round((v.robot_list[1].position.y))<< "," << round((v.robot_list[1].orientation*(180/PI))) << ")"; - v.robot2_pos_lb->set_text(aux2.str()); - - stringstream aux3; - aux3 << "(" << round((v.robot_list[2].position.x))<< "," << round((v.robot_list[2].position.y)) << "," << round((v.robot_list[2].orientation*(180/PI))) << ")"; - - v.robot3_pos_lb->set_text(aux3.str()); - stringstream aux4; - aux4 << "(" << round((Ball.x))<< "," << round((Ball.y)) << ")"; - v.ball_pos_lb->set_text(aux4.str()); - } - - void calcOrientation(int tag_id) { //Define a orientação da tag em analise; - float sx,sy,px,py; - - sx = v.robot_list[tag_id].secundary.x; - sy = v.robot_list[tag_id].secundary.y; - - px = v.robot_list[tag_id].position.x; - py = v.robot_list[tag_id].position.y; - - v.robot_list[tag_id].orientation = atan2((sy-py)*1.3/480,(sx-px)*1.5/640); - v.robot_list[tag_id].position.x = v.robot_list[tag_id].position.x; - v.robot_list[tag_id].position.y = v.robot_list[tag_id].position.y; - } - - float calcDistance(cv::Point position, cv::Point secundary) { - return sqrt(pow(position.x-secundary.x,2) + pow(position.y-secundary.y,2)); - } - - void parallel_tracking(cv::Mat im) { - cv::Mat image_copy = im.clone(); - cv::cvtColor(image_copy,image_copy,cv::COLOR_RGB2HSV); - cv::medianBlur(image_copy, image_copy, 5); - Team_Sec_area.clear(); - vector< double > a; - Team_Sec_area.push_back(a); - Team_Sec_area.push_back(a); - Team_Sec_area.push_back(a); - - for(int i =0; i<6; i++) { - - threshold_threads.add_thread(new boost::thread(&CamCap::img_tracking,this, boost::ref(image_copy), (i))); - } - - threshold_threads.join_all(); - image_copy.release(); - } - - void img_tracking(cv::Mat image,int color_id) { - int ec,e3c,H,S,V; - vector< vector > contours; - vector hierarchy; - - - for(int i =0; i=v.H[color_id][0]&&H<=v.H[color_id][1])&& - (S>=v.S[color_id][0]&&S<=v.S[color_id][1])&& - (V>=v.V[color_id][0]&&V<=v.V[color_id][1])) { - - threshold[color_id][e3c] = 255; - threshold[color_id][e3c+1] = 255; - threshold[color_id][e3c+2] = 255; - } else { - threshold[color_id][e3c] = 0; - threshold[color_id][e3c+1] = 0; - threshold[color_id][e3c+2] = 0; - } - } - } - cv::Mat temp(height,width,CV_8UC3,threshold[color_id]); - cv::cvtColor(temp,temp,cv::COLOR_RGB2GRAY); - cv::findContours(temp,contours,hierarchy,cv::RETR_CCOMP,cv::CHAIN_APPROX_SIMPLE); - - - switch(color_id) { - - case 0:// TEAM MAIN COLOR - - if (hierarchy.size() > 0) { - Team_Main.clear(); - int index = 0; - while(index >= 0) { - cv::Moments moment = moments((cv::Mat)contours[index]); - double area = contourArea(contours[index]); - //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. - if(area >= v.Amin[color_id]/100) { - Team_Main.push_back(cv::Point(moment.m10/area,moment.m01/area)); - } - index = hierarchy[index][0]; - } - - } - - break; - - - case 1:// TEAM FIRST SECUNDARY COLOR - if (hierarchy.size() > 0) { - Team_Sec[0].clear(); - int index = 0; - while(index >= 0) { - cv::Moments moment = moments((cv::Mat)contours[index]); - double area = contourArea(contours[index]); - //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. - if(area >= v.Amin[color_id]/100) { - Team_Sec[0].push_back(cv::Point(moment.m10/area,moment.m01/area)); - Team_Sec_area[0].push_back(area); - } - index = hierarchy[index][0]; - } - } - - - break; - - case 2:// TEAM SECOND SECUNDARY COLOR - if (hierarchy.size() > 0) { - Team_Sec[1].clear(); - int index = 0; - while(index >= 0) { - cv::Moments moment = moments((cv::Mat)contours[index]); - double area = contourArea(contours[index]); - //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. - if(area >= v.Amin[color_id]/100) { - Team_Sec[1].push_back(cv::Point(moment.m10/area,moment.m01/area)); - Team_Sec_area[1].push_back(area); - - } - index = hierarchy[index][0]; - } - } - break; - case 3:// TEAM THIRD SECUNDARY COLOR - if (hierarchy.size() > 0) { - Team_Sec[2].clear(); - int index = 0; - while(index >= 0) { - cv::Moments moment = moments((cv::Mat)contours[index]); - double area = contourArea(contours[index]); - //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. - if(area >= v.Amin[color_id]/100) { - Team_Sec[2].push_back(cv::Point(moment.m10/area,moment.m01/area)); - Team_Sec_area[2].push_back(area); - } - index = hierarchy[index][0]; - } - } - break; - - case 5:// ADVERSARY MAIN COLOR - - if (hierarchy.size() > 0) { - Adv_Main.clear(); - int j =0; - int index = 0; - while(index >= 0) { - cv::Moments moment = moments((cv::Mat)contours[index]); - double area = contourArea(contours[index]); - //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. - if(area >= v.Amin[color_id]/100) { - Adv_Main.push_back(cv::Point(moment.m10/area,moment.m01/area)); - } - index = hierarchy[index][0]; - } - - } - - break; - - case 4:// BALL - if (hierarchy.size() > 0) { - cv::Moments moment = moments((cv::Mat)contours[0]); - double area = contourArea(contours[0]); - //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. - - - if(area >= v.Amin[color_id]/100) { - Ball = cv::Point(moment.m10/area,moment.m01/area); - - /* kfBall.statePost = state; - - notFoundCount = 0; - - meas.at(0) = Ball.x; - meas.at(1) = Ball.y; - meas.at(2) = (float) 5; - meas.at(3) = (float) 5; - - if (!found) // First detection! - { - // >>>> Initialization - kfBall.errorCovPre.at(0) = 1; // px - kfBall.errorCovPre.at(7) = 1; // px - kfBall.errorCovPre.at(14) = 1; - kfBall.errorCovPre.at(21) = 1; - kfBall.errorCovPre.at(28) = 1; // px - kfBall.errorCovPre.at(35) = 1; // px - - state.at(0) = meas.at(0); - state.at(1) = meas.at(1); - state.at(2) = 0; - state.at(3) = 0; - state.at(4) = meas.at(2); - state.at(5) = meas.at(3); - // <<<< Initialization - - found = true; - } - else - kfBall.correct(meas); // Kalman Correction - - //}else{ - //notFoundCount++; - //cout << "notFoundCount:" << notFoundCount << endl; - //if( notFoundCount >= 10 ){ - // found = false; - //} - */} - } - break; - - } - } - void warp_transform(cv::Mat image){ cv::Point2f inputQuad[4]; cv::Point2f outputQuad[4]; @@ -1195,22 +544,7 @@ class CamCap: notebook.append_page(control, "Control"); notebook.append_page(strategy, "Strategy"); - vector< double > a; - Team_Sec_area.push_back(a); - Team_Sec_area.push_back(a); - Team_Sec_area.push_back(a); - - vector< cv::Point > p; - - p.push_back(cv::Point(0,0)); - Team_Sec.push_back(p); - Team_Sec.push_back(p); - Team_Sec.push_back(p); - - Team_Main.push_back(cv::Point(0,0)); - Team_Main.push_back(cv::Point(0,0)); - Team_Main.push_back(cv::Point(0,0)); for(int i=0; i<4; i++) { v.iv.adjust_mat[i][0] = -1; v.iv.adjust_mat[i][1] = -1; @@ -1234,12 +568,6 @@ class CamCap: con.disconnect(); v.iv.disable_image_show(); free(data); - if (threshold != NULL) - { - for(int i = 0; i < 6; i++) - free(threshold[i]); - free(threshold); - } data = 0; } diff --git a/cc/vision.cpp b/cc/vision.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/cc/vision.hpp b/cc/vision.hpp index bbd553aa..5ad0bb35 100644 --- a/cc/vision.hpp +++ b/cc/vision.hpp @@ -1 +1,471 @@ +#ifndef VISION_HPP_ +#define VISION_HPP_ + #include "opencv2/opencv.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include "../pack-capture-gui/capture-gui/Robot.hpp" +#include // std::cout + +boost::thread_group threshold_threads; + +class Vision +{ +public: + // Team_Main[INDEX] - Vector de cv::Point + // GUARDA A POSIÇÃO DAS TAGS PRIMÁRIAS DO TIME(.x e .y acessam a posição) + vector< cv::Point > Team_Main; + // Team_Sec[COLOR_INDEX][INDEX] - Vector de Vector de cv::Point + // CADA POSIÇÃO GUARDA UM VECTOR DE cv::Point PARA CADA COR SECUNDÁRIA DO TIME + vector> Team_Sec; + vector> Team_Sec_area; + + // ADV_Main[INDEX] - Vector de cv::Point + // GUARDA A POSIÇÃO DAS TAGS PRIMÁRIAS DO ADVERSÁRIO(.x e .y acessam a posição) + vector< cv::Point > Adv_Main; + // Ball - cv::Point + // GUARDA A POSIÇÃO DA BOLA + cv::Point Ball; + + std::vector robot_list; + + int hue[6][2]; + int saturation[6][2]; + int value[6][2]; + int areaMin[6]; + + int width; + int height; + + unsigned char **threshold = NULL; + + int getRobotListSize() + { + return robot_list.size(); + } + + Robot getRobotFromList(int index) + { + Robot r; + if (index < robot_list.size() && index >= 0) + return robot_list[index]; + else + { + std::cout << "COULD NOT GET ROBOT "< > contours; + vector hierarchy; + + + for(int i =0; i=hue[color_id][0]&&H<=hue[color_id][1])&& + (S>=saturation[color_id][0]&&S<=saturation[color_id][1])&& + (V>=value[color_id][0]&&V<=value[color_id][1])) { + + threshold[color_id][e3c] = 255; + threshold[color_id][e3c+1] = 255; + threshold[color_id][e3c+2] = 255; + } else { + threshold[color_id][e3c] = 0; + threshold[color_id][e3c+1] = 0; + threshold[color_id][e3c+2] = 0; + } + } + } + cv::Mat temp(height,width,CV_8UC3,threshold[color_id]); + cv::cvtColor(temp,temp,cv::COLOR_RGB2GRAY); + cv::findContours(temp,contours,hierarchy,cv::RETR_CCOMP,cv::CHAIN_APPROX_SIMPLE); + + + switch(color_id) { + + case 0:// TEAM MAIN COLOR + + if (hierarchy.size() > 0) { + Team_Main.clear(); + int index = 0; + while(index >= 0) { + cv::Moments moment = moments((cv::Mat)contours[index]); + double area = contourArea(contours[index]); + //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. + if(area >= areaMin[color_id]/100) { + Team_Main.push_back(cv::Point(moment.m10/area,moment.m01/area)); + } + index = hierarchy[index][0]; + } + + } + + break; + + + case 1:// TEAM FIRST SECUNDARY COLOR + if (hierarchy.size() > 0) { + Team_Sec_area[0].clear(); + Team_Sec[0].clear(); + int index = 0; + while(index >= 0) { + cv::Moments moment = moments((cv::Mat)contours[index]); + double area = contourArea(contours[index]); + //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. + if(area >= areaMin[color_id]/100) { + Team_Sec[0].push_back(cv::Point(moment.m10/area,moment.m01/area)); + Team_Sec_area[0].push_back(area); + } + index = hierarchy[index][0]; + } + } + + + break; + + case 2:// TEAM SECOND SECUNDARY COLOR + if (hierarchy.size() > 0) { + Team_Sec_area[1].clear(); + Team_Sec[1].clear(); + int index = 0; + while(index >= 0) { + cv::Moments moment = moments((cv::Mat)contours[index]); + double area = contourArea(contours[index]); + //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. + if(area >= areaMin[color_id]/100) { + Team_Sec[1].push_back(cv::Point(moment.m10/area,moment.m01/area)); + Team_Sec_area[1].push_back(area); + + } + index = hierarchy[index][0]; + } + } + break; + case 3:// TEAM THIRD SECUNDARY COLOR + if (hierarchy.size() > 0) { + Team_Sec_area[2].clear(); + Team_Sec[2].clear(); + int index = 0; + while(index >= 0) { + cv::Moments moment = moments((cv::Mat)contours[index]); + double area = contourArea(contours[index]); + //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. + if(area >= areaMin[color_id]/100) { + Team_Sec[2].push_back(cv::Point(moment.m10/area,moment.m01/area)); + Team_Sec_area[2].push_back(area); + } + index = hierarchy[index][0]; + } + } + break; + + case 5:// ADVERSARY MAIN COLOR + + if (hierarchy.size() > 0) { + Adv_Main.clear(); + int j =0; + int index = 0; + while(index >= 0) { + cv::Moments moment = moments((cv::Mat)contours[index]); + double area = contourArea(contours[index]); + //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. + if(area >= areaMin[color_id]/100) { + Adv_Main.push_back(cv::Point(moment.m10/area,moment.m01/area)); + } + index = hierarchy[index][0]; + } + + } + + break; + + case 4:// BALL + if (hierarchy.size() > 0) { + cv::Moments moment = moments((cv::Mat)contours[0]); + double area = contourArea(contours[0]); + //Se a área do objeto for muito pequena então provavelmente deve ser apenas ruído. + + + if(area >= areaMin[color_id]/100) { + Ball = cv::Point(moment.m10/area,moment.m01/area); + + + } + } + break; + + } + } + + void robot_creation_unitag() { + vector robot; + Robot r; + double omax = -99999; + double omin = 99999; + // cout<<"1"<Team_Sec_area[index2[0]][index2[1]]) { + robot[l].secundary = Team_Sec[index1[0]][index1[1]]; + robot[l].ternary = Team_Sec[index2[0]][index2[1]]; + + // cout<<"Area 1 = Secundary"<<" Area 2 = Ternary"<=30*PI/180) { + + robot_list[0].position = robot[i].position; // colocar em um vetor + + robot_list[0].secundary = robot[i].secundary; // colocar em um vetor + robot_list[0].orientation = robot[i].orientation; + + + } + if(o<=-30*PI/180) { + + robot_list[1].position = robot[i].position; // colocar em um vetor + robot_list[1].secundary = robot[i].secundary; // colocar em um vetor + robot_list[1].orientation = robot[i].orientation; + + + + } + if((o<=15*PI/180)&&(o>=-15*PI/180)) { + robot_list[2].position = robot[i].position; // colocar em um vetor + + robot_list[2].secundary = robot[i].secundary; // colocar em um vetor + robot_list[2].orientation = robot[i].orientation; + + + } + } + + + } + + void robot_creation() { + Robot robot; + cv::Point secundary; + int index[2]; + + // cout<<"1"< a; + Team_Sec_area.push_back(a); + Team_Sec_area.push_back(a); + Team_Sec_area.push_back(a); + + width = w; + height = h; + + vector< cv::Point > p; + Robot r; + robot_list.push_back(r); + robot_list.push_back(r); + robot_list.push_back(r); + + p.push_back(cv::Point(0,0)); + Team_Sec.push_back(p); + Team_Sec.push_back(p); + Team_Sec.push_back(p); + + Team_Main.push_back(cv::Point(0,0)); + Team_Main.push_back(cv::Point(0,0)); + Team_Main.push_back(cv::Point(0,0)); + + + + threshold = (unsigned char**) malloc(6 * sizeof(unsigned char *)); + for(int i = 0; i < 6; i++) + { + threshold[i] = (unsigned char*) malloc((3*(width*height + width) +3) * sizeof(unsigned char)); + if(threshold[i] == NULL) + { + cout<<"out of memory\n"< -#include -#pragma once -namespace capture { - - class ImageView: public Gtk::DrawingArea { - - virtual bool on_button_press_event(GdkEventButton *event){ - if(warp_event_flag){ - //cerr <<"EVENT"<button == 1) - { - - //cerr << event->x <<" "<< event->y<< endl; - warp_mat[warp_counter][0] = event->x; - warp_mat[warp_counter][1] = event->y; - warp_counter++; - if(warp_counter==4){ - // cerr <<"COUNTER END"<button == 1) - { - adj_counter++; - //cerr << event->x <<" "<< event->y<< endl; - cerr << adj_counter << endl; - if(event->x < width/2){ - if(event->y < height/2){ - //cerr <<"ESQUERDA SUPERIOR"<x; - adjust_mat[0][1] = event->y; - }else{ - //cerr <<"ESQUERDA INFERIOR"<x; - adjust_mat[1][1] = event->y; - } - }else{ - if(event->y < height/2){ - //cerr <<"DIREITA SUPERIOR"<x; - adjust_mat[2][1] = event->y; - }else{ - //cerr <<"DIREITA INFERIOR"<x; - adjust_mat[3][1] = event->y; - } - } - - if(adj_counter==4){ - adjust_rdy = true; - adjust_event_flag=false; - //cerr << "ADJ END"<< endl; - } - - } - }else if(PID_test_flag){ - robot_pos[0]=0; - robot_pos[1]=0; - if (event->button == 1) - { - robot_pos[0] = event->x; - robot_pos[1] = event->y; - - //cerr <button == 3) - { - tar_pos[0] = event->x; - tar_pos[1] = event->y; - //cerr < pb; - - - public: - unsigned char * data; - int width, height, stride; - int warp_mat[4][2]; - int adjust_mat[4][2]; - double tar_pos[2]; - double robot_pos[2]; - int warp_counter =0; - int adj_counter =0; - bool warp_event_flag = false; - bool PID_test_flag =false; - bool adjust_event_flag = false; - bool hold_warp = false; - bool adjust_rdy = false; - ImageView() : - data(0), width(0), height(0), stride(0) { - robot_pos[1]=0; - robot_pos[0]=0; - tar_pos[1]=-1; - tar_pos[0]=-1; - } - - void set_data(unsigned char * data, int width, int height) { - this->data = data; - this->width = width; - this->height = height; - this->stride = width * 3; - } - unsigned char * get_data() { - return this->data; - } - int get_width() { - return this->width; - } - - int get_height() { - return this->height; - } - - - void disable_image_show() { - data = 0; - width = 0; - height = 0; - stride = 0; - } - - void refresh() { - this->queue_draw(); - } - - virtual bool on_draw(const Cairo::RefPtr& cr) { - add_events(Gdk::BUTTON_PRESS_MASK); - if (!data) return false; - - pb = Gdk::Pixbuf::create_from_data(data, Gdk::COLORSPACE_RGB, false, 8, width, height, stride); - Gdk::Cairo::set_source_pixbuf(cr, pb, 0, 0); - cr->paint(); - -// cr->set_line_width(5.0); -// cr->set_source_rgb(1, 0, 0); -// cr->move_to(0, 0); -// cr->line_to(width, height); -// cr->stroke(); -// -// // http://developer.gnome.org/pangomm/unstable/classPango_1_1FontDescription.html -// Pango::FontDescription font; -// font.set_family("Monospace"); -// font.set_weight(Pango::WEIGHT_BOLD); -// // http://developer.gnome.org/pangomm/unstable/classPango_1_1Layout.html -// Glib::RefPtr layout = create_pango_layout("Hi there!"); -// layout->set_font_description(font); -// int text_width; -// int text_height; -// //get the text dimensions (it updates the variables -- by reference) -// layout->get_pixel_size(text_width, text_height); -// // Position the text in the middle -// cr->move_to(10, 10); -// layout->show_in_cairo_context(cr); - - return true; - } - - }; - -} - -#endif /* IMAGEVIEW_HPP_ */ diff --git a/pack-capture-gui/capture-gui/V4LInterface-aux.cpp b/pack-capture-gui/capture-gui/V4LInterface-aux.cpp index b5ba3ca9..2be277ec 100644 --- a/pack-capture-gui/capture-gui/V4LInterface-aux.cpp +++ b/pack-capture-gui/capture-gui/V4LInterface-aux.cpp @@ -7,7 +7,10 @@ #include "V4LInterface.hpp" -#include + +#include // std::string +#include // std::cout +#include // std::stringstream #define DEFAULT_STR " - " @@ -866,6 +869,25 @@ namespace capture } + void V4LInterface::updateRobotLabels() + { + std::stringstream aux1; + aux1 << "(" << round((robot_list[0].position.x))<< "," << round((robot_list[0].position.y))<< "," << round(robot_list[0].orientation*(180/PI)) << ")"; + robot1_pos_lb->set_text(aux1.str()); + + std::stringstream aux2; + aux2 << "(" << round((robot_list[1].position.x))<< "," << round((robot_list[1].position.y))<< "," << round((robot_list[1].orientation*(180/PI))) << ")"; + robot2_pos_lb->set_text(aux2.str()); + + std::stringstream aux3; + aux3 << "(" << round((robot_list[2].position.x))<< "," << round((robot_list[2].position.y)) << "," << round((robot_list[2].orientation*(180/PI))) << ")"; + + robot3_pos_lb->set_text(aux3.str()); + std::stringstream aux4; + aux4 << "(" << round((ballX))<< "," << round((ballY)) << ")"; + ball_pos_lb->set_text(aux4.str()); + } + void V4LInterface::createIDsFrame(){ Gtk::Label *label; info_hbox.pack_start(robots_id_fm, false, true, 5); @@ -1245,7 +1267,7 @@ namespace capture for(int i=0; i robot_list; @@ -215,6 +217,8 @@ class V4LInterface: public Gtk::VBox { void event_robots_id_done_bt_signal_clicked(); void event_robots_id_edit_bt_signal_pressed(); + void updateRobotLabels(); + public: /* Signals */ From 3120eb5210d9cc8405258bf4a4c5eeb34177612c Mon Sep 17 00:00:00 2001 From: pqvs Date: Mon, 20 Feb 2017 15:13:49 -0300 Subject: [PATCH 07/20] =?UTF-8?q?Mudan=C3=A7a=20no=20nome=20de=20alguns=20?= =?UTF-8?q?objetos=20e=20classes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cc/camcap.hpp | 438 +++++++++--------- cc/strategyGUI.hpp | 2 +- .../capture-gui/V4LInterface-events.cpp | 80 ++-- pack-capture-gui/capture-gui/V4LInterface.hpp | 2 +- 4 files changed, 261 insertions(+), 261 deletions(-) diff --git a/cc/camcap.hpp b/cc/camcap.hpp index 17a52a7a..4c2dc70a 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -42,9 +42,9 @@ class CamCap: cv::Mat image_copy; cv::Point Ball_Est; - StrategyGUI strategy; + StrategyGUI strategyGUI; ControlGUI control; - capture::V4LInterface v; + capture::V4LInterface interface; unsigned char * d; @@ -72,19 +72,19 @@ class CamCap: // cout<<"AQUI"<getRobotListSize(); i++) { - v.robot_list[i].position = vision->getRobotFromList(i).position; - v.robot_list[i].orientation = vision->getRobotFromList(i).orientation; - v.robot_list[i].secundary = vision->getRobotFromList(i).secundary; + interface.robot_list[i].position = vision->getRobotFromList(i).position; + interface.robot_list[i].orientation = vision->getRobotFromList(i).orientation; + interface.robot_list[i].secundary = vision->getRobotFromList(i).secundary; } // cout<<"AQUI"<getBallPosition().x; - v.ballY = vision->getBallPosition().y; + interface.ballX = vision->getBallPosition().x; + interface.ballY = vision->getBallPosition().y; - v.robot_list[0].feedHist(v.robot_list[0].position); - v.robot_list[1].feedHist(v.robot_list[1].position); - v.robot_list[2].feedHist(v.robot_list[2].position); + interface.robot_list[0].feedHist(interface.robot_list[0].position); + interface.robot_list[1].feedHist(interface.robot_list[1].position); + interface.robot_list[2].feedHist(interface.robot_list[2].position); - v.updateRobotLabels(); + interface.updateRobotLabels(); } bool start_signal(bool b) { @@ -94,39 +94,39 @@ class CamCap: cout << "Start Clicked!" << endl; if (data) { - v.iv.disable_image_show(); + interface.imageView.disable_image_show(); free(data); data = 0; } /*GdkScreen* screen = gdk_screen_get_default(); - if (v.vcap.format_dest.fmt.pix.width > gdk_screen_get_width(screen)/2 || v.vcap.format_dest.fmt.pix.height > gdk_screen_get_height(screen)/2) + if (interface.vcap.format_dest.fmt.pix.width > gdk_screen_get_width(screen)/2 || interface.vcap.format_dest.fmt.pix.height > gdk_screen_get_height(screen)/2) { width = gdk_screen_get_width(screen)/2; height = gdk_screen_get_height(screen)/2; - strategy.strats.set_constants(width,height); + strategyGUI.strategy.set_constants(width,height); } else {*/ - width = v.vcap.format_dest.fmt.pix.width; - height = v.vcap.format_dest.fmt.pix.height; - strategy.strats.set_constants(width,height); + width = interface.vcap.format_dest.fmt.pix.width; + height = interface.vcap.format_dest.fmt.pix.height; + strategyGUI.strategy.set_constants(width,height); //} // Liberar os botões de edit - v.robots_id_edit_bt.set_state(Gtk::STATE_NORMAL); - v.robots_speed_edit_bt.set_state(Gtk::STATE_NORMAL); - v.robots_function_edit_bt.set_state(Gtk::STATE_NORMAL); - v.robots_save_bt.set_state(Gtk::STATE_NORMAL); - v.robots_load_bt.set_state(Gtk::STATE_NORMAL); + interface.robots_id_edit_bt.set_state(Gtk::STATE_NORMAL); + interface.robots_speed_edit_bt.set_state(Gtk::STATE_NORMAL); + interface.robots_function_edit_bt.set_state(Gtk::STATE_NORMAL); + interface.robots_save_bt.set_state(Gtk::STATE_NORMAL); + interface.robots_load_bt.set_state(Gtk::STATE_NORMAL); vision = new Vision(width, height); - data = (unsigned char *) calloc(v.vcap.format_dest.fmt.pix.sizeimage, sizeof(unsigned char)); + data = (unsigned char *) calloc(interface.vcap.format_dest.fmt.pix.sizeimage, sizeof(unsigned char)); - v.iv.set_size_request(width, height); + interface.imageView.set_size_request(width, height); con = Glib::signal_idle().connect(sigc::mem_fun(*this, &CamCap::capture_and_show)); cout << "Start Clicked! 1" << endl; @@ -135,15 +135,15 @@ class CamCap: con.disconnect(); // Travar os botões de edit - v.robots_id_edit_bt.set_state(Gtk::STATE_INSENSITIVE); - v.robots_speed_edit_bt.set_state(Gtk::STATE_INSENSITIVE); - v.robots_function_edit_bt.set_state(Gtk::STATE_INSENSITIVE); - v.robots_save_bt.set_state(Gtk::STATE_INSENSITIVE); - v.robots_load_bt.set_state(Gtk::STATE_INSENSITIVE); + interface.robots_id_edit_bt.set_state(Gtk::STATE_INSENSITIVE); + interface.robots_speed_edit_bt.set_state(Gtk::STATE_INSENSITIVE); + interface.robots_function_edit_bt.set_state(Gtk::STATE_INSENSITIVE); + interface.robots_save_bt.set_state(Gtk::STATE_INSENSITIVE); + interface.robots_load_bt.set_state(Gtk::STATE_INSENSITIVE); } - v.__event_bt_quick_load_clicked(); + interface.__event_bt_quick_load_clicked(); return true; } @@ -152,40 +152,40 @@ class CamCap: if (!data) return false; //timer.start(); - v.vcap.grab_rgb(data); - v.iv.set_data(data, width, height); + interface.vcap.grab_rgb(data); + interface.imageView.set_data(data, width, height); - v.iv.refresh(); - d = v.iv.get_data(); + interface.imageView.refresh(); + d = interface.imageView.get_data(); - w = v.iv.get_width(); - h = v.iv.get_height(); + w = interface.imageView.get_width(); + h = interface.imageView.get_height(); - cv::Mat image(h,w,CV_8UC3,d); + cv::Mat imageView(h,w,CV_8UC3,d); - if(v.iv.hold_warp) { - v.warped = true; - v.bt_adjust.set_state(Gtk::STATE_NORMAL); - v.iv.warp_event_flag = false; - v.iv.warp_event_flag = false; - v.iv.hold_warp=false; + if(interface.imageView.hold_warp) { + interface.warped = true; + interface.bt_adjust.set_state(Gtk::STATE_NORMAL); + interface.imageView.warp_event_flag = false; + interface.imageView.warp_event_flag = false; + interface.imageView.hold_warp=false; } - v.iv.PID_test_flag = control.PID_test_flag; - v.iv.adjust_event_flag = v.adjust_event_flag; + interface.imageView.PID_test_flag = control.PID_test_flag; + interface.imageView.adjust_event_flag = interface.adjust_event_flag; - if(v.warped) { - v.bt_warp.set_active(false); - v.bt_warp.set_state(Gtk::STATE_INSENSITIVE); - warp_transform(image); - v.iv.warp_event_flag=false; + if(interface.warped) { + interface.bt_warp.set_active(false); + interface.bt_warp.set_state(Gtk::STATE_INSENSITIVE); + warp_transform(imageView); + interface.imageView.warp_event_flag=false; - if(v.invert_image_flag) + if(interface.invert_image_flag) { - cv::flip(image,image, -1); + cv::flip(imageView,imageView, -1); } } @@ -193,122 +193,122 @@ class CamCap: - vision->setHSV(v.H,v.S,v.V,v.Amin); + vision->setHSV(interface.H,interface.S,interface.V,interface.Amin); //TRACKING CAMERA - vision->parallel_tracking(image); + vision->parallel_tracking(imageView); - if(!v.HSV_calib_event_flag) { + if(!interface.HSV_calib_event_flag) { updateAllPositions(); - drawStrategyConstants(image, w, h); + drawStrategyConstants(imageView, w, h); - if (!v.draw_info_flag) + if (!interface.draw_info_flag) { - circle(image,v.robot_list[0].position, 15, cv::Scalar(255,255,0), 2); - line(image,v.robot_list[0].position,v.robot_list[0].secundary,cv::Scalar(255,255,0), 2); - //line(image,v.robot_list[0].position,v.robot_list[0].ternary,cv::Scalar(100,255,0), 2); - putText(image,"1",cv::Point(v.robot_list[0].position.x-5,v.robot_list[0].position.y-17),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0),2); - circle(image,v.robot_list[1].position, 15, cv::Scalar(255,255,0), 2); - line(image,v.robot_list[1].position,v.robot_list[1].secundary,cv::Scalar(255,255,0), 2); - //line(image,v.robot_list[1].position,v.robot_list[1].ternary,cv::Scalar(100,255,0), 2); - putText(image,"2",cv::Point(v.robot_list[1].position.x-5,v.robot_list[1].position.y-17),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0),2); - circle(image,v.robot_list[2].position, 15, cv::Scalar(255,255,0), 2); - line(image,v.robot_list[2].position,v.robot_list[2].secundary,cv::Scalar(255,255,0), 2); - //line(image,v.robot_list[2].position,v.robot_list[2].ternary,cv::Scalar(100,255,0), 2); - putText(image,"3",cv::Point(v.robot_list[2].position.x-5,v.robot_list[2].position.y-17),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0),2); - circle(image,vision->getBallPosition(), 7, cv::Scalar(255,255,255), 2); + circle(imageView,interface.robot_list[0].position, 15, cv::Scalar(255,255,0), 2); + line(imageView,interface.robot_list[0].position,interface.robot_list[0].secundary,cv::Scalar(255,255,0), 2); + //line(imageView,interface.robot_list[0].position,interface.robot_list[0].ternary,cv::Scalar(100,255,0), 2); + putText(imageView,"1",cv::Point(interface.robot_list[0].position.x-5,interface.robot_list[0].position.y-17),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0),2); + circle(imageView,interface.robot_list[1].position, 15, cv::Scalar(255,255,0), 2); + line(imageView,interface.robot_list[1].position,interface.robot_list[1].secundary,cv::Scalar(255,255,0), 2); + //line(imageView,interface.robot_list[1].position,interface.robot_list[1].ternary,cv::Scalar(100,255,0), 2); + putText(imageView,"2",cv::Point(interface.robot_list[1].position.x-5,interface.robot_list[1].position.y-17),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0),2); + circle(imageView,interface.robot_list[2].position, 15, cv::Scalar(255,255,0), 2); + line(imageView,interface.robot_list[2].position,interface.robot_list[2].secundary,cv::Scalar(255,255,0), 2); + //line(imageView,interface.robot_list[2].position,interface.robot_list[2].ternary,cv::Scalar(100,255,0), 2); + putText(imageView,"3",cv::Point(interface.robot_list[2].position.x-5,interface.robot_list[2].position.y-17),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0),2); + circle(imageView,vision->getBallPosition(), 7, cv::Scalar(255,255,255), 2); for(int i=0; iAdv_Main.size(); i++) - circle(image,vision->Adv_Main[i], 15, cv::Scalar(0,0,255), 2); + circle(imageView,vision->Adv_Main[i], 15, cv::Scalar(0,0,255), 2); } } - if(v.iv.PID_test_flag) PID_test(); + if(interface.imageView.PID_test_flag) PID_test(); else { - for(int i=0; igetBallPosition()); + strategyGUI.strategy.set_Ball(vision->getBallPosition()); /* - line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2),cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2),cv::Point(0,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2),cv::Point(0,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),0),cv::Point(strategy.strats.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),height), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),strategy.strats.MAX_GOL_Y),cv::Point(width,strategy.strats.MAX_GOL_Y), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),strategy.strats.MIN_GOL_Y),cv::Point(width,strategy.strats.MIN_GOL_Y), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),0),cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),height), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),strategyGUI.strategy.MAX_GOL_Y),cv::Point(width,strategyGUI.strategy.MAX_GOL_Y), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),strategyGUI.strategy.MIN_GOL_Y),cv::Point(width,strategyGUI.strategy.MIN_GOL_Y), cv::Scalar(255,255,255),2); */ - if(v.start_game_flag) { - Ball_Est=strategy.strats.get_Ball_Est(); - line(image,vision->getBallPosition(),Ball_Est,cv::Scalar(255,140,0), 2); - circle(image,Ball_Est, 7, cv::Scalar(255,140,0), 2); + if(interface.start_game_flag) { + Ball_Est=strategyGUI.strategy.get_Ball_Est(); + line(imageView,vision->getBallPosition(),Ball_Est,cv::Scalar(255,140,0), 2); + circle(imageView,Ball_Est, 7, cv::Scalar(255,140,0), 2); char buffer[3]; for(int i =0; i<3; i++) { - switch (v.robot_list[i].role) { + switch (interface.robot_list[i].role) { case 0: - v.robot_list[i].target = strategy.strats.get_gk_target(vision->Adv_Main); - v.robot_list[i].fixedPos = strategy.strats.Goalkeeper.fixedPos; - if(strategy.strats.GOAL_DANGER_ZONE) { + interface.robot_list[i].target = strategyGUI.strategy.get_gk_target(vision->Adv_Main); + interface.robot_list[i].fixedPos = strategyGUI.strategy.Goalkeeper.fixedPos; + if(strategyGUI.strategy.GOAL_DANGER_ZONE) { // cout<<"hist_wipe"<Adv_Main.size();j++){ - if ( sqrt(pow(vision->Adv_Main[j].x - v.robot_list[i].position.x, 2) + pow(vision->Adv_Main[j].y - v.robot_list[i].position.y, 2)) < 50) { - v.robot_list[i].histWipe(); + if ( sqrt(pow(vision->Adv_Main[j].x - interface.robot_list[i].position.x, 2) + pow(vision->Adv_Main[j].y - interface.robot_list[i].position.y, 2)) < 50) { + interface.robot_list[i].histWipe(); } }*/ break; case 1: - v.robot_list[i].target = strategy.strats.get_def_target(v.robot_list[i].position); - v.robot_list[i].fixedPos = strategy.strats.Defense.fixedPos; - v.robot_list[i].status = strategy.strats.Defense.status; + interface.robot_list[i].target = strategyGUI.strategy.get_def_target(interface.robot_list[i].position); + interface.robot_list[i].fixedPos = strategyGUI.strategy.Defense.fixedPos; + interface.robot_list[i].status = strategyGUI.strategy.Defense.status; /* for(int j=0;jAdv_Main.size();j++){ - if ( sqrt(pow(vision->Adv_Main[j].x - v.robot_list[i].position.x, 2) + pow(vision->Adv_Main[j].y - v.robot_list[i].position.y, 2)) < 50) { - v.robot_list[i].spin = true; + if ( sqrt(pow(vision->Adv_Main[j].x - interface.robot_list[i].position.x, 2) + pow(vision->Adv_Main[j].y - interface.robot_list[i].position.y, 2)) < 50) { + interface.robot_list[i].spin = true; } }*/ break; case 3: - v.robot_list[i].target = strategy.strats.get_opp_target(v.robot_list[i].position, v.robot_list[i].orientation); - v.robot_list[i].fixedPos = strategy.strats.Opponent.fixedPos; - v.robot_list[i].status = strategy.strats.Opponent.status; + interface.robot_list[i].target = strategyGUI.strategy.get_opp_target(interface.robot_list[i].position, interface.robot_list[i].orientation); + interface.robot_list[i].fixedPos = strategyGUI.strategy.Opponent.fixedPos; + interface.robot_list[i].status = strategyGUI.strategy.Opponent.status; break; } - //cout<threshold[v.Img_id][i]; + d[i]=vision->threshold[interface.Img_id][i]; } return true; } void send_vel_to_robots() { - for(int i=0; igetBallPosition()); + for(int i=0; igetBallPosition()); } else { - v.robot_list[i].Vr = 0 ; - v.robot_list[i].Vl = 0 ; + interface.robot_list[i].Vr = 0 ; + interface.robot_list[i].Vl = 0 ; } } - control.s.sendToThree(v.robot_list[0],v.robot_list[1],v.robot_list[2]); + control.s.sendToThree(interface.robot_list[0],interface.robot_list[1],interface.robot_list[2]); } void PID_test() { double dist; int old_Selec_index; old_Selec_index = Selec_index; - for(int i=0; i-1) { - v.robot_list[Selec_index].histWipe(); - if(sqrt(pow((vision->getBallPosition().x-v.robot_list[Selec_index].target.x),2)+pow((vision->getBallPosition().y-v.robot_list[Selec_index].target.y),2))<=7) + interface.robot_list[Selec_index].histWipe(); + if(sqrt(pow((vision->getBallPosition().x-interface.robot_list[Selec_index].target.x),2)+pow((vision->getBallPosition().y-interface.robot_list[Selec_index].target.y),2))<=7) fixed_ball[Selec_index]=true; if(fixed_ball[Selec_index]) - v.robot_list[Selec_index].target=vision->getBallPosition(); + interface.robot_list[Selec_index].target=vision->getBallPosition(); else - v.robot_list[Selec_index].target = cv::Point(v.iv.tar_pos[0],v.iv.tar_pos[1]); + interface.robot_list[Selec_index].target = cv::Point(interface.imageView.tar_pos[0],interface.imageView.tar_pos[1]); } - for(int i=0; igetBallPosition(); + interface.robot_list[i].target=vision->getBallPosition(); else { - if(sqrt(pow((v.robot_list[i].position.x-v.robot_list[i].target.x),2)+ - pow((v.robot_list[i].position.y-v.robot_list[i].target.y),2))<15) { - - v.robot_list[i].target = cv::Point(-1,-1); - v.iv.tar_pos[0]=-1; - v.iv.tar_pos[1]=-1; - v.robot_list[i].Vr = 0 ; - v.robot_list[i].Vl = 0 ; + if(sqrt(pow((interface.robot_list[i].position.x-interface.robot_list[i].target.x),2)+ + pow((interface.robot_list[i].position.y-interface.robot_list[i].target.y),2))<15) { + + interface.robot_list[i].target = cv::Point(-1,-1); + interface.imageView.tar_pos[0]=-1; + interface.imageView.tar_pos[1]=-1; + interface.robot_list[i].Vr = 0 ; + interface.robot_list[i].Vl = 0 ; } - if(v.robot_list[i].target.x!=-1&&v.robot_list[i].target.y!=-1) { - v.robot_list[i].goTo(v.robot_list[i].target,vision->getBallPosition()); + if(interface.robot_list[i].target.x!=-1&&interface.robot_list[i].target.y!=-1) { + interface.robot_list[i].goTo(interface.robot_list[i].target,vision->getBallPosition()); } else { - v.robot_list[i].Vr = 0 ; - v.robot_list[i].Vl = 0 ; + interface.robot_list[i].Vr = 0 ; + interface.robot_list[i].Vl = 0 ; } } @@ -415,118 +415,118 @@ class CamCap: } - void drawStrategyConstants(cv::Mat image, int w, int h) + void drawStrategyConstants(cv::Mat imageView, int w, int h) { - if (strategy.get_deslocamentoZagaAtaque_flag()) + if (strategyGUI.get_deslocamentoZagaAtaque_flag()) { - line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2),cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2),cv::Point(0,strategy.strats.LARGURA_CAMPO/2-strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.LIMITE_AREA_X,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2),cv::Point(0,strategy.strats.LARGURA_CAMPO/2+strategy.strats.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.Ball.x, strategy.strats.Ball.y - 100),cv::Point(strategy.strats.Ball.x, strategy.strats.Ball.y + 100), cv::Scalar(255,0,0),2); - line(image, cv::Point(strategy.strats.Ball),cv::Point(strategy.strats.Ball.x - 100, strategy.strats.Ball.y), cv::Scalar(255,0,0),2); + line(imageView, cv::Point(strategyGUI.strategy.Ball.x, strategyGUI.strategy.Ball.y - 100),cv::Point(strategyGUI.strategy.Ball.x, strategyGUI.strategy.Ball.y + 100), cv::Scalar(255,0,0),2); + line(imageView, cv::Point(strategyGUI.strategy.Ball),cv::Point(strategyGUI.strategy.Ball.x - 100, strategyGUI.strategy.Ball.y), cv::Scalar(255,0,0),2); - line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,0),cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,strategy.strats.MIN_GOL_Y), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,height),cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,strategy.strats.MAX_GOL_Y), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,strategy.strats.MAX_GOL_Y),cv::Point(width,strategy.strats.MAX_GOL_Y), cv::Scalar(255,255,255),2); - line(image, cv::Point(strategy.strats.COMPRIMENTO_CAMPO_TOTAL - strategy.strats.LIMITE_AREA_X,strategy.strats.MIN_GOL_Y),cv::Point(width,strategy.strats.MIN_GOL_Y), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,0),cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MIN_GOL_Y), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,height),cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MAX_GOL_Y), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MAX_GOL_Y),cv::Point(width,strategyGUI.strategy.MAX_GOL_Y), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MIN_GOL_Y),cv::Point(width,strategyGUI.strategy.MIN_GOL_Y), cv::Scalar(255,255,255),2); - line(image,cv::Point(strategy.strats.DESLOCAMENTO_ZAGA_ATAQUE, 0),cv::Point(strategy.strats.DESLOCAMENTO_ZAGA_ATAQUE, h),cv::Scalar(255,255,0), 2); + line(imageView,cv::Point(strategyGUI.strategy.DESLOCAMENTO_ZAGA_ATAQUE, 0),cv::Point(strategyGUI.strategy.DESLOCAMENTO_ZAGA_ATAQUE, h),cv::Scalar(255,255,0), 2); } - if (strategy.get_goalSize_flag()) - line(image,cv::Point(strategy.strats.TAMANHO_GOL, h/2-strategy.strats.TAMANHO_GOL/2),cv::Point(strategy.strats.TAMANHO_GOL, h/2+strategy.strats.TAMANHO_GOL/2),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_goalSize_flag()) + line(imageView,cv::Point(strategyGUI.strategy.TAMANHO_GOL, h/2-strategyGUI.strategy.TAMANHO_GOL/2),cv::Point(strategyGUI.strategy.TAMANHO_GOL, h/2+strategyGUI.strategy.TAMANHO_GOL/2),cv::Scalar(255,255,0), 2); - if (strategy.get_fieldWwidth_flag()) - line(image,cv::Point(0, h/2),cv::Point(strategy.strats.LARGURA_CAMPO, h/2),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_fieldWwidth_flag()) + line(imageView,cv::Point(0, h/2),cv::Point(strategyGUI.strategy.LARGURA_CAMPO, h/2),cv::Scalar(255,255,0), 2); - if (strategy.get_totalFieldLength_flag()) - line(image,cv::Point(w/2, 0),cv::Point(w/2, strategy.strats.COMPRIMENTO_CAMPO_TOTAL),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_totalFieldLength_flag()) + line(imageView,cv::Point(w/2, 0),cv::Point(w/2, strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL),cv::Scalar(255,255,0), 2); - if (strategy.get_fieldLength_flag()) - line(image,cv::Point(round(0.10*float(h)/1.70), h/2),cv::Point(strategy.strats.COMPRIMENTO_PISTA, h/2),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_fieldLength_flag()) + line(imageView,cv::Point(round(0.10*float(h)/1.70), h/2),cv::Point(strategyGUI.strategy.COMPRIMENTO_PISTA, h/2),cv::Scalar(255,255,0), 2); - if (strategy.get_coneRatio_flag()) - line(image,cv::Point(strategy.strats.CONE_RATIO, 0),cv::Point(strategy.strats.CONE_RATIO, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_coneRatio_flag()) + line(imageView,cv::Point(strategyGUI.strategy.CONE_RATIO, 0),cv::Point(strategyGUI.strategy.CONE_RATIO, h),cv::Scalar(255,255,0), 2); - if (strategy.get_offsetRatio_flag()) - line(image,cv::Point(strategy.strats.OFFSET_RATIO, 0),cv::Point(strategy.strats.OFFSET_RATIO, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_offsetRatio_flag()) + line(imageView,cv::Point(strategyGUI.strategy.OFFSET_RATIO, 0),cv::Point(strategyGUI.strategy.OFFSET_RATIO, h),cv::Scalar(255,255,0), 2); - if (strategy.get_defenseLine_flag()) - line(image,cv::Point(strategy.strats.LINHA_ZAGA, 0),cv::Point(strategy.strats.LINHA_ZAGA, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_defenseLine_flag()) + line(imageView,cv::Point(strategyGUI.strategy.LINHA_ZAGA, 0),cv::Point(strategyGUI.strategy.LINHA_ZAGA, h),cv::Scalar(255,255,0), 2); - if (strategy.get_goalMax_flag()) - putText(image, "X", cv::Point(w-strategy.strats.COMPRIMENTO_PISTA, strategy.strats.MEIO_GOL_Y+strategy.strats.TAMANHO_GOL/2),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); + if (strategyGUI.get_goalMax_flag()) + putText(imageView, "X", cv::Point(w-strategyGUI.strategy.COMPRIMENTO_PISTA, strategyGUI.strategy.MEIO_GOL_Y+strategyGUI.strategy.TAMANHO_GOL/2),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); - if (strategy.get_goalMin_flag()) - putText(image, "X", cv::Point(w-strategy.strats.COMPRIMENTO_PISTA, strategy.strats.MEIO_GOL_Y-strategy.strats.TAMANHO_GOL/2),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); + if (strategyGUI.get_goalMin_flag()) + putText(imageView, "X", cv::Point(w-strategyGUI.strategy.COMPRIMENTO_PISTA, strategyGUI.strategy.MEIO_GOL_Y-strategyGUI.strategy.TAMANHO_GOL/2),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); - if (strategy.get_goalCenter_flag()) - putText(image, "X", cv::Point(strategy.strats.MEIO_GOL_X, strategy.strats.MEIO_GOL_Y),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); + if (strategyGUI.get_goalCenter_flag()) + putText(imageView, "X", cv::Point(strategyGUI.strategy.MEIO_GOL_X, strategyGUI.strategy.MEIO_GOL_Y),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0), 2); - if (strategy.get_banheira_flag()) - line(image,cv::Point(strategy.strats.BANHEIRA, 0),cv::Point(strategy.strats.BANHEIRA, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_banheira_flag()) + line(imageView,cv::Point(strategyGUI.strategy.BANHEIRA, 0),cv::Point(strategyGUI.strategy.BANHEIRA, h),cv::Scalar(255,255,0), 2); - if (strategy.get_banheiraOffset_flag()) - line(image,cv::Point(strategy.strats.OFFSET_BANHEIRA, 0),cv::Point(strategy.strats.OFFSET_BANHEIRA, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_banheiraOffset_flag()) + line(imageView,cv::Point(strategyGUI.strategy.OFFSET_BANHEIRA, 0),cv::Point(strategyGUI.strategy.OFFSET_BANHEIRA, h),cv::Scalar(255,255,0), 2); - if (strategy.get_areasDivision_flag()) - line(image,cv::Point(strategy.strats.DIVISAO_AREAS, 0),cv::Point(strategy.strats.DIVISAO_AREAS, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_areasDivision_flag()) + line(imageView,cv::Point(strategyGUI.strategy.DIVISAO_AREAS, 0),cv::Point(strategyGUI.strategy.DIVISAO_AREAS, h),cv::Scalar(255,255,0), 2); - if (strategy.get_areaSize_flag()) - line(image,cv::Point(strategy.strats.TAMANHO_AREA, 0),cv::Point(strategy.strats.TAMANHO_AREA, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_areaSize_flag()) + line(imageView,cv::Point(strategyGUI.strategy.TAMANHO_AREA, 0),cv::Point(strategyGUI.strategy.TAMANHO_AREA, h),cv::Scalar(255,255,0), 2); - if (strategy.get_areaLimitX_flag()) - line(image,cv::Point(strategy.strats.LIMITE_AREA_X, 0),cv::Point(strategy.strats.LIMITE_AREA_X, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_areaLimitX_flag()) + line(imageView,cv::Point(strategyGUI.strategy.LIMITE_AREA_X, 0),cv::Point(strategyGUI.strategy.LIMITE_AREA_X, h),cv::Scalar(255,255,0), 2); } - void warp_transform(cv::Mat image){ + void warp_transform(cv::Mat imageView){ cv::Point2f inputQuad[4]; cv::Point2f outputQuad[4]; - cv::Mat lambda = cv::Mat::zeros( image.rows, image.cols, image.type() ); + cv::Mat lambda = cv::Mat::zeros( imageView.rows, imageView.cols, imageView.type() ); - inputQuad[0] = cv::Point2f( v.iv.warp_mat[0][0]-v.offsetL,v.iv.warp_mat[0][1]); - inputQuad[1] = cv::Point2f( v.iv.warp_mat[1][0]+v.offsetR,v.iv.warp_mat[1][1]); - inputQuad[2] = cv::Point2f( v.iv.warp_mat[2][0]+v.offsetR,v.iv.warp_mat[2][1]); - inputQuad[3] = cv::Point2f( v.iv.warp_mat[3][0]-v.offsetL,v.iv.warp_mat[3][1]); + inputQuad[0] = cv::Point2f( interface.imageView.warp_mat[0][0]-interface.offsetL,interface.imageView.warp_mat[0][1]); + inputQuad[1] = cv::Point2f( interface.imageView.warp_mat[1][0]+interface.offsetR,interface.imageView.warp_mat[1][1]); + inputQuad[2] = cv::Point2f( interface.imageView.warp_mat[2][0]+interface.offsetR,interface.imageView.warp_mat[2][1]); + inputQuad[3] = cv::Point2f( interface.imageView.warp_mat[3][0]-interface.offsetL,interface.imageView.warp_mat[3][1]); outputQuad[0] = cv::Point2f( 0,0 ); outputQuad[1] = cv::Point2f( w-1,0); outputQuad[2] = cv::Point2f( w-1,h-1); outputQuad[3] = cv::Point2f( 0,h-1 ); lambda = getPerspectiveTransform( inputQuad, outputQuad ); - warpPerspective(image,image,lambda,image.size()); - if(v.iv.adjust_rdy) { - v.bt_adjust.set_active(false); - v.bt_adjust.set_state(Gtk::STATE_INSENSITIVE); - v.adjust_event_flag = false; - v.iv.adjust_event_flag = false; - for(int i =0; i(i, j) =0; + warpPerspective(imageView,imageView,lambda,imageView.size()); + if(interface.imageView.adjust_rdy) { + interface.bt_adjust.set_active(false); + interface.bt_adjust.set_state(Gtk::STATE_INSENSITIVE); + interface.adjust_event_flag = false; + interface.imageView.adjust_event_flag = false; + for(int i =0; i(i, j) =0; } } - for(int i = height; i>v.iv.adjust_mat[1][1]; i--) { - for(int j =0; j<3*v.iv.adjust_mat[1][0]; j++) { - image.at(i, j) =0; + for(int i = height; i>interface.imageView.adjust_mat[1][1]; i--) { + for(int j =0; j<3*interface.imageView.adjust_mat[1][0]; j++) { + imageView.at(i, j) =0; } } - for(int i =0; i3*v.iv.adjust_mat[2][0]; j--) { - image.at(i, j) =0; + for(int i =0; i3*interface.imageView.adjust_mat[2][0]; j--) { + imageView.at(i, j) =0; } } - for(int i =height; i>v.iv.adjust_mat[3][1]; i--) { - for(int j =3*width; j>3*v.iv.adjust_mat[3][0]; j--) { - image.at(i, j) =0; + for(int i =height; i>interface.imageView.adjust_mat[3][1]; i--) { + for(int j =3*width; j>3*interface.imageView.adjust_mat[3][0]; j--) { + imageView.at(i, j) =0; } } @@ -538,16 +538,16 @@ class CamCap: fixed_ball[0]=false; fixed_ball[1]=false; fixed_ball[2]=false; - fm.set_label("Image"); - fm.add(v.iv); - notebook.append_page(v, "Vision"); + fm.set_label("imageView"); + fm.add(interface.imageView); + notebook.append_page(interface, "Vision"); notebook.append_page(control, "Control"); - notebook.append_page(strategy, "Strategy"); + notebook.append_page(strategyGUI, "strategyGUI"); for(int i=0; i<4; i++) { - v.iv.adjust_mat[i][0] = -1; - v.iv.adjust_mat[i][1] = -1; + interface.imageView.adjust_mat[i][0] = -1; + interface.imageView.adjust_mat[i][1] = -1; } @@ -555,18 +555,18 @@ class CamCap: camera_vbox.pack_start(fm, false, true, 10); camera_vbox.pack_start(info_fm, false, true, 10); - info_fm.add(v.info_hbox); + info_fm.add(interface.info_hbox); pack_start(camera_vbox, true, true, 10); pack_start(notebook, false, false, 10); - v.signal_start().connect(sigc::mem_fun(*this, &CamCap::start_signal)); + interface.signal_start().connect(sigc::mem_fun(*this, &CamCap::start_signal)); } ~CamCap(){ con.disconnect(); - v.iv.disable_image_show(); + interface.imageView.disable_image_show(); free(data); data = 0; diff --git a/cc/strategyGUI.hpp b/cc/strategyGUI.hpp index 82401e56..cf24b24d 100644 --- a/cc/strategyGUI.hpp +++ b/cc/strategyGUI.hpp @@ -52,7 +52,7 @@ class StrategyGUI: public Gtk::VBox Gtk::TextView strategy_description_view; Glib::RefPtr strategy_description_text; - Strategy strats; + Strategy strategy; protected: Gtk::Grid constants_grid; diff --git a/pack-capture-gui/capture-gui/V4LInterface-events.cpp b/pack-capture-gui/capture-gui/V4LInterface-events.cpp index 7f05b409..957b027c 100644 --- a/pack-capture-gui/capture-gui/V4LInterface-events.cpp +++ b/pack-capture-gui/capture-gui/V4LInterface-events.cpp @@ -336,14 +336,14 @@ void V4LInterface::__event_bt_start_clicked() { void V4LInterface::__event_bt_warp_clicked() { std::cout<<"Warp drive engaged"<>>>>>>INVERTED<<<<<<<" << std::endl; + std::cout << "imageView >>>>>>>INVERTED<<<<<<<" << std::endl; } else { invert_image_flag = false; - std::cout << "image >>>>>>>NORMAL<<<<<<<" << std::endl; + std::cout << "imageView >>>>>>>NORMAL<<<<<<<" << std::endl; } } diff --git a/pack-capture-gui/capture-gui/V4LInterface.hpp b/pack-capture-gui/capture-gui/V4LInterface.hpp index e03662f6..95e156f6 100644 --- a/pack-capture-gui/capture-gui/V4LInterface.hpp +++ b/pack-capture-gui/capture-gui/V4LInterface.hpp @@ -26,7 +26,7 @@ class V4LInterface: public Gtk::VBox { bool warped = false; - ImageView iv; + ImageView imageView; double ballX, ballY; From fe34544ce2349847741ae15151f585bacc393cb5 Mon Sep 17 00:00:00 2001 From: pqvs Date: Mon, 20 Feb 2017 16:20:17 -0300 Subject: [PATCH 08/20] =?UTF-8?q?HSV=20inicia=20corretamente.=20Corre?= =?UTF-8?q?=C3=A7=C3=B5es=20no=20desenho=20das=20constantes=20da=20estrat?= =?UTF-8?q?=C3=A9gia.=20Arrumado=20algumas=20leitoagens=20do=20c=C3=B3digo?= =?UTF-8?q?=20da=20vis=C3=A3o.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cc/camcap.hpp | 89 +++++------- cc/strategyGUI.hpp | 136 +++++------------- cc/vision.hpp | 6 +- .../capture-gui/V4LInterface-aux.cpp | 38 +++-- pack-capture-gui/capture-gui/V4LInterface.hpp | 1 + 5 files changed, 97 insertions(+), 173 deletions(-) diff --git a/cc/camcap.hpp b/cc/camcap.hpp index 4c2dc70a..d77d5ae0 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -66,19 +66,23 @@ class CamCap: void updateAllPositions() { + Robot robot; + cv::Point ballPosition; // cout<<"AQUI"<robot_creation_unitag(); //vision->robot_creation(); // cout<<"AQUI"<getRobotListSize(); i++) + for (int i = 0; i < vision->get_robot_list_size(); i++) { - interface.robot_list[i].position = vision->getRobotFromList(i).position; - interface.robot_list[i].orientation = vision->getRobotFromList(i).orientation; - interface.robot_list[i].secundary = vision->getRobotFromList(i).secundary; + robot = vision->get_robot_from_list(i); + interface.robot_list[i].position = robot.position; + interface.robot_list[i].orientation = robot.orientation; + interface.robot_list[i].secundary = robot.secundary; } - // cout<<"AQUI"<getBallPosition().x; - interface.ballY = vision->getBallPosition().y; + + ballPosition = vision->get_ball_position(); + interface.ballX = ballPosition.x; + interface.ballY = ballPosition.y; interface.robot_list[0].feedHist(interface.robot_list[0].position); interface.robot_list[1].feedHist(interface.robot_list[1].position); @@ -143,7 +147,7 @@ class CamCap: } - interface.__event_bt_quick_load_clicked(); + //interface.__event_bt_quick_load_clicked(); return true; } @@ -220,7 +224,7 @@ class CamCap: line(imageView,interface.robot_list[2].position,interface.robot_list[2].secundary,cv::Scalar(255,255,0), 2); //line(imageView,interface.robot_list[2].position,interface.robot_list[2].ternary,cv::Scalar(100,255,0), 2); putText(imageView,"3",cv::Point(interface.robot_list[2].position.x-5,interface.robot_list[2].position.y-17),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0),2); - circle(imageView,vision->getBallPosition(), 7, cv::Scalar(255,255,255), 2); + circle(imageView,vision->get_ball_position(), 7, cv::Scalar(255,255,255), 2); for(int i=0; iAdv_Main.size(); i++) circle(imageView,vision->Adv_Main[i], 15, cv::Scalar(0,0,255), 2); @@ -248,7 +252,7 @@ class CamCap: } // ----------- ESTRATEGIA -----------------// - strategyGUI.strategy.set_Ball(vision->getBallPosition()); + strategyGUI.strategy.set_Ball(vision->get_ball_position()); /* line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); @@ -260,7 +264,7 @@ class CamCap: */ if(interface.start_game_flag) { Ball_Est=strategyGUI.strategy.get_Ball_Est(); - line(imageView,vision->getBallPosition(),Ball_Est,cv::Scalar(255,140,0), 2); + line(imageView,vision->get_ball_position(),Ball_Est,cv::Scalar(255,140,0), 2); circle(imageView,Ball_Est, 7, cv::Scalar(255,140,0), 2); char buffer[3]; for(int i =0; i<3; i++) { @@ -352,7 +356,7 @@ class CamCap: void send_vel_to_robots() { for(int i=0; igetBallPosition()); + interface.robot_list[i].goTo(interface.robot_list[i].target,vision->get_ball_position()); } else { interface.robot_list[i].Vr = 0 ; interface.robot_list[i].Vl = 0 ; @@ -377,12 +381,12 @@ class CamCap: } if(Selec_index>-1) { interface.robot_list[Selec_index].histWipe(); - if(sqrt(pow((vision->getBallPosition().x-interface.robot_list[Selec_index].target.x),2)+pow((vision->getBallPosition().y-interface.robot_list[Selec_index].target.y),2))<=7) + if(sqrt(pow((vision->get_ball_position().x-interface.robot_list[Selec_index].target.x),2)+pow((vision->get_ball_position().y-interface.robot_list[Selec_index].target.y),2))<=7) fixed_ball[Selec_index]=true; if(fixed_ball[Selec_index]) - interface.robot_list[Selec_index].target=vision->getBallPosition(); + interface.robot_list[Selec_index].target=vision->get_ball_position(); else interface.robot_list[Selec_index].target = cv::Point(interface.imageView.tar_pos[0],interface.imageView.tar_pos[1]); } @@ -390,7 +394,7 @@ class CamCap: for(int i=0; igetBallPosition(); + interface.robot_list[i].target=vision->get_ball_position(); else { if(sqrt(pow((interface.robot_list[i].position.x-interface.robot_list[i].target.x),2)+ pow((interface.robot_list[i].position.y-interface.robot_list[i].target.y),2))<15) { @@ -402,7 +406,7 @@ class CamCap: interface.robot_list[i].Vl = 0 ; } if(interface.robot_list[i].target.x!=-1&&interface.robot_list[i].target.y!=-1) { - interface.robot_list[i].goTo(interface.robot_list[i].target,vision->getBallPosition()); + interface.robot_list[i].goTo(interface.robot_list[i].target,vision->get_ball_position()); } else { interface.robot_list[i].Vr = 0 ; interface.robot_list[i].Vl = 0 ; @@ -419,39 +423,28 @@ class CamCap: { if (strategyGUI.get_deslocamentoZagaAtaque_flag()) - { - line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - - line(imageView, cv::Point(strategyGUI.strategy.Ball.x, strategyGUI.strategy.Ball.y - 100),cv::Point(strategyGUI.strategy.Ball.x, strategyGUI.strategy.Ball.y + 100), cv::Scalar(255,0,0),2); - line(imageView, cv::Point(strategyGUI.strategy.Ball),cv::Point(strategyGUI.strategy.Ball.x - 100, strategyGUI.strategy.Ball.y), cv::Scalar(255,0,0),2); + line(imageView,cv::Point(strategyGUI.strategy.DESLOCAMENTO_ZAGA_ATAQUE, 0),cv::Point(strategyGUI.strategy.DESLOCAMENTO_ZAGA_ATAQUE, h),cv::Scalar(255,255,0), 2); - line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,0),cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MIN_GOL_Y), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,height),cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MAX_GOL_Y), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MAX_GOL_Y),cv::Point(width,strategyGUI.strategy.MAX_GOL_Y), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MIN_GOL_Y),cv::Point(width,strategyGUI.strategy.MIN_GOL_Y), cv::Scalar(255,255,255),2); - - line(imageView,cv::Point(strategyGUI.strategy.DESLOCAMENTO_ZAGA_ATAQUE, 0),cv::Point(strategyGUI.strategy.DESLOCAMENTO_ZAGA_ATAQUE, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_goalArea_flag()) + { + line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); } - if (strategyGUI.get_goalSize_flag()) - line(imageView,cv::Point(strategyGUI.strategy.TAMANHO_GOL, h/2-strategyGUI.strategy.TAMANHO_GOL/2),cv::Point(strategyGUI.strategy.TAMANHO_GOL, h/2+strategyGUI.strategy.TAMANHO_GOL/2),cv::Scalar(255,255,0), 2); - - if (strategyGUI.get_fieldWwidth_flag()) - line(imageView,cv::Point(0, h/2),cv::Point(strategyGUI.strategy.LARGURA_CAMPO, h/2),cv::Scalar(255,255,0), 2); - - if (strategyGUI.get_totalFieldLength_flag()) - line(imageView,cv::Point(w/2, 0),cv::Point(w/2, strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL),cv::Scalar(255,255,0), 2); - - if (strategyGUI.get_fieldLength_flag()) - line(imageView,cv::Point(round(0.10*float(h)/1.70), h/2),cv::Point(strategyGUI.strategy.COMPRIMENTO_PISTA, h/2),cv::Scalar(255,255,0), 2); - - if (strategyGUI.get_coneRatio_flag()) - line(imageView,cv::Point(strategyGUI.strategy.CONE_RATIO, 0),cv::Point(strategyGUI.strategy.CONE_RATIO, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_sideRectangles_flag()) + { + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,0),cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MIN_GOL_Y), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,height),cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MAX_GOL_Y), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MAX_GOL_Y),cv::Point(width,strategyGUI.strategy.MAX_GOL_Y), cv::Scalar(255,255,255),2); + line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO_TOTAL - strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.MIN_GOL_Y),cv::Point(width,strategyGUI.strategy.MIN_GOL_Y), cv::Scalar(255,255,255),2); + } - if (strategyGUI.get_offsetRatio_flag()) - line(imageView,cv::Point(strategyGUI.strategy.OFFSET_RATIO, 0),cv::Point(strategyGUI.strategy.OFFSET_RATIO, h),cv::Scalar(255,255,0), 2); + if (strategyGUI.get_ballRadius_flag()) + { + ellipse(imageView, strategyGUI.strategy.Ball, cv::Size(100,100), 90, 0, 180, cv::Scalar(255,0,0), 2); + line(imageView, cv::Point(strategyGUI.strategy.Ball.x, strategyGUI.strategy.Ball.y+100), cv::Point(strategyGUI.strategy.Ball.x, strategyGUI.strategy.Ball.y-100), cv::Scalar(255,0,0), 2); + } if (strategyGUI.get_defenseLine_flag()) line(imageView,cv::Point(strategyGUI.strategy.LINHA_ZAGA, 0),cv::Point(strategyGUI.strategy.LINHA_ZAGA, h),cv::Scalar(255,255,0), 2); @@ -468,15 +461,9 @@ class CamCap: if (strategyGUI.get_banheira_flag()) line(imageView,cv::Point(strategyGUI.strategy.BANHEIRA, 0),cv::Point(strategyGUI.strategy.BANHEIRA, h),cv::Scalar(255,255,0), 2); - if (strategyGUI.get_banheiraOffset_flag()) - line(imageView,cv::Point(strategyGUI.strategy.OFFSET_BANHEIRA, 0),cv::Point(strategyGUI.strategy.OFFSET_BANHEIRA, h),cv::Scalar(255,255,0), 2); - if (strategyGUI.get_areasDivision_flag()) line(imageView,cv::Point(strategyGUI.strategy.DIVISAO_AREAS, 0),cv::Point(strategyGUI.strategy.DIVISAO_AREAS, h),cv::Scalar(255,255,0), 2); - if (strategyGUI.get_areaSize_flag()) - line(imageView,cv::Point(strategyGUI.strategy.TAMANHO_AREA, 0),cv::Point(strategyGUI.strategy.TAMANHO_AREA, h),cv::Scalar(255,255,0), 2); - if (strategyGUI.get_areaLimitX_flag()) line(imageView,cv::Point(strategyGUI.strategy.LIMITE_AREA_X, 0),cv::Point(strategyGUI.strategy.LIMITE_AREA_X, h),cv::Scalar(255,255,0), 2); } diff --git a/cc/strategyGUI.hpp b/cc/strategyGUI.hpp index cf24b24d..9dbf3f30 100644 --- a/cc/strategyGUI.hpp +++ b/cc/strategyGUI.hpp @@ -56,76 +56,52 @@ class StrategyGUI: public Gtk::VBox protected: Gtk::Grid constants_grid; - Gtk::CheckButton fieldLength_bt; - Gtk::CheckButton totalFieldLength_bt; - Gtk::CheckButton fieldWwidth_bt; - Gtk::CheckButton goalSize_bt; - Gtk::CheckButton areaSize_bt; Gtk::CheckButton areaLimitX_bt; Gtk::CheckButton banheira_bt; Gtk::CheckButton areasDivision_bt; - Gtk::CheckButton banheiraOffset_bt; Gtk::CheckButton goalCenter_bt; Gtk::CheckButton goalMin_bt; Gtk::CheckButton goalMax_bt; Gtk::CheckButton defenseLine_bt; - Gtk::CheckButton offsetRatio_bt; - Gtk::CheckButton coneRatio_bt; Gtk::CheckButton deslocamentoZagaAtaque_bt; + Gtk::CheckButton ballRadius_bt; + Gtk::CheckButton goalArea_bt; + Gtk::CheckButton sideRectangles_bt; private: bool deslocamentoZagaAtaque_flag = false; bool goalSize_flag = false; - bool fieldWwidth_flag = false; - bool totalFieldLength_flag = false; - bool fieldLength_flag = false; - bool coneRatio_flag = false; - bool offsetRatio_flag = false; bool defenseLine_flag = false; bool goalMax_flag = false; bool goalMin_flag = false; bool goalCenter_flag = false; - bool banheiraOffset_flag = false; bool areasDivision_flag = false; bool banheira_flag = false; bool areaLimitX_flag = false; - bool areaSize_flag = false; + bool ballRadius_flag = false; + bool goalArea_flag = false; + bool sideRectangles_flag = false; public: - bool get_deslocamentoZagaAtaque_flag() - { - return deslocamentoZagaAtaque_flag; - } - - bool get_goalSize_flag() - { - return goalSize_flag; - } - - bool get_fieldWwidth_flag() + bool get_ballRadius_flag() { - return fieldWwidth_flag; + return ballRadius_flag; } - bool get_totalFieldLength_flag() + bool get_goalArea_flag() { - return totalFieldLength_flag; + return goalArea_flag; } - bool get_fieldLength_flag() + bool get_sideRectangles_flag() { - return fieldLength_flag; + return sideRectangles_flag; } - bool get_coneRatio_flag() - { - return coneRatio_flag; - } - - bool get_offsetRatio_flag() + bool get_deslocamentoZagaAtaque_flag() { - return offsetRatio_flag; + return deslocamentoZagaAtaque_flag; } bool get_defenseLine_flag() @@ -148,11 +124,6 @@ class StrategyGUI: public Gtk::VBox return goalCenter_flag; } - bool get_banheiraOffset_flag() - { - return banheiraOffset_flag; - } - bool get_areasDivision_flag() { return areasDivision_flag; @@ -163,10 +134,6 @@ class StrategyGUI: public Gtk::VBox return banheira_flag; } - bool get_areaSize_flag() - { - return areaSize_flag; - } bool get_areaLimitX_flag() { @@ -188,90 +155,66 @@ class StrategyGUI: public Gtk::VBox constants_fm.add(constants_grid); constants_fm.set_label("Draw Constants"); - fieldLength_bt.set_label("Field Length"); - totalFieldLength_bt.set_label("Total Field Length"); - fieldWwidth_bt.set_label("Field Width"); - goalSize_bt.set_label("Goal Size"); - areaSize_bt.set_label("Area Size"); areaLimitX_bt.set_label("Area Limit X"); banheira_bt.set_label("Banheira"); areasDivision_bt.set_label("Areas Division"); - banheiraOffset_bt.set_label("Banheira Offset"); goalCenter_bt.set_label("Goal Center"); goalMin_bt.set_label("Goal Min."); goalMax_bt.set_label("Goal Max."); defenseLine_bt.set_label("Defense Line"); - offsetRatio_bt.set_label("Offset Ratio"); - coneRatio_bt.set_label("Cone Ratio"); deslocamentoZagaAtaque_bt.set_label("Deslocamento Zaga Ataque"); + ballRadius_bt.set_label("Ball Radius"); + goalArea_bt.set_label("Goal Area"); + sideRectangles_bt.set_label("Side Rectangles"); constants_grid.set_border_width(10); constants_grid.set_column_spacing(5); - constants_grid.attach(fieldLength_bt, 0, 0, 1 , 1); - constants_grid.attach(totalFieldLength_bt, 1, 0, 1, 1); - constants_grid.attach(fieldWwidth_bt, 2, 0, 1, 1); + constants_grid.attach(ballRadius_bt, 0, 0, 1, 1); + constants_grid.attach(goalArea_bt, 1, 0, 1, 1); - constants_grid.attach(goalSize_bt, 0, 1, 1, 1); - constants_grid.attach(areaSize_bt, 1, 1, 1, 1); - constants_grid.attach(areaLimitX_bt, 2, 1, 1, 1); + constants_grid.attach(sideRectangles_bt, 0, 1, 1, 1); + constants_grid.attach(areaLimitX_bt, 1, 1, 1, 1); constants_grid.attach(banheira_bt, 0, 2, 1, 1); constants_grid.attach(areasDivision_bt, 1, 2, 1, 1); - constants_grid.attach(banheiraOffset_bt, 2, 2, 1, 1); constants_grid.attach(goalCenter_bt, 0, 3, 1, 1); constants_grid.attach(goalMin_bt, 1, 3, 1, 1); - constants_grid.attach(goalMax_bt, 2, 3, 1, 1); - constants_grid.attach(defenseLine_bt, 0, 4, 1, 1); - constants_grid.attach(offsetRatio_bt, 1, 4, 1, 1); - constants_grid.attach(coneRatio_bt, 2, 4, 1, 1); + constants_grid.attach(goalMax_bt, 0, 4, 1, 1); + constants_grid.attach(defenseLine_bt, 1, 4, 1, 1); constants_grid.attach(deslocamentoZagaAtaque_bt, 0, 5, 2, 1); - fieldLength_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_fieldLength_bt_clicked)); - totalFieldLength_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_totalFieldLength_bt_clicked)); - fieldWwidth_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_fieldWwidth_bt_clicked)); - goalSize_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalSize_bt_clicked)); - areaSize_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_areaSize_bt_clicked)); + ballRadius_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_ballRadius_bt_clicked)); + goalArea_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalArea_bt_clicked)); + sideRectangles_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_sideRectangles_bt_clicked)); areaLimitX_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_areaLimitX_bt_clicked)); banheira_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_banheira_bt_clicked)); areasDivision_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_areasDivision_bt_clicked)); - banheiraOffset_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_banheiraOffset_bt_clicked)); goalCenter_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalCenter_bt_clicked)); goalMin_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalMin_bt_clicked)); goalMax_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_goalMax_bt_clicked)); defenseLine_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_defenseLine_bt_clicked)); - offsetRatio_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_offsetRatio_bt_clicked)); - coneRatio_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_coneRatio_bt_clicked)); deslocamentoZagaAtaque_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_deslocamentoZagaAtaque_bt_clicked)); } private: - void _event_fieldLength_bt_clicked() - { - fieldLength_flag = !fieldLength_flag; - } - void _event_totalFieldLength_bt_clicked() + void _event_sideRectangles_bt_clicked() { - totalFieldLength_flag = !totalFieldLength_flag; + sideRectangles_flag = !sideRectangles_flag; } - void _event_fieldWwidth_bt_clicked() + void _event_ballRadius_bt_clicked() { - fieldWwidth_flag = !fieldWwidth_flag; + ballRadius_flag = !ballRadius_flag; } - void _event_goalSize_bt_clicked() + void _event_goalArea_bt_clicked() { - goalSize_flag = !goalSize_flag; - } - - void _event_areaSize_bt_clicked() - { - areaSize_flag = !areaSize_flag; + goalArea_flag = !goalArea_flag; } void _event_areaLimitX_bt_clicked() @@ -289,11 +232,6 @@ class StrategyGUI: public Gtk::VBox areasDivision_flag = !areasDivision_flag; } - void _event_banheiraOffset_bt_clicked() - { - banheiraOffset_flag = !banheiraOffset_flag; - } - void _event_goalCenter_bt_clicked() { goalCenter_flag = !goalCenter_flag; @@ -314,16 +252,6 @@ class StrategyGUI: public Gtk::VBox defenseLine_flag = !defenseLine_flag; } - void _event_offsetRatio_bt_clicked() - { - offsetRatio_flag = !offsetRatio_flag; - } - - void _event_coneRatio_bt_clicked() - { - coneRatio_flag = !coneRatio_flag; - } - void _event_deslocamentoZagaAtaque_bt_clicked() { deslocamentoZagaAtaque_flag = !deslocamentoZagaAtaque_flag; diff --git a/cc/vision.hpp b/cc/vision.hpp index 5ad0bb35..60ab284a 100644 --- a/cc/vision.hpp +++ b/cc/vision.hpp @@ -45,12 +45,12 @@ class Vision unsigned char **threshold = NULL; - int getRobotListSize() + int get_robot_list_size() { return robot_list.size(); } - Robot getRobotFromList(int index) + Robot get_robot_from_list(int index) { Robot r; if (index < robot_list.size() && index >= 0) @@ -62,7 +62,7 @@ class Vision } } - cv::Point getBallPosition() + cv::Point get_ball_position() { return Ball; } diff --git a/pack-capture-gui/capture-gui/V4LInterface-aux.cpp b/pack-capture-gui/capture-gui/V4LInterface-aux.cpp index 2be277ec..c6f24daf 100644 --- a/pack-capture-gui/capture-gui/V4LInterface-aux.cpp +++ b/pack-capture-gui/capture-gui/V4LInterface-aux.cpp @@ -1167,6 +1167,21 @@ namespace capture draw_info_checkbox.set_can_focus(false); } + void V4LInterface::init_HSV() + { + for(int i =0; i<6; i++) { + HScale_Hmin.set_value(-1); + HScale_Hmax.set_value(256); + HScale_Smin.set_value(-1); + HScale_Smax.set_value(256); + HScale_Vmin.set_value(-1); + HScale_Vmax.set_value(256); + HScale_Amin.set_value(500); + + __event_bt_left_HSV_calib_clicked(); + } + } + // Constructor V4LInterface::V4LInterface() : @@ -1198,6 +1213,11 @@ namespace capture bt_save_warp.set_sensitive(false); bt_quick_save.set_sensitive(false); bt_quick_load.set_sensitive(false); + bt_save_HSV_calib.set_state(Gtk::STATE_INSENSITIVE); + bt_load_HSV_calib.set_state(Gtk::STATE_INSENSITIVE); + bt_auto_calib.set_state(Gtk::STATE_INSENSITIVE); + bt_adjust.set_state(Gtk::STATE_INSENSITIVE); + m_signal_start.emit(false); @@ -1208,10 +1228,7 @@ namespace capture HScale_Smax.set_state(Gtk::STATE_INSENSITIVE); HScale_Vmax.set_state(Gtk::STATE_INSENSITIVE); HScale_Amin.set_state(Gtk::STATE_INSENSITIVE); - bt_save_HSV_calib.set_state(Gtk::STATE_INSENSITIVE); - bt_load_HSV_calib.set_state(Gtk::STATE_INSENSITIVE); - bt_auto_calib.set_state(Gtk::STATE_INSENSITIVE); - bt_adjust.set_state(Gtk::STATE_INSENSITIVE); + notebook.set_scrollable(true); adjust_event_flag = false; @@ -1254,20 +1271,12 @@ namespace capture robot_list[1].role = 1; robot_list[2].role = 2; - for(int i =0; i<6; i++) { - HScale_Hmin.set_value(-1); - HScale_Hmax.set_value(256); - HScale_Smin.set_value(-1); - HScale_Smax.set_value(256); - HScale_Vmin.set_value(-1); - HScale_Vmax.set_value(256); - } - + init_HSV(); for(int i=0; i Date: Wed, 22 Feb 2017 17:24:28 -0300 Subject: [PATCH 09/20] =?UTF-8?q?Mudan=C3=A7as=20no=20unitag=20e=20outras?= =?UTF-8?q?=20coisas?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CAM_quicksave.txt | 14 ++++++------ cc/Strategy.hpp | 8 +++---- cc/camcap.hpp | 45 +++++-------------------------------- cc/vision.hpp | 56 +++++++++++++++++++++-------------------------- 4 files changed, 41 insertions(+), 82 deletions(-) diff --git a/CAM_quicksave.txt b/CAM_quicksave.txt index 7431ea99..1ebe2c4e 100644 --- a/CAM_quicksave.txt +++ b/CAM_quicksave.txt @@ -1,25 +1,25 @@ 9963776 -82 +153 9963777 -48 +34 9963778 -38 +56 9963788 0 9963795 -22 +18 9963800 2 9963802 2800 9963803 -100 +255 9963804 0 10094849 1 10094850 -377 +310 10094851 0 10094856 @@ -27,7 +27,7 @@ 10094857 0 10094858 -85 +0 10094860 0 10094861 diff --git a/cc/Strategy.hpp b/cc/Strategy.hpp index 5b5ac66e..4069a354 100644 --- a/cc/Strategy.hpp +++ b/cc/Strategy.hpp @@ -735,11 +735,11 @@ void set_Ball_Est(cv::Point b) { } void set_Ball(cv::Point b) { Ball = b; - //LS_ball_x.addValue(Ball.x); - //LS_ball_y.addValue(Ball.y); + LS_ball_x.addValue(Ball.x); + LS_ball_y.addValue(Ball.y); - // Ball_Est.x = LS_ball_x.estimate(5); - //Ball_Est.y = LS_ball_y.estimate(5); + Ball_Est.x = LS_ball_x.estimate(5); + Ball_Est.y = LS_ball_y.estimate(5); } diff --git a/cc/camcap.hpp b/cc/camcap.hpp index d77d5ae0..5e0887ce 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -194,9 +194,6 @@ class CamCap: } - - - vision->setHSV(interface.H,interface.S,interface.V,interface.Amin); //TRACKING CAMERA @@ -226,6 +223,10 @@ class CamCap: putText(imageView,"3",cv::Point(interface.robot_list[2].position.x-5,interface.robot_list[2].position.y-17),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,0),2); circle(imageView,vision->get_ball_position(), 7, cv::Scalar(255,255,255), 2); + //std::cout << "Robot 0 position: (" << interface.robot_list[0].position.x << "," << interface.robot_list[0].position.y << ")" << std::endl; + //std::cout << "Robot 1 position: (" << interface.robot_list[1].position.x << "," << interface.robot_list[1].position.y << ")" << std::endl; + //std::cout << "Robot 2 position: (" << interface.robot_list[2].position.x << "," << interface.robot_list[2].position.y << ")" << std::endl; + for(int i=0; iAdv_Main.size(); i++) circle(imageView,vision->Adv_Main[i], 15, cv::Scalar(0,0,255), 2); } @@ -254,14 +255,6 @@ class CamCap: // ----------- ESTRATEGIA -----------------// strategyGUI.strategy.set_Ball(vision->get_ball_position()); - /* - line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2-strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.LIMITE_AREA_X,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2),cv::Point(0,strategyGUI.strategy.LARGURA_CAMPO/2+strategyGUI.strategy.TAMANHO_AREA/2), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),0),cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),height), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),strategyGUI.strategy.MAX_GOL_Y),cv::Point(width,strategyGUI.strategy.MAX_GOL_Y), cv::Scalar(255,255,255),2); - line(imageView, cv::Point(strategyGUI.strategy.COMPRIMENTO_CAMPO - round(0.2*float(width)/1.70),strategyGUI.strategy.MIN_GOL_Y),cv::Point(width,strategyGUI.strategy.MIN_GOL_Y), cv::Scalar(255,255,255),2); - */ if(interface.start_game_flag) { Ball_Est=strategyGUI.strategy.get_Ball_Est(); line(imageView,vision->get_ball_position(),Ball_Est,cv::Scalar(255,140,0), 2); @@ -273,7 +266,6 @@ class CamCap: interface.robot_list[i].target = strategyGUI.strategy.get_gk_target(vision->Adv_Main); interface.robot_list[i].fixedPos = strategyGUI.strategy.Goalkeeper.fixedPos; if(strategyGUI.strategy.GOAL_DANGER_ZONE) { - // cout<<"hist_wipe"<Adv_Main.size()<Adv_Main.size();i++){ - cout<Adv_Main[i].x<<" "<Adv_Main[i].y<<" "; - } - cout< robot; Robot r; - double omax = -99999; - double omin = 99999; + double omax = -99999; // angulo maximo + double omin = 99999; // angulo minimo // cout<<"1"<=30*PI/180) { - - robot_list[0].position = robot[i].position; // colocar em um vetor - - robot_list[0].secundary = robot[i].secundary; // colocar em um vetor - robot_list[0].orientation = robot[i].orientation; + o = atan2(sin(robot[i].orientation2-robot[i].orientation+3.1415),cos(robot[i].orientation2-robot[i].orientation+3.1415)); + // cout<=35*PI/180) { + robot_list[0].position = robot[i].position; // colocar em um vetor + robot_list[0].secundary = robot[i].secundary; // colocar em um vetor + robot_list[0].orientation = robot[i].orientation; - } - if(o<=-30*PI/180) { - - robot_list[1].position = robot[i].position; // colocar em um vetor - robot_list[1].secundary = robot[i].secundary; // colocar em um vetor - robot_list[1].orientation = robot[i].orientation; + }else if(o<=-35*PI/180) { + robot_list[1].position = robot[i].position; // colocar em um vetor + robot_list[1].secundary = robot[i].secundary; // colocar em um vetor + robot_list[1].orientation = robot[i].orientation; + } + if((o<30*PI/180)&&(o>-30*PI/180)) { + else{ + robot_list[2].position = robot[i].position; // colocar em um vetor + robot_list[2].secundary = robot[i].secundary; // colocar em um vetor + robot_list[2].orientation = robot[i].orientation; - } - if((o<=15*PI/180)&&(o>=-15*PI/180)) { - robot_list[2].position = robot[i].position; // colocar em um vetor - - robot_list[2].secundary = robot[i].secundary; // colocar em um vetor - robot_list[2].orientation = robot[i].orientation; - - - } - } + } + } } From b5f2f0dc01a35b4d75694892fa7cc480775c3082 Mon Sep 17 00:00:00 2001 From: pqvs Date: Fri, 24 Feb 2017 13:53:39 -0300 Subject: [PATCH 10/20] Corrigindo leitoagem --- cc/vision.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/cc/vision.hpp b/cc/vision.hpp index 4b7175df..9a3ce672 100644 --- a/cc/vision.hpp +++ b/cc/vision.hpp @@ -269,7 +269,7 @@ class Vision distanceRef2 = 999999999.0; - + for(int i = 0; i < 1; i++) { for(int k = 0; k < Team_Sec[i].size(); k++) { distance = calcDistance(Team_Main[j],Team_Sec[i][k]); @@ -343,14 +343,13 @@ class Vision robot_list[1].orientation = robot[i].orientation; } - if((o<30*PI/180)&&(o>-30*PI/180)) { + // if((o<30*PI/180)&&(o>-30*PI/180)) { else{ robot_list[2].position = robot[i].position; // colocar em um vetor robot_list[2].secundary = robot[i].secundary; // colocar em um vetor robot_list[2].orientation = robot[i].orientation; } - } } From 64de48b140e2a56a0004ec608c59689a490c0745 Mon Sep 17 00:00:00 2001 From: Pequi Date: Fri, 24 Feb 2017 17:41:12 +0000 Subject: [PATCH 11/20] BACK UP dos codigos dos robos --- Robot_Code/.gitkeep | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Robot_Code/.gitkeep diff --git a/Robot_Code/.gitkeep b/Robot_Code/.gitkeep new file mode 100644 index 00000000..e69de29b From 0d4472f67892ac9a619ffe5b8b2a07a8c5dce6f5 Mon Sep 17 00:00:00 2001 From: Pequi Date: Fri, 24 Feb 2017 17:41:46 +0000 Subject: [PATCH 12/20] robo 2016 --- Robot_Code/ROBO2016_V1.ino | 386 +++++++++++++++++++++++++++++++++++++ 1 file changed, 386 insertions(+) create mode 100644 Robot_Code/ROBO2016_V1.ino diff --git a/Robot_Code/ROBO2016_V1.ino b/Robot_Code/ROBO2016_V1.ino new file mode 100644 index 00000000..aacb1857 --- /dev/null +++ b/Robot_Code/ROBO2016_V1.ino @@ -0,0 +1,386 @@ +//Dados Fixos +#define ID 'A' //ID do Robo +#define I1A 5 //Motor da Direita +#define I1B 6 +#define I2A 11 //Motor da Esquerda +#define I2B 10 + +//Variáveis +String inputString = ""; +boolean stringComplete = false; +bool msgComplete = false; +bool myMessage = false; + +unsigned long previousMillis = 0; +unsigned long timer = 0; +double Dt = 0; + +double encoderRIGHT = 0; +double encoderLEFT = 0; + +int PWMs[2]; +String vel1 = ""; +String vel2 = ""; + +String KP = ""; +String KI = ""; +String KD = ""; + +double actualVelocity[] = {0.0, 0.0}; +double desiredVelocity[] = {0.0, 0.0}; + +bool calibrar = false; + +//Variáveis - Controle PID +double kp = 3; //...Constante PROPORCIONAL +double ki = 0; //...Constante INTEGRATIVA +double kd = 0; //...Constante DERIVATIVA +double accumulator[] = {0, 0}; +double lastError[] = {0, 0}; +double actualError[] = {0, 0}; +double P_Gain[] = {0, 0}; +double I_Gain[] = {0, 0}; +double D_Gain[] = {0, 0}; +double PID[] = {0, 0}; + +//Setup Principal - Sala da Justiça +void setup() { + + Serial.begin(57600); + Serial.print("Robo "); + Serial.print(ID); + Serial.println("- Iniciado"); + + //Reserva 200 bytes para a variavel inputString + inputString.reserve(200); + + pinMode(I1B, OUTPUT); + pinMode(I1A, OUTPUT); + pinMode(I2B, OUTPUT); + pinMode(I2A, OUTPUT); + + timer = millis(); + + //Ativa interrupcoes dos encoders + attachInterrupt(0, ENC_RIGHT, RISING); //Pin2 + attachInterrupt(1, ENC_LEFT, RISING); //Pin3 +} + +//Loop principal - Casa do Batman +void loop() { + + //Delay da zueira - So funciona com ele aqui + delay(1); + + // Serial.print(inputString); + // Serial.print(" ER - "); + // Serial.print(encoderRIGHT); + // Serial.print(" EL - "); + // Serial.print(encoderLEFT); + // Serial.print(" 0: "); + // Serial.print(actualVelocity[0]); + // Serial.print(" 1: "); + // Serial.print(actualVelocity[1]); + + if (stringComplete) { + //Serial.println(inputString); + if (calibrar) { + calibrarPID(inputString); + Serial.print("Robo "); + Serial.print(ID); + Serial.print(" - Kp = "); + Serial.print(kp); + Serial.print(" Ki = "); + Serial.print(ki); + Serial.print(" Kd = "); + Serial.print(kd); + Serial.println(" - Calibrado"); + + inputString = ""; + stringComplete = false; + } + else { + interpretaMSG(inputString); + CriaVELCITY(); + previousMillis = millis(); + inputString = ""; + stringComplete = false; + } + } + + if ((millis() - previousMillis) < 200) { + + PIDControler(); + if(desiredVelocity[0]==0) + PWMs[0] = 0; + if(desiredVelocity[1]==0) + PWMs[1] = 0; + Run(PWMs); + } + else { + Stop(); + } +} + +//Rotina para calibrar as constantes do PID +void calibrarPID(String msg) { + char floatbuf[32]; + int cont = 0; + + for (int j = 2; j < msg.length(); j++) + { + if (msg[j] != ';') + { + if (cont == 0) + KP += msg[j]; + if (cont == 1) + KI += msg[j]; + if (cont == 2) + KD += msg[j]; + } + else //Se encontrou um ';' significa que o primeiro pwm ja foi lido + cont++; + } + + //Zueira... Trasforma para vetor de char para poder usar função atof + KP.toCharArray(floatbuf, sizeof(floatbuf)); + kp = atof(floatbuf); + KI.toCharArray(floatbuf, sizeof(floatbuf)); + ki = atof(floatbuf); + KD.toCharArray(floatbuf, sizeof(floatbuf)); + kd = atof(floatbuf); + calibrar = false; + + KP = ""; + KI = ""; + KD = ""; +} + +//Controlador PID - O Zueiro +void PIDControler() { + + // Serial.print(" P_GAIN: "); + // Serial.print(actualError[0]); + // Serial.print(" desiredVelocity0: "); + // Serial.print(desiredVelocity[0]); + // Serial.print(" desiredVelocity1: "); + // Serial.println(desiredVelocity[1]); + // Serial.print(" actualVelocity0: "); + // Serial.print(actualVelocity[0]); + // Serial.print(" actualVelocity1: "); + // Serial.println(actualVelocity[1]); + // Serial.print(" actualError: "); + // Serial.print(actualError[0]); + // Serial.print(" lastError: "); + // Serial.println(lastError[0]); + + //Conrolador PROPORCIONAL - Contribui para a estabilidade, tem resposabilidade mediana + Amostra(); + actualError[0] = desiredVelocity[0] - (PWMs[0] / abs(PWMs[0])) * actualVelocity[0]; + actualError[1] = desiredVelocity[1] - (PWMs[1] / abs(PWMs[1])) * actualVelocity[1]; + P_Gain [0] = actualError[0] * kp; + P_Gain [1] = actualError[1] * kp; + + //Controlador INTEGRAL - Rapido tempo de subida, lenta resposta + accumulator[0] += actualError[0]; + accumulator[1] += actualError[1]; + I_Gain [0] = accumulator[0] * ki; + I_Gain [1] = accumulator[1] * ki; + + // Serial.print(" accumulator: "); + // Serial.print(accumulator[0]); + + //Controlador DERIVATIVO - Antecipa decisões, é rapido, sensivel a ruidos + D_Gain [0] = (actualError[0] - lastError[0]) * kd; + D_Gain [1] = (actualError[1] - lastError[1]) * kd; + lastError[0] = actualError[0]; + lastError[1] = actualError[1]; + + //Contolador PID Final.... Ganho em Velocidade -- (((((((CORRIGIDO PARA PWM)))))) + PID[0] = P_Gain[0] + I_Gain[0] + D_Gain[0]; + PID[1] = P_Gain[1] + I_Gain[1] + D_Gain[1]; + + // Serial.print(" P_GAIN: "); + // Serial.print(P_Gain[0]); + // Serial.print(" I_GAIN: "); + // Serial.print(I_Gain[0]); + // Serial.print(" D_GAIN: "); + // Serial.print(D_Gain[0]); + // Serial.print(" PID: "); + // Serial.print(PID[0]); + + //Corrigindo PWM + PWMs[0] += (int)PID[0]; + PWMs[1] += (int)PID[1]; + + //Evitar saturações + if (PWMs[0] > 255) PWMs[0] = 255; + if (PWMs[1] > 255) PWMs[1] = 255; + if (PWMs[0] < -255) PWMs[0] = -255; + if (PWMs[1] < -255) PWMs[1] = -255; +} + +//Verifica Serial toda vez que um loop é finalizado -> Loop menor = Leitura mais rápida +void serialEvent() { + + while (Serial.available()) { + char inChar = (char)Serial.read(); + // Serial.print(inChar); + if (inChar == ID || myMessage) { + + if (inChar == 'K') { + calibrar = true; + // Serial.println("Calibrando"); + } + + + myMessage = true; + + if (inChar == '#') { + myMessage = false; + stringComplete = true; + return; + } + inputString += inChar; + } + } + +} + +//Separa as velocidade pada cada motor +void interpretaMSG(String msg) { + + bool f = false; + + for (int j = 1; j < msg.length(); j++) + { + if (msg[j] != ';') + { + if (f == false) //Ainda esta lendo o primeiro pwm + vel1 += msg[j]; + else + vel2 += msg[j]; + } + else //Se encontrou um ';' significa que o primeiro pwm ja foi lido + f = true; + } +} + +//Converte PWM em inteiro e armazena no vetor PWMs[] +void CriaVELCITY() { + + char floatbuf[32]; + vel1.toCharArray(floatbuf, sizeof(floatbuf)); + desiredVelocity[0] = atof(floatbuf); + vel2.toCharArray(floatbuf, sizeof(floatbuf)); + desiredVelocity[1] = atof(floatbuf); + + vel1 = ""; + vel2 = ""; +} + +//Atualiza PWMs dos Motores +void Run(int PWMs[]) { + + int PWMA, PWMB; + PWMA = PWMs[0]; + PWMB = PWMs[1]; + + // Serial.print(" PWM_1: "); + // Serial.print(PWMs[0]); + // Serial.print(" PWM_2: "); + // Serial.println(PWMs[1]); + + if (PWMA < 0) { + analogWrite(I1A, -PWMA); //Seta Rotação Normal + digitalWrite(I1B, LOW); + } + else { + digitalWrite(I1A, LOW); //Seta Rotação Contrária + analogWrite(I1B, PWMA); + } + + if (PWMB < 0) { + analogWrite(I2A, -PWMB); //Seta Rotação Contrária + digitalWrite(I2B, LOW); + } + else { + digitalWrite(I2A, LOW); //Seta Rotação Contrária + analogWrite(I2B, PWMB); + } +} + +//Para os Motores +void Stop() +{ + analogWrite(I2A, 0); + digitalWrite(I2B, LOW); + analogWrite(I1A, 0); + digitalWrite(I1B, LOW); +} + +//Coleta a velocidade atual dos Motores +void Amostra() { + Dt = (millis() - timer); + + // Serial.print(" Dt: "); + // Serial.print(Dt); + // Serial.print(" eRIGHT: "); + // Serial.print(encoderRIGHT); + // Serial.print(" - eLEFT: "); + // Serial.println(encoderLEFT); + + actualVelocity[0] = 0; + actualVelocity[1] = 0; + + if (Dt != 0) { + actualVelocity[0] = 1000 * encoderRIGHT / (5 * 75 * Dt); //(5 - Numero de pulsos por volta) (75/1 - 1 volta da roda por 75 do motor) (1000 transformar em milisegundos para segundos) + actualVelocity[1] = 1000 * encoderLEFT / (5 * 75 * Dt); //(5 - Numero de pulsos por volta) (75/1 - 1 volta da roda por 75 do motor) (1000 transformar em milisegundos para segundos) + } + + // Serial.print(" DesiredVEL_1: "); + // Serial.print(desiredVelocity[0]); + // Serial.print(" DesiredVEL_2: "); + // Serial.print(desiredVelocity[1]); + // Serial.print(" ActualVEL_1: "); + // Serial.print(actualVelocity[0]); + // Serial.print(" ActualVEL_2: "); + // Serial.println(actualVelocity[1]); + + encoderRIGHT = 0; + encoderLEFT = 0; + + timer = millis(); +} + +//Conta pulsos do encoder da Direita ->INTERRUPÇÃO INT0 +void ENC_RIGHT() { + encoderRIGHT++; +} + +//Conta pulsos do encoder da Esquerda ->INTERRUPÇÃO INT1 +void ENC_LEFT() { + encoderLEFT++; +} + +/* Very Small @2015 - Núcleo de Robotica Pequi Mecânico - Universidade Federal de Goias + * + * Algoritimo utilizado nos robos: OJUARA, LENHADOR e GOLEIRO + * + * Utilizando: - Controlador PID + * - Comunicação sem fio através de Xbee S1 + * - Encoders nos motores + * - Padrao de Mensagem: ID+VelocityOne+;+VelocityTwo+# -> Exemplo: "A1.8;2.5#" + * + * Algoritimo desenvolvido pela equipe VS-2015: + * + * - Lucas da Silva Assis - Apolo Silva Marton + * - Vinicius Araújo Santos - Vinicius Paulo Lopes de Oliveira + * - Walisson Gobbo de Aguas - Kléber Macedo Cabral + * - Rafael Ribeiro Carvalho Vaz - Pedro Santos de Rezende Alves + * - Gabriel Pereira Lima Reis - Átila Gracco + * - Alexandre Monteiro Ribeiro - Victor Philipe Moreira dos Santos Camargo + * + *Todos os direitos reservados; + * +*/ From fa253a17f4ce77fe5dc4e4b4c0053bdd5b52d63a Mon Sep 17 00:00:00 2001 From: Pequi Date: Fri, 24 Feb 2017 17:42:26 +0000 Subject: [PATCH 13/20] robo 2015 --- Robot_Code/ROBO2015_V7.ino | 386 +++++++++++++++++++++++++++++++++++++ 1 file changed, 386 insertions(+) create mode 100644 Robot_Code/ROBO2015_V7.ino diff --git a/Robot_Code/ROBO2015_V7.ino b/Robot_Code/ROBO2015_V7.ino new file mode 100644 index 00000000..d5b94a07 --- /dev/null +++ b/Robot_Code/ROBO2015_V7.ino @@ -0,0 +1,386 @@ +//Dados Fixos +#define ID 'C' //ID do Robo +#define I1A 5 //Motor da Direita +#define I1B 6 +#define I2A 11 //Motor da Esquerda +#define I2B 10 + +//Variáveis +String inputString = ""; +boolean stringComplete = false; +bool msgComplete = false; +bool myMessage = false; + +unsigned long previousMillis = 0; +unsigned long timer = 0; +double Dt = 0; + +double encoderRIGHT = 0; +double encoderLEFT = 0; + +int PWMs[2]; +String vel1 = ""; +String vel2 = ""; + +String KP = ""; +String KI = ""; +String KD = ""; + +double actualVelocity[] = {0.0, 0.0}; +double desiredVelocity[] = {0.0, 0.0}; + +bool calibrar = false; + +//Variáveis - Controle PID +double kp = 3; //...Constante PROPORCIONAL +double ki = 0; //...Constante INTEGRATIVA +double kd = 0; //...Constante DERIVATIVA +double accumulator[] = {0, 0}; +double lastError[] = {0, 0}; +double actualError[] = {0, 0}; +double P_Gain[] = {0, 0}; +double I_Gain[] = {0, 0}; +double D_Gain[] = {0, 0}; +double PID[] = {0, 0}; + +//Setup Principal - Sala da Justiça +void setup() { + + Serial.begin(57600); + Serial.print("Robo "); + Serial.print(ID); + Serial.println("- Iniciado"); + + //Reserva 200 bytes para a variavel inputString + inputString.reserve(200); + + pinMode(I1B, OUTPUT); + pinMode(I1A, OUTPUT); + pinMode(I2B, OUTPUT); + pinMode(I2A, OUTPUT); + + timer = millis(); + + //Ativa interrupcoes dos encoders + attachInterrupt(0, ENC_RIGHT, RISING); //Pin2 + attachInterrupt(1, ENC_LEFT, RISING); //Pin3 +} + +//Loop principal - Casa do Batman +void loop() { + + //Delay da zueira - So funciona com ele aqui + delay(1); + + // Serial.print(inputString); + // Serial.print(" ER - "); + // Serial.print(encoderRIGHT); + // Serial.print(" EL - "); + // Serial.print(encoderLEFT); + // Serial.print(" 0: "); + // Serial.print(actualVelocity[0]); + // Serial.print(" 1: "); + // Serial.print(actualVelocity[1]); + + if (stringComplete) { + //Serial.println(inputString); + if (calibrar) { + calibrarPID(inputString); + Serial.print("Robo "); + Serial.print(ID); + Serial.print(" - Kp = "); + Serial.print(kp); + Serial.print(" Ki = "); + Serial.print(ki); + Serial.print(" Kd = "); + Serial.print(kd); + Serial.println(" - Calibrado"); + + inputString = ""; + stringComplete = false; + } + else { + interpretaMSG(inputString); + CriaVELCITY(); + previousMillis = millis(); + inputString = ""; + stringComplete = false; + } + } + + if ((millis() - previousMillis) < 100) { + + PIDControler(); + if(desiredVelocity[0]==0) + PWMs[0] = 0; + if(desiredVelocity[1]==0) + PWMs[1] = 0; + Run(PWMs); + } + else { + Stop(); + } +} + +//Rotina para calibrar as constantes do PID +void calibrarPID(String msg) { + char floatbuf[32]; + int cont = 0; + + for (int j = 2; j < msg.length(); j++) + { + if (msg[j] != ';') + { + if (cont == 0) + KP += msg[j]; + if (cont == 1) + KI += msg[j]; + if (cont == 2) + KD += msg[j]; + } + else //Se encontrou um ';' significa que o primeiro pwm ja foi lido + cont++; + } + + //Zueira... Trasforma para vetor de char para poder usar função atof + KP.toCharArray(floatbuf, sizeof(floatbuf)); + kp = atof(floatbuf); + KI.toCharArray(floatbuf, sizeof(floatbuf)); + ki = atof(floatbuf); + KD.toCharArray(floatbuf, sizeof(floatbuf)); + kd = atof(floatbuf); + calibrar = false; + + KP = ""; + KI = ""; + KD = ""; +} + +//Controlador PID - O Zueiro +void PIDControler() { + + // Serial.print(" P_GAIN: "); + // Serial.print(actualError[0]); + // Serial.print(" desiredVelocity0: "); + // Serial.print(desiredVelocity[0]); + // Serial.print(" desiredVelocity1: "); + // Serial.println(desiredVelocity[1]); + // Serial.print(" actualVelocity0: "); + // Serial.print(actualVelocity[0]); + // Serial.print(" actualVelocity1: "); + // Serial.println(actualVelocity[1]); + // Serial.print(" actualError: "); + // Serial.print(actualError[0]); + // Serial.print(" lastError: "); + // Serial.println(lastError[0]); + + //Conrolador PROPORCIONAL - Contribui para a estabilidade, tem resposabilidade mediana + Amostra(); + actualError[0] = desiredVelocity[0] - (PWMs[0] / abs(PWMs[0])) * actualVelocity[0]; + actualError[1] = desiredVelocity[1] - (PWMs[1] / abs(PWMs[1])) * actualVelocity[1]; + P_Gain [0] = actualError[0] * kp; + P_Gain [1] = actualError[1] * kp; + + //Controlador INTEGRAL - Rapido tempo de subida, lenta resposta + accumulator[0] += actualError[0]; + accumulator[1] += actualError[1]; + I_Gain [0] = accumulator[0] * ki; + I_Gain [1] = accumulator[1] * ki; + + // Serial.print(" accumulator: "); + // Serial.print(accumulator[0]); + + //Controlador DERIVATIVO - Antecipa decisões, é rapido, sensivel a ruidos + D_Gain [0] = (actualError[0] - lastError[0]) * kd; + D_Gain [1] = (actualError[1] - lastError[1]) * kd; + lastError[0] = actualError[0]; + lastError[1] = actualError[1]; + + //Contolador PID Final.... Ganho em Velocidade -- (((((((CORRIGIDO PARA PWM)))))) + PID[0] = P_Gain[0] + I_Gain[0] + D_Gain[0]; + PID[1] = P_Gain[1] + I_Gain[1] + D_Gain[1]; + + // Serial.print(" P_GAIN: "); + // Serial.print(P_Gain[0]); + // Serial.print(" I_GAIN: "); + // Serial.print(I_Gain[0]); + // Serial.print(" D_GAIN: "); + // Serial.print(D_Gain[0]); + // Serial.print(" PID: "); + // Serial.print(PID[0]); + + //Corrigindo PWM + PWMs[0] += (int)PID[0]; + PWMs[1] += (int)PID[1]; + + //Evitar saturações + if (PWMs[0] > 255) PWMs[0] = 255; + if (PWMs[1] > 255) PWMs[1] = 255; + if (PWMs[0] < -255) PWMs[0] = -255; + if (PWMs[1] < -255) PWMs[1] = -255; +} + +//Verifica Serial toda vez que um loop é finalizado -> Loop menor = Leitura mais rápida +void serialEvent() { + + while (Serial.available()) { + char inChar = (char)Serial.read(); + // Serial.print(inChar); + if (inChar == ID || myMessage) { + + if (inChar == 'K') { + calibrar = true; + // Serial.println("Calibrando"); + } + + + myMessage = true; + + if (inChar == '#') { + myMessage = false; + stringComplete = true; + return; + } + inputString += inChar; + } + } + +} + +//Separa as velocidade pada cada motor +void interpretaMSG(String msg) { + + bool f = false; + + for (int j = 1; j < msg.length(); j++) + { + if (msg[j] != ';') + { + if (f == false) //Ainda esta lendo o primeiro pwm + vel1 += msg[j]; + else + vel2 += msg[j]; + } + else //Se encontrou um ';' significa que o primeiro pwm ja foi lido + f = true; + } +} + +//Converte PWM em inteiro e armazena no vetor PWMs[] +void CriaVELCITY() { + + char floatbuf[32]; + vel1.toCharArray(floatbuf, sizeof(floatbuf)); + desiredVelocity[0] = atof(floatbuf); + vel2.toCharArray(floatbuf, sizeof(floatbuf)); + desiredVelocity[1] = atof(floatbuf); + + vel1 = ""; + vel2 = ""; +} + +//Atualiza PWMs dos Motores +void Run(int PWMs[]) { + + int PWMA, PWMB; + PWMA = PWMs[0]; + PWMB = PWMs[1]; + + // Serial.print(" PWM_1: "); + // Serial.print(PWMs[0]); + // Serial.print(" PWM_2: "); + // Serial.println(PWMs[1]); + + if (PWMA < 0) { + analogWrite(I1A, -PWMA); //Seta Rotação Normal + digitalWrite(I1B, LOW); + } + else { + digitalWrite(I1A, LOW); //Seta Rotação Contrária + analogWrite(I1B, PWMA); + } + + if (PWMB < 0) { + analogWrite(I2A, -PWMB); //Seta Rotação Contrária + digitalWrite(I2B, LOW); + } + else { + digitalWrite(I2A, LOW); //Seta Rotação Contrária + analogWrite(I2B, PWMB); + } +} + +//Para os Motores +void Stop() +{ + analogWrite(I2A, 0); + digitalWrite(I2B, LOW); + analogWrite(I1A, 0); + digitalWrite(I1B, LOW); +} + +//Coleta a velocidade atual dos Motores +void Amostra() { + Dt = (millis() - timer); + + // Serial.print(" Dt: "); + // Serial.print(Dt); + // Serial.print(" eRIGHT: "); + // Serial.print(encoderRIGHT); + // Serial.print(" - eLEFT: "); + // Serial.println(encoderLEFT); + + actualVelocity[0] = 0; + actualVelocity[1] = 0; + + if (Dt != 0) { + actualVelocity[0] = 1000 * encoderRIGHT / (5 * 75 * Dt); //(5 - Numero de pulsos por volta) (75/1 - 1 volta da roda por 75 do motor) (1000 transformar em milisegundos para segundos) + actualVelocity[1] = 1000 * encoderLEFT / (5 * 75 * Dt); //(5 - Numero de pulsos por volta) (75/1 - 1 volta da roda por 75 do motor) (1000 transformar em milisegundos para segundos) + } + + // Serial.print(" DesiredVEL_1: "); + // Serial.print(desiredVelocity[0]); + // Serial.print(" DesiredVEL_2: "); + // Serial.print(desiredVelocity[1]); + // Serial.print(" ActualVEL_1: "); + // Serial.print(actualVelocity[0]); + // Serial.print(" ActualVEL_2: "); + // Serial.println(actualVelocity[1]); + + encoderRIGHT = 0; + encoderLEFT = 0; + + timer = millis(); +} + +//Conta pulsos do encoder da Direita ->INTERRUPÇÃO INT0 +void ENC_RIGHT() { + encoderRIGHT++; +} + +//Conta pulsos do encoder da Esquerda ->INTERRUPÇÃO INT1 +void ENC_LEFT() { + encoderLEFT++; +} + +/* Very Small @2015 - Núcleo de Robotica Pequi Mecânico - Universidade Federal de Goias + * + * Algoritimo utilizado nos robos: OJUARA, LENHADOR e GOLEIRO + * + * Utilizando: - Controlador PID + * - Comunicação sem fio através de Xbee S1 + * - Encoders nos motores + * - Padrao de Mensagem: ID+VelocityOne+;+VelocityTwo+# -> Exemplo: "A1.8;2.5#" + * + * Algoritimo desenvolvido pela equipe VS-2015: + * + * - Lucas da Silva Assis - Apolo Silva Marton + * - Vinicius Araújo Santos - Vinicius Paulo Lopes de Oliveira + * - Walisson Gobbo de Aguas - Kléber Macedo Cabral + * - Rafael Ribeiro Carvalho Vaz - Pedro Santos de Rezende Alves + * - Gabriel Pereira Lima Reis - Átila Gracco + * - Alexandre Monteiro Ribeiro - Victor Philipe Moreira dos Santos Camargo + * + *Todos os direitos reservados; + * +*/ From e2d34a41816076045da69a0978d81d1c70a9f4a9 Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 27 Feb 2017 12:10:52 -0300 Subject: [PATCH 14/20] Tentativa de fazer com que ao iniciar o jogo, o PID Test seja desligado automaticamente. --- cc/camcap.hpp | 10 +++++++--- cc/controlGUI.hpp | 13 ++++++++++--- pack-capture-gui/capture-gui/V4LInterface-aux.cpp | 5 +++++ pack-capture-gui/capture-gui/V4LInterface.hpp | 1 + 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/cc/camcap.hpp b/cc/camcap.hpp index 5e0887ce..45c99d13 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -69,8 +69,8 @@ class CamCap: Robot robot; cv::Point ballPosition; // cout<<"AQUI"<robot_creation_unitag(); - //vision->robot_creation(); + //vision->robot_creation_unitag(); + vision->robot_creation(); // cout<<"AQUI"<get_robot_list_size(); i++) { @@ -155,6 +155,10 @@ class CamCap: bool capture_and_show() { if (!data) return false; + if (interface.get_start_game_flag()) + control.set_PID_test_flag(false); + + //timer.start(); interface.vcap.grab_rgb(data); interface.imageView.set_data(data, width, height); @@ -494,7 +498,7 @@ class CamCap: fm.add(interface.imageView); notebook.append_page(interface, "Vision"); notebook.append_page(control, "Control"); - notebook.append_page(strategyGUI, "strategyGUI"); + notebook.append_page(strategyGUI, "Strategy"); for(int i=0; i<4; i++) { diff --git a/cc/controlGUI.hpp b/cc/controlGUI.hpp index d96e5f0a..a830b77b 100644 --- a/cc/controlGUI.hpp +++ b/cc/controlGUI.hpp @@ -33,7 +33,6 @@ class ControlGUI: public Gtk::VBox // Flag para saber se o botão PID está pressionado ou não. bool Serial_Enabled; - bool PID_bt_event_flag = false; bool PID_test_flag = false; // Containers para o conteúdo da interface gráfica Gtk::Frame Serial_fm; @@ -64,10 +63,18 @@ class ControlGUI: public Gtk::VBox Gtk::ComboBoxText cb_pid; bool pid_edit_flag = false; + bool get_PID_test_flag() + { + return PID_test_flag; + } + + void set_PID_test_flag(bool input) + { + PID_test_flag = input; + } + ControlGUI() { - PID_bt_event_flag=false; - PID_bt_event_flag =false; Serial_Enabled=false; // Adicionar o frame do Serial e sua VBOX pack_start(Top_hbox, false, true, 5); diff --git a/pack-capture-gui/capture-gui/V4LInterface-aux.cpp b/pack-capture-gui/capture-gui/V4LInterface-aux.cpp index c6f24daf..97c4de43 100644 --- a/pack-capture-gui/capture-gui/V4LInterface-aux.cpp +++ b/pack-capture-gui/capture-gui/V4LInterface-aux.cpp @@ -30,6 +30,11 @@ namespace capture } + bool V4LInterface::get_start_game_flag() + { + return start_game_flag; + } + void V4LInterface::HScale_Amin_value_changed() { Amin[Img_id]=HScale_Amin.get_value(); diff --git a/pack-capture-gui/capture-gui/V4LInterface.hpp b/pack-capture-gui/capture-gui/V4LInterface.hpp index 0056f3e2..630e8018 100644 --- a/pack-capture-gui/capture-gui/V4LInterface.hpp +++ b/pack-capture-gui/capture-gui/V4LInterface.hpp @@ -219,6 +219,7 @@ class V4LInterface: public Gtk::VBox { void updateRobotLabels(); void init_HSV(); + bool get_start_game_flag(); public: From 12b807ca73c6e4562fe36f4112588fa91880c460 Mon Sep 17 00:00:00 2001 From: pqvs Date: Mon, 27 Feb 2017 14:58:27 -0300 Subject: [PATCH 15/20] =?UTF-8?q?Ao=20come=C3=A7ar=20o=20jogo,=20o=20PID?= =?UTF-8?q?=5Ftest=20=C3=A9=20desligado.=20BUG:=20ao=20pausar=20jogo,=20o?= =?UTF-8?q?=20PID=5Ftest=20n=C3=A3o=20volta=20a=20funcionar=20normalmente.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CAM_quicksave.txt | 4 +-- cc/camcap.hpp | 29 ++++++++++++++-------- cc/vision.hpp | 2 +- pack-capture-gui/capture-gui/ImageView.hpp | 12 ++------- 4 files changed, 24 insertions(+), 23 deletions(-) diff --git a/CAM_quicksave.txt b/CAM_quicksave.txt index 1ebe2c4e..573954ed 100644 --- a/CAM_quicksave.txt +++ b/CAM_quicksave.txt @@ -1,9 +1,9 @@ 9963776 -153 +129 9963777 34 9963778 -56 +32 9963788 0 9963795 diff --git a/cc/camcap.hpp b/cc/camcap.hpp index 45c99d13..a4dae65e 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -52,9 +52,6 @@ class CamCap: int w, h; CPUTimer timer; - - - Gtk::Frame fm; Gtk::Frame info_fm; Gtk::VBox camera_vbox; @@ -69,8 +66,8 @@ class CamCap: Robot robot; cv::Point ballPosition; // cout<<"AQUI"<robot_creation_unitag(); - vision->robot_creation(); + vision->robot_creation_unitag(); + //vision->robot_creation(); // cout<<"AQUI"<get_robot_list_size(); i++) { @@ -147,7 +144,7 @@ class CamCap: } - //interface.__event_bt_quick_load_clicked(); + interface.__event_bt_quick_load_clicked(); return true; } @@ -155,8 +152,8 @@ class CamCap: bool capture_and_show() { if (!data) return false; - if (interface.get_start_game_flag()) - control.set_PID_test_flag(false); + + //timer.start(); @@ -182,6 +179,7 @@ class CamCap: interface.imageView.hold_warp=false; } + interface.imageView.PID_test_flag = control.PID_test_flag; interface.imageView.adjust_event_flag = interface.adjust_event_flag; @@ -238,7 +236,12 @@ class CamCap: } - if(interface.imageView.PID_test_flag) PID_test(); + if(interface.imageView.PID_test_flag && !interface.get_start_game_flag()) + { + control.button_PID_Test.set_active(true); + PID_test(); + } + else { for(int i=0; ibutton == 1) @@ -96,16 +98,6 @@ namespace capture { } - if (auto_calib_flag){ - if (event->button == 1) { - pointClicked = cv::Point((int)event->x,(int)event->y); - std::cout<<"("<<(int)event->x<<","<<(int)event->y<<")"< Date: Wed, 1 Mar 2017 15:25:50 -0300 Subject: [PATCH 16/20] Criado o Uni duni t-ag --- CAM_quicksave.txt | 10 +- cc/camcap.hpp | 7 +- cc/vision.hpp | 113 ++++++++++++++++++ pack-capture-gui/capture-gui/Robot.hpp | 5 +- .../capture-gui/V4LInterface-events.cpp | 1 + 5 files changed, 127 insertions(+), 9 deletions(-) diff --git a/CAM_quicksave.txt b/CAM_quicksave.txt index 573954ed..892c97fb 100644 --- a/CAM_quicksave.txt +++ b/CAM_quicksave.txt @@ -1,13 +1,13 @@ 9963776 -129 +146 9963777 -34 +41 9963778 -32 +54 9963788 0 9963795 -18 +26 9963800 2 9963802 @@ -19,7 +19,7 @@ 10094849 1 10094850 -310 +173 10094851 0 10094856 diff --git a/cc/camcap.hpp b/cc/camcap.hpp index a4dae65e..4d340169 100644 --- a/cc/camcap.hpp +++ b/cc/camcap.hpp @@ -66,9 +66,11 @@ class CamCap: Robot robot; cv::Point ballPosition; // cout<<"AQUI"<robot_creation_unitag(); + //vision->robot_creation_unitag(); //vision->robot_creation(); + vision->robot_creation_uni_duni_tag(); // cout<<"AQUI"<get_robot_list_size(); i++) { robot = vision->get_robot_from_list(i); @@ -98,6 +100,7 @@ class CamCap: interface.imageView.disable_image_show(); free(data); data = 0; + } @@ -256,7 +259,7 @@ class CamCap: if(Selec_index!=-1) { circle(imageView,interface.robot_list[Selec_index].position, 17, cv::Scalar(255,255,255), 2); } - + for(int i=0; i robot; + Robot r; + + double omax = -99999; // angulo maximo + double omin = 99999; // angulo minimo + // cout<<"1"<Team_Sec_area[index2[0]][index2[1]]) { + robot[l].secundary = Team_Sec[index1[0]][index1[1]]; + robot[l].ternary = Team_Sec[index2[0]][index2[1]]; + + // cout<<"Area 1 = Secundary"<<" Area 2 = Ternary"<=35*PI/180) { + + robot_list[0].position = robot[l].position; // colocar em um vetor + robot_list[0].secundary = robot[l].secundary; // colocar em um vetor + robot_list[0].orientation = robot[l].orientation; + + }else if(o<=-35*PI/180) { + + robot_list[1].position = robot[l].position; // colocar em um vetor + robot_list[1].secundary = robot[l].secundary; // colocar em um vetor + robot_list[1].orientation = robot[l].orientation; + + } + + l==3? : l++; + + } + } + // cout<<"4"< Date: Wed, 1 Mar 2017 16:21:53 -0300 Subject: [PATCH 17/20] =?UTF-8?q?Correga=C3=A7=C3=A3o=20Uni=20duni=20tag?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CAM_quicksave.txt | 4 ++++ cc/vision.hpp | 7 ++++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/CAM_quicksave.txt b/CAM_quicksave.txt index 892c97fb..cddc21d0 100644 --- a/CAM_quicksave.txt +++ b/CAM_quicksave.txt @@ -32,3 +32,7 @@ 0 10094861 1 +168062213 +3 +168062214 +0 diff --git a/cc/vision.hpp b/cc/vision.hpp index a18740d1..6f98ca17 100644 --- a/cc/vision.hpp +++ b/cc/vision.hpp @@ -436,19 +436,19 @@ class Vision o = atan2(sin(robot[l].orientation2-robot[l].orientation+3.1415), cos(robot[l].orientation2-robot[l].orientation+3.1415)); - // cout<=35*PI/180) { + }else if(o>0) { robot_list[0].position = robot[l].position; // colocar em um vetor robot_list[0].secundary = robot[l].secundary; // colocar em um vetor robot_list[0].orientation = robot[l].orientation; - }else if(o<=-35*PI/180) { + }else { robot_list[1].position = robot[l].position; // colocar em um vetor robot_list[1].secundary = robot[l].secundary; // colocar em um vetor @@ -459,6 +459,7 @@ class Vision l==3? : l++; } + cout< Date: Sat, 4 Mar 2017 13:32:05 -0300 Subject: [PATCH 18/20] =?UTF-8?q?Tirado=20a=20frame=20de=20configura=C3=A7?= =?UTF-8?q?=C3=A3o=20do=20PID?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CAM_quicksave.txt | 12 ++++-------- cc/controlGUI.hpp | 2 +- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/CAM_quicksave.txt b/CAM_quicksave.txt index cddc21d0..32278423 100644 --- a/CAM_quicksave.txt +++ b/CAM_quicksave.txt @@ -1,9 +1,9 @@ 9963776 -146 +128 9963777 -41 +32 9963778 -54 +32 9963788 0 9963795 @@ -27,12 +27,8 @@ 10094857 0 10094858 -0 +68 10094860 0 10094861 1 -168062213 -3 -168062214 -0 diff --git a/cc/controlGUI.hpp b/cc/controlGUI.hpp index a830b77b..dd9175d7 100644 --- a/cc/controlGUI.hpp +++ b/cc/controlGUI.hpp @@ -129,7 +129,7 @@ class ControlGUI: public Gtk::VBox bt_Serial_test.set_label("Send"); - _create_pid_frame(); + //_create_pid_frame(); _update_cb_serial(); // Conectar os sinais para o acontecimento dos eventos From 36151e7d11b1512caab0cacace86105e92a7b58b Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 7 Mar 2017 14:34:30 -0300 Subject: [PATCH 19/20] Corrigindo leitoagens e fazendo ajustes --- CAM_quicksave.txt | 14 +++++++------- cc/Strategy.hpp | 2 +- cc/vision.hpp | 4 ++-- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/CAM_quicksave.txt b/CAM_quicksave.txt index 32278423..2e7e0fd1 100644 --- a/CAM_quicksave.txt +++ b/CAM_quicksave.txt @@ -1,17 +1,17 @@ 9963776 -128 +156 9963777 -32 +50 9963778 -32 +55 9963788 0 9963795 -26 +4 9963800 2 9963802 -2800 +5698 9963803 255 9963804 @@ -19,7 +19,7 @@ 10094849 1 10094850 -173 +411 10094851 0 10094856 @@ -27,7 +27,7 @@ 10094857 0 10094858 -68 +0 10094860 0 10094861 diff --git a/cc/Strategy.hpp b/cc/Strategy.hpp index 4069a354..6266996a 100644 --- a/cc/Strategy.hpp +++ b/cc/Strategy.hpp @@ -84,7 +84,7 @@ class Strategy LIMITE_AREA_X = 113; BANHEIRA = round((0.50*COMPRIMENTO_CAMPO_TOTAL))+round(0.16*float(width)/1.70); - DIVISAO_AREAS = round((0.50*COMPRIMENTO_CAMPO_TOTAL)); + DIVISAO_AREAS = round((0.50*COMPRIMENTO_CAMPO_TOTAL) - 10); // O valor negativo é um offset para não ficar exatamente no meio. OFFSET_BANHEIRA= round(0.20*float(width)/1.70); MEIO_GOL_X = round(COMPRIMENTO_CAMPO_TOTAL-round(0.05*float(width)/1.70)); MEIO_GOL_Y = round(LARGURA_CAMPO/2); diff --git a/cc/vision.hpp b/cc/vision.hpp index 6f98ca17..14eeeb11 100644 --- a/cc/vision.hpp +++ b/cc/vision.hpp @@ -436,7 +436,7 @@ class Vision o = atan2(sin(robot[l].orientation2-robot[l].orientation+3.1415), cos(robot[l].orientation2-robot[l].orientation+3.1415)); - cout<<"Robot "< Date: Thu, 9 Mar 2017 20:15:54 -0300 Subject: [PATCH 20/20] =?UTF-8?q?Corre=C3=A7=C3=A3o=20de=20um=20bug=20da?= =?UTF-8?q?=20interface?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CAM_quicksave.txt | 4 ++-- pack-capture-gui/capture-gui/V4LInterface-events.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CAM_quicksave.txt b/CAM_quicksave.txt index 2e7e0fd1..7d1d8f2b 100644 --- a/CAM_quicksave.txt +++ b/CAM_quicksave.txt @@ -23,9 +23,9 @@ 10094851 0 10094856 -0 +18000 10094857 -0 +28800 10094858 0 10094860 diff --git a/pack-capture-gui/capture-gui/V4LInterface-events.cpp b/pack-capture-gui/capture-gui/V4LInterface-events.cpp index 60b77c4e..33a3fed8 100644 --- a/pack-capture-gui/capture-gui/V4LInterface-events.cpp +++ b/pack-capture-gui/capture-gui/V4LInterface-events.cpp @@ -757,7 +757,7 @@ void V4LInterface::__event_bt_left_HSV_calib_clicked() { HScale_Amin.set_value(Amin[Img_id]); switch(Img_id) { case 0: - HSV_label.set_text("Primaria"); + HSV_label.set_text("Main"); HScale_Hmin.set_value(H[Img_id][0]); HScale_Hmax.set_value(H[Img_id][1]);