diff --git a/CAM_quicksave.txt b/CAM_quicksave.txt index 56298aa5..7d1d8f2b 100644 --- a/CAM_quicksave.txt +++ b/CAM_quicksave.txt @@ -1,22 +1,34 @@ 9963776 -71 +156 9963777 -48 -9963778 -38 -9963779 50 +9963778 +55 9963788 0 -9963792 -50 +9963795 +4 9963800 2 9963802 -2800 +5698 9963803 -100 +255 9963804 0 +10094849 +1 +10094850 +411 10094851 +0 +10094856 +18000 +10094857 +28800 +10094858 +0 +10094860 +0 +10094861 1 diff --git a/cc/vision.cpp b/Robot_Code/.gitkeep similarity index 100% rename from cc/vision.cpp rename to Robot_Code/.gitkeep 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; + * +*/ 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; + * +*/ diff --git a/cc/Strategy.hpp b/cc/Strategy.hpp index 480d8e92..6266996a 100644 --- a/cc/Strategy.hpp +++ b/cc/Strategy.hpp @@ -1,4 +1,4 @@ - #ifndef STRATEGY_HPP_ +#ifndef STRATEGY_HPP_ #define STRATEGY_HPP_ #define PI 3.14159265453 #include "opencv2/opencv.hpp" @@ -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) - 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); + 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 @@ -29,44 +30,27 @@ #include #include #include "CPUTimer.cpp" -#include "Strategy.hpp" -boost::mutex io_mutex; -boost::thread_group threshold_threads; -struct ImageGray; + +Vision *vision; class CamCap: public Gtk::HBox { public: - Strategy strats; + cv::Mat image_copy; cv::Point Ball_Est; - StrategyGUI strategy; + StrategyGUI strategyGUI; ControlGUI control; - capture::V4LInterface v; + capture::V4LInterface interface; + 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; - - Gtk::Frame fm; Gtk::Frame info_fm; @@ -76,151 +60,77 @@ 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(); + vision->robot_creation_uni_duni_tag(); + // cout<<"AQUI"<get_robot_list_size(); i++) + { + 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; + } + + 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); + interface.robot_list[2].feedHist(interface.robot_list[2].position); + + interface.updateRobotLabels(); } bool start_signal(bool b) { + if (b) { cout << "Start Clicked!" << endl; if (data) { - v.iv.disable_image_show(); + interface.imageView.disable_image_show(); 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) + 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; - 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; - 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); - - 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)); + interface.__event_bt_quick_load_clicked(); return true; } @@ -299,361 +155,246 @@ class CamCap: bool capture_and_show() { 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); + + + interface.imageView.refresh(); + d = interface.imageView.get_data(); + w = interface.imageView.get_width(); + h = interface.imageView.get_height(); - v.iv.refresh(); - d = v.iv.get_data(); - w = v.iv.get_width(); - h = v.iv.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.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; + 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); } } - // RESOLVER - - if(v.auto_calib_flag) { - //cout<<"----------------------------------------"<setHSV(interface.H,interface.S,interface.V,interface.Amin); + //TRACKING CAMERA + + + vision->parallel_tracking(imageView); - //cout<<"AQUI"<(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); */ - // strats.set_Ball(Ball); - //strats.set_Ball_Est(center); - for(int i=0; iget_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); } } - if(v.iv.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; iget_ball_position()); + + 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); + 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 = strats.get_gk_target(Adv_Main); - v.robot_list[i].fixedPos = strats.Goalkeeper.fixedPos; - if(strats.GOAL_DANGER_ZONE) { - // cout<<"hist_wipe"<Adv_Main); + interface.robot_list[i].fixedPos = strategyGUI.strategy.Goalkeeper.fixedPos; + if(strategyGUI.strategy.GOAL_DANGER_ZONE) { + interface.robot_list[i].histWipe(); } break; case 2: - v.robot_list[i].target = strats.get_atk_target(v.robot_list[i].position, v.robot_list[i].orientation); - v.robot_list[i].fixedPos = strats.Attack.fixedPos; - v.robot_list[i].status = strats.Attack.status; - /* for(int j=0;jAdv_Main.size();j++){ + 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 = strats.get_def_target(v.robot_list[i].position); - v.robot_list[i].fixedPos = strats.Defense.fixedPos; - v.robot_list[i].status = strats.Defense.status; - /* for(int j=0;jAdv_Main.size();j++){ + 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 = strats.get_opp_target(v.robot_list[i].position, v.robot_list[i].orientation); - v.robot_list[i].fixedPos = strats.Opponent.fixedPos; - v.robot_list[i].status = 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[interface.Img_id][i]; } return true; } void send_vel_to_robots() { - for(int i=0; iget_ball_position()); } 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() { + if (interface.get_start_game_flag()) return; + 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((Ball.x-v.robot_list[Selec_index].target.x),2)+pow((Ball.y-v.robot_list[Selec_index].target.y),2))<=7) + interface.robot_list[Selec_index].histWipe(); + 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]) - v.robot_list[Selec_index].target=Ball; + interface.robot_list[Selec_index].target=vision->get_ball_position(); 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; iget_ball_position(); 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,Ball); + 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->get_ball_position()); } 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 ; } } @@ -663,464 +404,97 @@ class CamCap: } - 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 + 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); - v.robot_list[1].secundary = robot[i].secundary; // colocar em um vetor - v.robot_list[1].orientation = robot[i].orientation; + 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 (strategyGUI.get_banheira_flag()) + line(imageView,cv::Point(strategyGUI.strategy.BANHEIRA, 0),cv::Point(strategyGUI.strategy.BANHEIRA, h),cv::Scalar(255,255,0), 2); - 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()); + 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 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){ + 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; } } @@ -1132,31 +506,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, "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; + interface.imageView.adjust_mat[i][0] = -1; + interface.imageView.adjust_mat[i][1] = -1; } @@ -1164,25 +523,19 @@ 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); - if (threshold != NULL) - { - for(int i = 0; i < 6; i++) - free(threshold[i]); - free(threshold); - } data = 0; } diff --git a/cc/controlGUI.hpp b/cc/controlGUI.hpp index 78a2702c..dd9175d7 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; @@ -54,10 +53,28 @@ class ControlGUI: public Gtk::VBox Gtk::Entry Tbox_V1; Gtk::Entry Tbox_V2; + Gtk::Frame pid_fm; + Gtk::VBox pid_vbox; + Gtk::HBox pid_hbox[2]; + Gtk::Button pid_edit_bt; + Gtk::Button pid_done_bt; + Gtk::Entry pid_box[3]; + Glib::ustring pid_tmp[4]; + 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); @@ -112,6 +129,8 @@ class ControlGUI: public Gtk::VBox bt_Serial_test.set_label("Send"); + //_create_pid_frame(); + _update_cb_serial(); // Conectar os sinais para o acontecimento dos eventos button_PID_Test.signal_pressed().connect(sigc::mem_fun(*this, &ControlGUI::_PID_Test)); @@ -270,6 +289,190 @@ for(int i = 0; i < 256; ++i) } +void _create_pid_frame(){ + + pack_start(pid_fm, false, true, 5); + pid_fm.set_label("PID"); + pid_fm.add(pid_vbox); + pid_vbox.pack_start(pid_hbox[0], false, true, 5); + pid_vbox.pack_start(pid_hbox[1], false, true, 5); + + pid_hbox[0].pack_start(pid_edit_bt, false, true, 5); + pid_hbox[0].pack_end(pid_done_bt, false, true, 5); + pid_edit_bt.set_label("Edit"); + pid_done_bt.set_label("Done"); + + cb_pid.append("All Robots"); + cb_pid.append("Robot A"); + cb_pid.append("Robot B"); + cb_pid.append("Robot C"); + cb_pid.append("Robot D"); + cb_pid.set_active(0); + + pid_hbox[1].pack_start(cb_pid, false, true, 5); + label = new Gtk::Label("P: "); + pid_hbox[1].pack_start(*label, false, true, 5); + pid_hbox[1].pack_start(pid_box[0], false, true, 5); + pid_box[0].set_max_length(5); + pid_box[0].set_width_chars(6); + pid_box[0].set_text(Glib::ustring::format("0")); + + label = new Gtk::Label("I: "); + pid_hbox[1].pack_start(*label, false, true, 5); + pid_hbox[1].pack_start(pid_box[1], false, true, 5); + pid_box[1].set_max_length(5); + pid_box[1].set_width_chars(6); + pid_box[1].set_text(Glib::ustring::format("0")); + + label = new Gtk::Label("D: "); + pid_hbox[1].pack_start(*label, false, true, 5); + pid_hbox[1].pack_start(pid_box[2], false, true, 5); + pid_box[2].set_max_length(5); + pid_box[2].set_width_chars(6); + pid_box[2].set_text(Glib::ustring::format("0")); + + /*pid_box[0].set_state(Gtk::STATE_INSENSITIVE); + pid_box[1].set_state(Gtk::STATE_INSENSITIVE); + pid_box[2].set_state(Gtk::STATE_INSENSITIVE); + pid_done_bt.set_state(Gtk::STATE_INSENSITIVE); + pid_edit_bt.set_state(Gtk::STATE_INSENSITIVE); + cb_pid.set_state(Gtk::STATE_INSENSITIVE);*/ + + pid_edit_bt.signal_pressed().connect(sigc::mem_fun(*this, &ControlGUI::_event_pid_edit_bt_clicked)); + pid_done_bt.signal_clicked().connect(sigc::mem_fun(*this, &ControlGUI::_event_pid_done_bt_clicked)); + } + + void _event_pid_edit_bt_clicked(){ + if (!pid_edit_flag) + { + pid_edit_flag = true; + pid_edit_bt.set_label("Cancel"); + pid_box[0].set_state(Gtk::STATE_NORMAL); + pid_box[1].set_state(Gtk::STATE_NORMAL); + pid_box[2].set_state(Gtk::STATE_NORMAL); + pid_done_bt.set_state(Gtk::STATE_NORMAL); + cb_pid.set_state(Gtk::STATE_NORMAL); + pid_tmp[0] = pid_box[0].get_text(); + pid_tmp[1] = pid_box[1].get_text(); + pid_tmp[2] = pid_box[2].get_text(); + + } + else + { + pid_edit_flag = false; + pid_edit_bt.set_label("Edit"); + pid_box[0].set_state(Gtk::STATE_INSENSITIVE); + pid_box[1].set_state(Gtk::STATE_INSENSITIVE); + pid_box[2].set_state(Gtk::STATE_INSENSITIVE); + pid_done_bt.set_state(Gtk::STATE_INSENSITIVE); + cb_pid.set_state(Gtk::STATE_INSENSITIVE); + pid_box[0].set_text(pid_tmp[0]); + pid_box[1].set_text(pid_tmp[1]); + pid_box[2].set_text(pid_tmp[2]); + } + } + + void _event_pid_done_bt_clicked() + { + std::string cmd; + switch (cb_pid.get_active_row_number()) + { + case 0: + cmd.append("AK"); + cmd.append(pid_box[0].get_text()); + cmd.append(";"); + cmd.append(pid_box[1].get_text()); + cmd.append(";"); + cmd.append(pid_box[2].get_text()); + cmd.append("#"); + + cmd.append("BK"); + cmd.append(pid_box[0].get_text()); + cmd.append(";"); + cmd.append(pid_box[1].get_text()); + cmd.append(";"); + cmd.append(pid_box[2].get_text()); + cmd.append("#"); + + cmd.append("CK"); + cmd.append(pid_box[0].get_text()); + cmd.append(";"); + cmd.append(pid_box[1].get_text()); + cmd.append(";"); + cmd.append(pid_box[2].get_text()); + cmd.append("#"); + + cmd.append("DK"); + cmd.append(pid_box[0].get_text()); + cmd.append(";"); + cmd.append(pid_box[1].get_text()); + cmd.append(";"); + cmd.append(pid_box[2].get_text()); + cmd.append("#"); + s.sendSerial(cmd); + break; + + case 1: + cmd.append("AK"); + cmd.append(pid_box[0].get_text()); + cmd.append(";"); + cmd.append(pid_box[1].get_text()); + cmd.append(";"); + cmd.append(pid_box[2].get_text()); + cmd.append("#"); + s.sendSerial(cmd); + break; + + case 2: + cmd.append("BK"); + cmd.append(pid_box[0].get_text()); + cmd.append(";"); + cmd.append(pid_box[1].get_text()); + cmd.append(";"); + cmd.append(pid_box[2].get_text()); + cmd.append("#"); + s.sendSerial(cmd); + break; + + case 3: + cmd.append("CK"); + cmd.append(pid_box[0].get_text()); + cmd.append(";"); + cmd.append(pid_box[1].get_text()); + cmd.append(";"); + cmd.append(pid_box[2].get_text()); + cmd.append("#"); + s.sendSerial(cmd); + break; + + case 4: + cmd.append("DK"); + cmd.append(pid_box[0].get_text()); + cmd.append(";"); + cmd.append(pid_box[1].get_text()); + cmd.append(";"); + cmd.append(pid_box[2].get_text()); + cmd.append("#"); + s.sendSerial(cmd); + break; + + default: + std::cout << "SET PID ERROR: Could not set PID." << std::endl; + break; + } + + std::cout << "Set PID - cmd: " << cmd << std::endl; + + pid_edit_flag = false; + pid_edit_bt.set_label("Edit"); + pid_done_bt.set_state(Gtk::STATE_INSENSITIVE); + pid_box[0].set_state(Gtk::STATE_INSENSITIVE); + pid_box[1].set_state(Gtk::STATE_INSENSITIVE); + pid_box[2].set_state(Gtk::STATE_INSENSITIVE); + cb_pid.set_state(Gtk::STATE_INSENSITIVE); + + } + }; diff --git a/cc/strategyGUI.hpp b/cc/strategyGUI.hpp index 738289da..9dbf3f30 100644 --- a/cc/strategyGUI.hpp +++ b/cc/strategyGUI.hpp @@ -9,6 +9,7 @@ #define STRATEGYGUI_HPP_ #include +#include "Strategy.hpp" class StrategyGUI: public Gtk::VBox { @@ -31,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; @@ -42,7 +44,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,12 +52,209 @@ class StrategyGUI: public Gtk::VBox Gtk::TextView strategy_description_view; Glib::RefPtr strategy_description_text; + Strategy strategy; + +protected: + Gtk::Grid constants_grid; + Gtk::CheckButton areaLimitX_bt; + Gtk::CheckButton banheira_bt; + Gtk::CheckButton areasDivision_bt; + Gtk::CheckButton goalCenter_bt; + Gtk::CheckButton goalMin_bt; + Gtk::CheckButton goalMax_bt; + Gtk::CheckButton defenseLine_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 defenseLine_flag = false; + bool goalMax_flag = false; + bool goalMin_flag = false; + bool goalCenter_flag = false; + bool areasDivision_flag = false; + bool banheira_flag = false; + bool areaLimitX_flag = false; + bool ballRadius_flag = false; + bool goalArea_flag = false; + bool sideRectangles_flag = false; + +public: + + bool get_ballRadius_flag() + { + return ballRadius_flag; + } + + bool get_goalArea_flag() + { + return goalArea_flag; + } + + bool get_sideRectangles_flag() + { + return sideRectangles_flag; + } + + bool get_deslocamentoZagaAtaque_flag() + { + return deslocamentoZagaAtaque_flag; + } + + bool get_defenseLine_flag() + { + return defenseLine_flag; + } + + bool get_goalMax_flag() + { + return goalMax_flag; + } + + bool get_goalMin_flag() + { + return goalMin_flag; + } + + bool get_goalCenter_flag() + { + return goalCenter_flag; + } + + bool get_areasDivision_flag() + { + return areasDivision_flag; + } + + bool get_banheira_flag() + { + return banheira_flag; + } + + + bool get_areaLimitX_flag() + { + return areaLimitX_flag; + } + StrategyGUI() { - createSelectionFrame(); - createMenuFrame(); - createInfoTextFrame(); - createInfoImageFrame(); + 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"); + + areaLimitX_bt.set_label("Area Limit X"); + banheira_bt.set_label("Banheira"); + areasDivision_bt.set_label("Areas Division"); + goalCenter_bt.set_label("Goal Center"); + goalMin_bt.set_label("Goal Min."); + goalMax_bt.set_label("Goal Max."); + defenseLine_bt.set_label("Defense Line"); + 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(ballRadius_bt, 0, 0, 1, 1); + constants_grid.attach(goalArea_bt, 1, 0, 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(goalCenter_bt, 0, 3, 1, 1); + constants_grid.attach(goalMin_bt, 1, 3, 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); + + 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)); + 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)); + deslocamentoZagaAtaque_bt.signal_pressed().connect(sigc::mem_fun(*this, &StrategyGUI::_event_deslocamentoZagaAtaque_bt_clicked)); + } + +private: + + void _event_sideRectangles_bt_clicked() + { + sideRectangles_flag = !sideRectangles_flag; + } + + void _event_ballRadius_bt_clicked() + { + ballRadius_flag = !ballRadius_flag; + } + + void _event_goalArea_bt_clicked() + { + goalArea_flag = !goalArea_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_goalCenter_bt_clicked() + { + goalCenter_flag = !goalCenter_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_deslocamentoZagaAtaque_bt_clicked() + { + deslocamentoZagaAtaque_flag = !deslocamentoZagaAtaque_flag; } void createSelectionFrame() @@ -69,13 +268,14 @@ 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); 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 +309,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 +323,7 @@ class StrategyGUI: public Gtk::VBox ~StrategyGUI() {} - + @@ -132,4 +332,4 @@ class StrategyGUI: public Gtk::VBox }; -#endif /* STRATEGYGUI_HPP_ */ \ No newline at end of file +#endif /* STRATEGYGUI_HPP_ */ diff --git a/cc/vision.hpp b/cc/vision.hpp index bbd553aa..14eeeb11 100644 --- a/cc/vision.hpp +++ b/cc/vision.hpp @@ -1 +1,578 @@ +#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 get_robot_list_size() + { + return robot_list.size(); + } + + Robot get_robot_from_list(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; // 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[i].position; // colocar em um vetor + robot_list[0].secundary = robot[i].secundary; // colocar em um vetor + robot_list[0].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; + + } + } + + } + + + void robot_creation_uni_duni_tag() { + vector 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"<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 { + + 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< 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"<button == 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<<")"< -#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/Robot.hpp b/pack-capture-gui/capture-gui/Robot.hpp index 484855c4..f23654d8 100644 --- a/pack-capture-gui/capture-gui/Robot.hpp +++ b/pack-capture-gui/capture-gui/Robot.hpp @@ -10,6 +10,7 @@ class Robot public: cv::Point position, secundary, target, ternary; char ID; + bool pink = false; double orientation=0; double orientation2=0; bool backward = false; @@ -262,8 +263,8 @@ class Robot Vl = vmax; spin = true; } - - + + } } else { //se nao esta perto da bola, o robo deve dar re if(spin) { diff --git a/pack-capture-gui/capture-gui/V4LInterface-aux.cpp b/pack-capture-gui/capture-gui/V4LInterface-aux.cpp index b0635787..97c4de43 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 " - " @@ -27,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(); @@ -353,7 +361,7 @@ namespace capture hbox3->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); @@ -866,6 +874,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); @@ -944,7 +971,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 +993,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 +1015,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 +1037,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(){ @@ -1125,6 +1172,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() : @@ -1156,6 +1218,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); @@ -1166,10 +1233,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; @@ -1212,15 +1276,7 @@ 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 #include +#include #define DEFAULT_STR " - " @@ -131,11 +132,16 @@ 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()); + std::cout << "ATOF " << atof(linha.c_str()) << std::endl; + 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)); } @@ -331,14 +337,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; } } @@ -676,7 +682,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 +693,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 +704,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 +715,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 +726,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 +737,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]); @@ -751,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]); @@ -762,7 +768,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 +779,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 +790,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 +801,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 +812,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]); @@ -1202,103 +1208,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"<