TrackBolaV3 OpenCV

1D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp

// TRACK3.cpp: define el punto de entrada de la aplicación de consola.//TEST DE SEGUIMIENTO DE UNA PELOTA IDENTIFICANDO CIRCULOS CON HOUGH CIRCLES// Y CON MATCH

/* ;B2 ENTRADA DIGITAL DIRECTA (MUELLE LANZADOR ADELANTE);B3 ENTRADA DIGITAL DIRECTA (MUELLE LANZADOR DETRAS);B4 ENTRADA DIGITAL DIRECTA (NO OBSTACULO DELANTE);B5 ENTRADA DIGITAL DIRECTA (NO OBSTACULO DETRAS) */

#undef _GLIBCXX_DEBUG

#include "stdafx.h"#include "cv.h"#include "highgui.h"#include "serie.h"#include "AnsiUnicode.h"#include "conversiones.h"

using namespace std;using namespace cv;

int NumCamara=0; int ServoMode=0; int ShowFilter=0; int TeachBall=0; int TeachGoal=0; int CmdTeachBall=0; int CmdTeachGoal=0; bool BallColorTrFltrCreate=false; bool JogMovTrCreate=false; bool GoalColorTrFltrCreate=false; bool HoughCircTrCreate=false; bool EcualizImgOrig=false; // control ecualizacion imagen original

// colores capturados para busqueda balon CvScalar AvgBallColor; int BallColorR=225; int BallColorG=64; int BallColorB=64;

// ventana de aceptacion color balon // margenes positivos balon int BallColorR_Plus=50; int BallColorG_Plus=50; int BallColorB_Plus=50; // variables para el trackbar positivo balon int BCRP=0; int BCGP=0; int BCBP=0; // margenes negativos balon int BallColorR_Minus=50; int BallColorG_Minus=50; int BallColorB_Minus=50; // variables para el trackbar negativo balon int BCRN=0; int BCGN=0; int BCBN=0;

// colores capturados busqueda de porteria CvScalar AvgGoalColor; int GoalColorR=255; int GoalColorG=255; int GoalColorB=255;

// ventana aceptacion color porteria

// margenes positivos balon int GoalColorR_Plus=50; int GoalColorG_Plus=50; int GoalColorB_Plus=50; // variables para el trackbar positivo balon int GCRP=0;


int GCGP=0; int GCBP=0; // margenes negativos balon int GoalColorR_Minus=50; int GoalColorG_Minus=50; int GoalColorB_Minus=50; // variables para el trackbar negativo balon int GCRN=0; int GCGN=0; int GCBN=0;

// variables calculo posicion ballon double BallPosXm=0; // posicion X centro gravedad balon double BallPosYm=0; // posicion Y centro gravedad balon double BallPosXe=0; // distancia X centro pantalla-posicion balon double BallPosYe=0; // distancia Y centro pantalla-posicion balon CvMoments BallMoment;

// variables calculo posicion porteria double GoalPosXm=0; // posicion X centro gravedad balon double GoalPosYm=0; // posicion Y centro gravedad balon double GoalPosXe=0; // distancia X centro pantalla-posicion balon double GoalPosYe=0; // distancia Y centro pantalla-posicion balon CvMoments GoalMoment; CvFont *Fuente;

Mat frame,miniframe; IplImage fotograma; char StrSalida[32]; char StrCoordMouse[32]; char StrBallData[32]; char StrGoalData[32];

// variables globales posicion del raton en ventana Live int XMouseLive,YMouseLive;

// variables globales movimiento del navegador

int SteerAngle = 90; // posiciones servo de direccion int SteerAngleJog = 90; int SteerAngleAut = 90; int SteerAngleTrack = 0;

int CameraAngle = 90; // posiciones servo camara int CameraAngleJog = 90; int CameraAngleAut = 90; int CameraAngleTrack = 0; int GunTriggerPos =0; // posiciones servo disparo lanzador int GunTriggerPosJog =0; int GunTriggerPosAut =0; int GunTriggerPosTrack =0;

int DrivePower = 0; // potencia motor de traccion int DrivePowerJog = 0; int DrivePowerAut = 0; int DrivePowerTrack=0;

int GunDrive=1; // motor carga lanzador int GunDriveJog=1; int GunDriveAut=0; int GunDriveTrack=0;

int DriPWR = 0; // valores a enviar a la placa de E/S int DriGUN = 0; int DriOUT = 0;

int BitA0; // estas variables estan relacionadas con DriOUT pues son sus bits int BitA1; int BitA2; int BitA3;


// variables busqueda bordes canny // y HoughCircles para balon int HoughCircSrcImg=0; int HoughCirc_dp=3; int HoughCirc_mindist=50; int HoughCirc_LoTrh=20; int HoughCirc_HiTrh=80; int HoughCirc_MinRad=5; int HoughCirc_MaxRad=60;

// y sus correspondientes para los trackbars int HoughCirc_dpTrk; int HoughCirc_mindistTrk; int HoughCirc_LoTrhTrk; int HoughCirc_HiTrhTrk; int HoughCirc_MinRadTrk; int HoughCirc_MaxRadTrk; // Variables envio valores al puerto

int DuttiC2GP4=100; //servo camara dirmemo 60 int DuttiC2GP5=100; // servo direccion dirmemo 61 int DuttiC3GP4=100; // servo gatillo chutador dirmemo 62 int DuttiC3GP5=100; int DuttiC0GP4=100; int DuttiC0GP5=0xFF; // motor PWM avance dirmemo 65 int DuttiC1GP4=100; int DuttiC1GP5=0xFF; // motor PWM carg. muelle dirmemo 67 int DirA3A2A1A0=0; // salidas activar motores dirmemo 68

bool RefreshC2GP4=false; bool RefreshC2GP5=false; bool RefreshC3GP4=false;

bool RefreshC0GP5=false; bool RefreshC1GP5=false; bool RefreshA3A2A1A0=false;

char StrServoC2GP4[32]; char StrServoC2GP5[32]; char StrServoC3GP4[32]; char StrServoC0GP5[32]; char StrServoC1GP5[32]; char StrDirA3A2A1A0[32];

char StrServo[32]; char StrRec06[16]; char StrDat06[3]; int ValDat06;

// variable del calculo de contornos CvMemStorage *Storage; CvMemStorage *Storage_PC;

// funciones callback trackbar afinado parametros hough circles void FctHoughCircNewDp (int position) { if (position > 1) HoughCirc_dp=position; }void FctHoughCircNewmindist(int position) { if (position >0) HoughCirc_mindist=position; }void FctHoughCircNewLoTrh (int position) { if (position >0) HoughCirc_LoTrh=position; }void FctHoughCircNewHiTrh (int position) { if (position >0) HoughCirc_HiTrh=position; }void FctHoughCircNewMinRad (int position) {


if (position >0) HoughCirc_MinRad=position; }void FctHoughCircNewMaxRad (int position) { if (position >0) HoughCirc_MaxRad=position; }

// funciones callback trackbars jogvoid FctDriveNewPower(int position) { DrivePowerJog=position; }void FctSteerNewAngle(int position) { SteerAngleJog=position; }

void FctCameraNewAngle(int position) { CameraAngleJog=position; }

void FctGunTriggerNewPos(int position) { GunTriggerPosJog=position; }void FctGunDriveNewVal(int position) { GunDriveJog=position; }

// funciones callback trackbars afinado de color balonvoid TrackBallColorRP_CallBack (int pos) { BallColorR_Plus=pos; }

void TrackBallColorGP_CallBack (int pos) { BallColorG_Plus=pos; }

void TrackBallColorBP_CallBack (int pos){ BallColorB_Plus=pos;}

void TrackBallColorRN_CallBack (int pos){ BallColorR_Minus=pos;}

void TrackBallColorGN_CallBack (int pos){ BallColorG_Minus=pos;}

void TrackBallColorBN_CallBack (int pos){ BallColorB_Minus=pos;}

// funciones callback trackbars afinado color porteriavoid TrackGoalColorRP_CallBack (int pos) { GoalColorR_Plus=pos; }

void TrackGoalColorGP_CallBack (int pos) { GoalColorG_Plus=pos; }


void TrackGoalColorBP_CallBack (int pos){ GoalColorB_Plus=pos;}

void TrackGoalColorRN_CallBack (int pos){ GoalColorR_Minus=pos;}

void TrackGoalColorGN_CallBack (int pos){ GoalColorG_Minus=pos;}

void TrackGoalColorBN_CallBack (int pos){ GoalColorB_Minus=pos;}

// funcion Callback raton en ventana barra de herramientas derechavoid ToolBar1MouseCallback(int evento,int x, int y,int flags,void* param){ int FilaToolBar; FilaToolBar =y/32; if (evento==1) { switch (FilaToolBar) { case 0: if (ServoMode==0) ServoMode=1; else ServoMode=0; break;

case 1: if (ShowFilter<12) ShowFilter++; else ShowFilter=0; break;

case 2: if (TeachBall==0) TeachBall=1; else TeachBall=0; TeachGoal=0; break;

case 3: if (TeachGoal==0) TeachGoal=1; else TeachGoal=0; TeachBall=0; break;

case 4: if (cvGetWindowHandle("WTrackBallFineColor")!=NULL) cvDestroyWindow

("WTrackBallFineColor"); if (cvGetWindowHandle("WTrackJog") !=NULL) cvDestroyWindow(

"WTrackJog"); if (cvGetWindowHandle("WTrackGoalFineColor")!=NULL) cvDestroyWindow

("WTrackGoalFineColor"); if (cvGetWindowHandle("WTrackHoughCirc")!=NULL) cvDestroyWindow(

"WTrackHoughCirc"); BallColorTrFltrCreate=true; break;






"WTrackHoughCirc"); GoalColorTrFltrCreate=true; break;





"WTrackHoughCirc"); JogMovTrCreate=true; break;

case 7: // boton comando cambiar de imagen fuente para el HoughCircles if (HoughCircSrcImg < 3) HoughCircSrcImg++; else HoughCircSrcImg=0; break;





"WTrackHoughCirc"); HoughCircTrCreate=true; break;

default: break; } }

sprintf(StrCoordMouse,"Fila %d",FilaToolBar);}

// funcion callback raton en ventana livevoid LiveMouseCallback(int evento,int x, int y,int flags,void* param){ XMouseLive=x; YMouseLive=y; if (evento==1) // click raton en ventana Live { if (TeachBall==1) // Estando activo modo aprendizaje balon CmdTeachBall=1; // activo marcador de comando Teach Ball Color

if (TeachGoal==1) // Estando activo modo aprendizaje porteria CmdTeachGoal=1; // activo marcador de comando Teach goal Color }}

////// funcion main int _tmain(int argc, _TCHAR* argv[]){ //variables para comunicacion pto serie HANDLE hPort; BOOL bRes; BYTE byte; BOOL timeout;

// entidades auxiliares para busqueda de circulos de hough CvSeq *results; CvMemStorage *storage=cvCreateMemStorage(0);

/* abrir puerto com1 */


char NomPuerto_utf8[8]="COM1"; wchar_t NomPuerto_utf16[8]; AnsiToUnicode16(NomPuerto_utf8,NomPuerto_utf16,8); hPort=OpenSerialPort(NomPuerto_utf16,CBR_9600,8,NOPARITY,ONESTOPBIT,1000);

if (hPort==INVALID_HANDLE_VALUE) { printf("Error abriendo COM1"); cvWaitKey(0); return -2; }

SerialSendByte(hPort,0x49); // envio una I para inicializar la pila de la placa de E/S

waitKey(10); bRes = SerialReceiveByte(hPort,&byte,&timeout); // recogemos la I devuelta para que no

se descuadre la lectura

/* solicitamos seleccion de camara */ printf("%s","Selecciona la camara 0-1 \n"); cin >> NumCamara; printf("La camara seleccionada es la %d",NumCamara);

Fuente=new CvFont(); cvInitFont(Fuente,CV_FONT_HERSHEY_COMPLEX,0.5,0.5,0,1,8);

// creamos el toolbar1 cvNamedWindow("ToolBar1",1); cvResizeWindow("ToolBar1",128,590); cvMoveWindow("ToolBar1",890,0); IplImage *ImgToolBar1=cvCreateImage(cvSize(128,590),IPL_DEPTH_8U,3); cvZero(ImgToolBar1);

for ( int j=0;j<18;j++) cvRectangle(ImgToolBar1,cvPoint(0,32*j),cvPoint(126,28+(32*j)),CV_RGB(0,0,255),

CV_FILLED);

cvPutText(ImgToolBar1,"ServoMode",cvPoint(5,18),Fuente,CV_RGB(255,255,255)); cvPutText(ImgToolBar1,"ViewFilter",cvPoint(5,50),Fuente,CV_RGB(255,255,255)); cvPutText(ImgToolBar1,"TeachBall",cvPoint(5,82),Fuente,CV_RGB(255,255,255)); cvPutText(ImgToolBar1,"TeachGoal",cvPoint(5,114),Fuente,CV_RGB(255,255,255)); cvPutText(ImgToolBar1,"TrkBallClr",cvPoint(5,146),Fuente,CV_RGB(255,255,255)); cvPutText(ImgToolBar1,"TrkGoalClr",cvPoint(5,178),Fuente,CV_RGB(255,255,255)); cvPutText(ImgToolBar1,"TrackJog",cvPoint(5,210),Fuente,CV_RGB(255,255,255)); cvPutText(ImgToolBar1,"SrcHougCirc",cvPoint(5,242),Fuente,CV_RGB(255,255,255)); cvPutText(ImgToolBar1,"TrkHougCirc",cvPoint(5,274),Fuente,CV_RGB(255,255,255)); cvShowImage("ToolBar1",ImgToolBar1);

// registro la funcion de callback del toolbar 1 cvSetMouseCallback("ToolBar1",ToolBar1MouseCallback);

// creamos ventanas

// Ventana Live cvNamedWindow("Live",1); cvResizeWindow("Live",320,240); cvMoveWindow ("Live",210,0); // registro de la funcion de callback de la ventana live cvSetMouseCallback("Live",LiveMouseCallback);

// Ventana Navegacion cvNamedWindow("Navegador", 1 ); cvResizeWindow("Navegador",320,320); cvMoveWindow("Navegador",210,265);

// ventana de filtrado cvNamedWindow("Filter",1); cvResizeWindow("Filter",320,240); cvMoveWindow("Filter",550,0);

//ventana de estado cvNamedWindow("Status",1);


cvResizeWindow("Status",200,590); cvMoveWindow("Status",0,0);

// creando y cargando variables imagen // creamos imagenes vacias // IplImage *ImgStatus=cvCreateImage(cvSize(200,590),IPL_DEPTH_8U,3); // image de color para procesado IplImage *ImgColorOrig=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); IplImage *ImgColor=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); // image de color para visualizacion con informacion superpuesta IplImage *ImgLive=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); // imagen Blanco y negro IplImage *ImgByW=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgByWCanny=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); // imagen filtro azul IplImage *ImgBlue=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgBlueOrig=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgBlueShow=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); //IplImage *ImgBlueTrh=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgBlueCanny=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); // imagen filtro rojo IplImage *ImgRed=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgRedOrig=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgRedShow=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); //IplImage *ImgRedTrh=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgRedCanny=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); // imagen filtro verde IplImage *ImgGreen=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgGreenOrig=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgGreenShow=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); //IplImage *ImgGreenTrh=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgGreenCanny=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); // imagen en negro para mezclas IplImage *ImgBlack=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); cvZero(ImgBlack);

// imagen para proceso de ventana de color balon IplImage *ImgRangeBall=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgBallErode=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgBallDilate=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); // imagen para proceso de ventana de color porteria IplImage *ImgRangeGoal=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgGoalErode=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1);

// imagen para proceso del canny balon activo IplImage *ImgCannyBall=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); IplImage *ImgCannyGoal=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1);

// imagen para mostrar el canny balon activo y sus circulos sobreimpresos en color IplImage *ImgShowCircles=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); // imagen para mostrar el canny porteria activo y sus lineas superpuestas; IplImage *ImgShowLines=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); // imagen para mostrar los contornos encontrados IplImage *ImgShowContours=cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); // imagen patron de contornos IplImage *ImgPatronC=cvLoadImage("circulo.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgGreyPC=cvCreateImage(cvSize(ImgPatronC->width,ImgPatronC->height),

IPL_DEPTH_8U,1); IplImage *ImgTrhPC=cvCreateImage(cvSize(ImgPatronC->width,ImgPatronC->height),

IPL_DEPTH_8U,1); // imagen del indicador de direccion cargada desde archivo IplImage *ImgSteer=cvLoadImage("steering.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgSteerRot=cvCreateImage(cvSize(320,320),IPL_DEPTH_8U,3); // imagen del indicador de camara cargada desde archivo IplImage *ImgCamara=cvLoadImage("camara.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgCamaraRot=cvCreateImage(cvSize(320,320),IPL_DEPTH_8U,3); // imagen indicador velocidad de movimientos IplImage *ImgDrivePwr0=cvLoadImage("DrivePwr0.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgDrivePwr1=cvLoadImage("DrivePwr1.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgDrivePwr2=cvLoadImage("DrivePwr2.bmp",CV_LOAD_IMAGE_ANYCOLOR); // Imagen indicador posicion del gatillo del canon de la bola IplImage *ImgGunTriggerOpen=cvLoadImage("GunTriggerOpen.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgGunTriggerClosed=cvLoadImage("GunTriggerClosed.bmp",

CV_LOAD_IMAGE_ANYCOLOR);


// Imagen indicador movimiento cargador del lanzador IplImage *ImgGunDriveFW=cvLoadImage("GunLoadFW.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgGunDriveBW=cvLoadImage("GunLoadBW.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgGunDriveST=cvLoadImage("GunLoadST.bmp",CV_LOAD_IMAGE_ANYCOLOR); // imagenes acumulador calculo ventana navegador IplImage *ImgAccu1=cvCreateImage(cvSize(320,320),IPL_DEPTH_8U,3); IplImage *ImgAccu2=cvCreateImage(cvSize(320,320),IPL_DEPTH_8U,3); IplImage *ImgAccu3=cvCreateImage(cvSize(320,320),IPL_DEPTH_8U,3); IplImage *ImgNull=cvCreateImage(cvSize(320,320),IPL_DEPTH_8U,3); // imagen a mostrar en la ventana navegador IplImage *ImgNavigator=cvCreateImage(cvSize(320,320),IPL_DEPTH_8U,3); // iconos a mostrar en la ventana navegador // indicador del sensor de balon IplImage *ImgBalonRojo=cvLoadImage("BALONROJO.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgBalonVerde=cvLoadImage("BALONVERDE.bmp",CV_LOAD_IMAGE_ANYCOLOR); // indicador del sensor de muelle lanzador reposo IplImage *ImgMuelleRepON=cvLoadImage("MUEREP_ON.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgMuelleRepOFF=cvLoadImage("MUEREP_OFF.bmp",CV_LOAD_IMAGE_ANYCOLOR); // indicador del sensor de muelle lanzador comprimido IplImage *ImgMuelleComON=cvLoadImage("MUECOM_ON.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgMuelleComOFF=cvLoadImage("MUECOM_OFF.bmp",CV_LOAD_IMAGE_ANYCOLOR); // indicador de bloqueo marcha adelante (obstaculo delante) IplImage *ImgFeedFwd=cvLoadImage("FEEDFWD.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgHoldFeedFwd=cvLoadImage("HOLDFEEDFWD.bmp",CV_LOAD_IMAGE_ANYCOLOR); // indicador de bloqueo marcha atras (obstaculo detras) IplImage *ImgFeedBwd=cvLoadImage("FEEDBWD.bmp",CV_LOAD_IMAGE_ANYCOLOR); IplImage *ImgHoldFeedBwd=cvLoadImage("HOLDFEEDBWD.bmp",CV_LOAD_IMAGE_ANYCOLOR);

cvSetImageROI(ImgBalonVerde,cvRect(1,1,31,31)); cvSetImageROI(ImgBalonRojo,cvRect(1,1,31,31)); cvSetImageROI(ImgMuelleRepOFF,cvRect(1,1,31,31)); cvSetImageROI(ImgMuelleRepON,cvRect(1,1,31,31)); cvSetImageROI(ImgMuelleComOFF,cvRect(1,1,31,31)); cvSetImageROI(ImgMuelleComON,cvRect(1,1,31,31)); cvSetImageROI(ImgFeedFwd,cvRect(1,1,31,31)); cvSetImageROI(ImgHoldFeedFwd,cvRect(1,1,31,31)); cvSetImageROI(ImgFeedBwd,cvRect(1,1,31,31)); cvSetImageROI(ImgHoldFeedBwd,cvRect(1,1,31,31));

// variables auxiliares para las rotaciones del navegador CvPoint2D32f center=cvPoint2D32f(160,160); double angle=0; double scale=1.0; CvMat *RotMat=cvCreateMat(2,3,CV_32FC1); /* inicilializo dispositivo captura*/ VideoCapture cap(NumCamara); if( !cap.isOpened() ) { printf("La camara %d no se puede asignar",NumCamara); cvWaitKey(0); return -1; } // imagen monitor de camara miniframe.create(240,320,CV_8UC3);

// calculos y variable de contornos

cvCvtColor(ImgPatronC,ImgGreyPC,CV_BGR2GRAY); // proceso imagen patron cvThreshold(ImgGreyPC,ImgTrhPC,128,255,CV_THRESH_BINARY_INV);

Storage=cvCreateMemStorage(0); Storage_PC=cvCreateMemStorage(0); CvSeq *FirstContour=NULL; CvSeq *contour_PC=NULL; // creacion contorno patron circular int NumPatronC=cvFindContours(ImgTrhPC,Storage_PC,&contour_PC,sizeof(CvContour),

CV_RETR_LIST); char strprt[32];

if (contour_PC) { cvDrawContours(ImgPatronC,contour_PC,CV_RGB(255,0,0),CV_RGB(0,255,0),0,1,8);


} sprintf(strprt,"Patrones=%i",NumPatronC); cvPutText(ImgPatronC,strprt,cvPoint(15,64),Fuente,CV_RGB(255,255,255));

for(;;) { /* creacion de ventanas en bucle de programa*/

if (CmdTeachBall==1) { cvSetImageROI(ImgColor,cvRect(XMouseLive-10,YMouseLive-10,20,20)); AvgBallColor=cvAvg(ImgColor,NULL); cvResetImageROI(ImgColor); BallColorR=(int)AvgBallColor.val[2]; BallColorG=(int)AvgBallColor.val[1]; BallColorB=(int)AvgBallColor.val[0]; CmdTeachBall=0; }

if (CmdTeachGoal==1) { cvSetImageROI(ImgColor,cvRect(XMouseLive-2,YMouseLive-2,4,4)); AvgGoalColor=cvAvg(ImgColor,NULL); cvResetImageROI(ImgColor); GoalColorR=(int)AvgGoalColor.val[2]; GoalColorG=(int)AvgGoalColor.val[1]; GoalColorB=(int)AvgGoalColor.val[0]; CmdTeachGoal=0; }

if (BallColorTrFltrCreate) { // Construccion ventana trackbars con shitt de color en RGB cvNamedWindow("WTrackBallFineColor",0); cvResizeWindow("WTrackBallFineColor",320,320); cvMoveWindow("WTrackBallFineColor",550,265);

// Trackbar shift Red Plus cvCreateTrackbar("BallClrR+","WTrackBallFineColor",&BCRP,128,

TrackBallColorRP_CallBack); cvSetTrackbarPos("BallClrR+","WTrackBallFineColor",BallColorR_Plus);

// Trackbar shift Red Minus cvCreateTrackbar("BallClrR-","WTrackBallFineColor",&BCRN,128,

TrackBallColorRN_CallBack); cvSetTrackbarPos("BallClrR-","WTrackBallFineColor",BallColorR_Minus);

// Trackbar shift green Plus cvCreateTrackbar("BallClrG+","WTrackBallFineColor",&BCGP,128,

TrackBallColorGP_CallBack); cvSetTrackbarPos("BallClrG+","WTrackBallFineColor",BallColorG_Plus);

// Trackbar shift Green Minus cvCreateTrackbar("BallClrG-","WTrackBallFineColor",&BCGN,128,

TrackBallColorGN_CallBack); cvSetTrackbarPos("BallClrG-","WTrackBallFineColor",BallColorG_Minus);

// Trackbar shift blue Plus cvCreateTrackbar("BallClrB+","WTrackBallFineColor",&BCBP,128,

TrackBallColorBP_CallBack); cvSetTrackbarPos("BallClrB+","WTrackBallFineColor",BallColorB_Plus);

// Trackbar shift Blue Minus cvCreateTrackbar("BallClrB-","WTrackBallFineColor",&BCBN,128,

TrackBallColorBN_CallBack); cvSetTrackbarPos("BallClrB-","WTrackBallFineColor",BallColorB_Minus);

BallColorTrFltrCreate=false; }

if (GoalColorTrFltrCreate) { // Construccion ventana trackbars con shitt de color en RGB


cvNamedWindow("WTrackGoalFineColor",0); cvResizeWindow("WTrackGoalFineColor",320,320); cvMoveWindow("WTrackGoalFineColor",550,265);

// Trackbar shift Red Plus cvCreateTrackbar("GoalClrR+","WTrackGoalFineColor",&GCRP,128,

TrackGoalColorRP_CallBack); cvSetTrackbarPos("GoalClrR+","WTrackGoalFineColor",GoalColorR_Plus);

// Trackbar shift Red Minus cvCreateTrackbar("GoalClrR-","WTrackGoalFineColor",&GCRN,128,

TrackGoalColorRN_CallBack); cvSetTrackbarPos("GoalClrR-","WTrackGoalFineColor",GoalColorR_Minus);

// Trackbar shift green Plus cvCreateTrackbar("GoalClrG+","WTrackGoalFineColor",&GCGP,128,

TrackGoalColorGP_CallBack); cvSetTrackbarPos("GoalClrG+","WTrackGoalFineColor",GoalColorG_Plus);

// Trackbar shift Green Minus cvCreateTrackbar("GoalClrG-","WTrackGoalFineColor",&GCGN,128,

TrackGoalColorGN_CallBack); cvSetTrackbarPos("GoalClrG-","WTrackGoalFineColor",GoalColorG_Minus);

// Trackbar shift blue Plus cvCreateTrackbar("GoalClrB+","WTrackGoalFineColor",&GCBP,128,

TrackGoalColorBP_CallBack); cvSetTrackbarPos("GoalClrB+","WTrackGoalFineColor",GoalColorB_Plus);

// Trackbar shift Blue Minus cvCreateTrackbar("GoalClrB-","WTrackGoalFineColor",&GCBN,128,

TrackGoalColorBN_CallBack); cvSetTrackbarPos("GoalClrB-","WTrackGoalFineColor",GoalColorB_Minus);

GoalColorTrFltrCreate=false; }

if (JogMovTrCreate) { // Construccion ventana trackbars con movimientos JOG cvNamedWindow("WTrackJog",0); cvResizeWindow("WTrackJog",320,320); cvMoveWindow("WTrackJog",550,265); cvCreateTrackbar("A_Steer","WTrackJog",&SteerAngleTrack,180,

FctSteerNewAngle); cvSetTrackbarPos("A_Steer","WTrackJog",SteerAngle); cvCreateTrackbar("A_Camera","WTrackJog",&CameraAngleTrack,180,

FctCameraNewAngle); cvSetTrackbarPos("A_Camera","WTrackJog",CameraAngle); cvCreateTrackbar("GunTrig","WTrackJog",&GunTriggerPosTrack,1,

FctGunTriggerNewPos); cvSetTrackbarPos("GunTrig","WTrackJog",GunTriggerPos); cvCreateTrackbar("PWRDrive","WTrackJog",&DrivePowerTrack,2,

FctDriveNewPower); cvSetTrackbarPos("PWRDrive","WTrackJog",DrivePower); cvCreateTrackbar("GUNDrive","WTrackJog",&GunDriveTrack,2,FctGunDriveNewVal)

; cvSetTrackbarPos("GUNDrive","WTrackJog",GunDrive);

JogMovTrCreate=false; }

if (HoughCircTrCreate) { // construccion ventana trackbars hough circles cvNamedWindow("WTrackHoughCirc",0); cvResizeWindow("WTrackHoughCirc",320,320); cvMoveWindow("WTrackHoughCirc",550,265);

cvCreateTrackbar("HC_dp","WTrackHoughCirc",&HoughCirc_dpTrk,4,FctHoughCircNewDp);

cvSetTrackbarPos("HC_dp","WTrackHoughCirc",HoughCirc_dp);

cvCreateTrackbar("HC_mindst","WTrackHoughCirc",&HoughCirc_mindistTrk,255,


FctHoughCircNewmindist); cvSetTrackbarPos("HC_mindst","WTrackHoughCirc",HoughCirc_mindist);

cvCreateTrackbar("HC_LoTrh","WTrackHoughCirc",&HoughCirc_LoTrhTrk,255,FctHoughCircNewLoTrh);

cvSetTrackbarPos("HC_LoTrh","WTrackHoughCirc",HoughCirc_LoTrh);

cvCreateTrackbar("HC_HiTrh","WTrackHoughCirc",&HoughCirc_HiTrhTrk,255,FctHoughCircNewHiTrh);

cvSetTrackbarPos("HC_HiTrh","WTrackHoughCirc",HoughCirc_HiTrh);

cvCreateTrackbar("HC_MinRad","WTrackHoughCirc",&HoughCirc_MinRadTrk,255,FctHoughCircNewMinRad);

cvSetTrackbarPos("HC_MinRad","WTrackHoughCirc",HoughCirc_MinRad);

cvCreateTrackbar("HC_MaxRad","WTrackHoughCirc",&HoughCirc_MaxRadTrk,255,FctHoughCircNewMaxRad);

cvSetTrackbarPos("HC_MaxRad","WTrackHoughCirc",HoughCirc_MaxRad);

HoughCircTrCreate=false;

}

// actualizacion estado de las entradas envio requerimiento mensaje a placa servo para estado entradas

sprintf(StrServo,"R06"); SerialSendByte(hPort,StrServo[0]); // R waitKey(1); SerialSendByte(hPort,StrServo[1]); // DIR waitKey(1); SerialSendByte(hPort,StrServo[2]); // DIR waitKey(1); SerialSendByte(hPort,0x0D); // CR waitKey(1); // le damos tiempo a contestar for (int i=0;i<5;i++) { bRes = SerialReceiveByte(hPort,&byte,&timeout); if (timeout) printf ("\n timeout \n"); else StrRec06[i]=byte; }

sprintf (StrDat06,"%c%c",StrRec06[3],StrRec06[4]);

int BitB0; int BitB1; int BitB2; int BitB3; int BitB4; int BitB5; int BitB6; int BitB7;

ValDat06=16*HexToInt(StrRec06[3])+HexToInt(StrRec06[4]); BitB5=(ValDat06 & 32)/32; // B5 ENTRADA DIGITAL DIRECTA (OBSTACULO DETRAS) BitB4=(ValDat06 & 16)/16; // B4 ENTRADA DIGITAL DIRECTA (OBSTACULO DELANTE) BitB3=(ValDat06 & 8)/8; // B3 ENTRADA DIGITAL DIRECTA (CARGADOR MUELLE

LANZADOR ATRAS) BitB2=(ValDat06 & 4)/4; // B2 ENTRADA DIGITAL DIRECTA (CARGADOR MUELLE

LANZADOR ADELANTE) BitB0=(ValDat06 & 1); // B0 ENTRADA DIGITAL DIRECTA (HAY BALON)

/* captura de la imagen */ cap >> frame; resize(frame,miniframe,miniframe.size(),0,0,INTER_LINEAR); fotograma=(IplImage) miniframe;

cvConvertImage(&fotograma,ImgColorOrig,0); cvSplit(ImgColorOrig,ImgBlueOrig,ImgGreenOrig,ImgRedOrig,NULL);

// ecualizacioin condicional de la imagen por canales


if (EcualizImgOrig) { cvEqualizeHist(ImgBlueOrig,ImgBlue); cvEqualizeHist(ImgGreenOrig,ImgGreen); cvEqualizeHist(ImgRedOrig,ImgRed); } else { cvCopy(ImgBlueOrig,ImgBlue); cvCopy(ImgGreenOrig,ImgGreen); cvCopy(ImgRedOrig,ImgRed); }

cvMerge(ImgBlack,ImgBlack,ImgRed,NULL,ImgRedShow); cvMerge(ImgBlue,ImgBlack,ImgBlack,NULL,ImgBlueShow); cvMerge(ImgBlack,ImgGreen,ImgBlack,NULL,ImgGreenShow); cvMerge(ImgBlue,ImgGreen,ImgRed,NULL,ImgColor); cvCopy(ImgColor,ImgLive);

cvCvtColor(ImgColor,ImgByW,CV_RGB2GRAY); //cvEqualizeHist(ImgByW,ImgByW); cvCanny(ImgByW,ImgByWCanny,HoughCirc_LoTrh,HoughCirc_HiTrh);

// ventana de color aceptacion color balon int BCRMax=BallColorR+BallColorR_Plus; int BCGMax=BallColorG+BallColorG_Plus; int BCBMax=BallColorB+BallColorB_Plus;

int BCRMin=BallColorR-BallColorR_Minus; int BCGMin=BallColorG-BallColorG_Minus; int BCBMin=BallColorB-BallColorB_Minus;

if (BCRMax >= 300) BCRMax=300; if (BCGMax >= 300) BCGMax=300; if (BCBMax >= 300) BCBMax=300;

if (BCRMin < 0) BCRMin=0; if (BCGMin < 0) BCGMin=0; if (BCBMin < 0) BCBMin=0;

// ventana color aceptacion color porteria goalkick int GCRMax=GoalColorR+GoalColorR_Plus; int GCGMax=GoalColorG+GoalColorG_Plus; int GCBMax=GoalColorB+GoalColorB_Plus;

int GCRMin=GoalColorR-GoalColorR_Minus; int GCGMin=GoalColorG-GoalColorG_Minus; int GCBMin=GoalColorB-GoalColorB_Minus;

if (GCRMax >= 300) GCRMax=300; if (GCGMax >= 300) GCGMax=300; if (GCBMax >= 300) GCBMax=300;

if (GCRMin < 0) GCRMin=0; if (GCGMin < 0) GCGMin=0; if (GCBMin < 0) GCBMin=0;

cvInRangeS(ImgColor,CV_RGB(BCRMin,BCGMin,BCBMin),CV_RGB(BCRMax,BCGMax,BCBMax),ImgRangeBall);

cvErode(ImgRangeBall,ImgBallErode,NULL,1); cvDilate(ImgRangeBall,ImgBallDilate,NULL,2);

cvInRangeS(ImgColor,CV_RGB(GCRMin,GCGMin,GCBMin),CV_RGB(GCRMax,GCGMax,GCBMax),ImgRangeGoal);

cvErode(ImgRangeGoal,ImgGoalErode,NULL,1);

cvCanny(ImgRangeBall,ImgCannyBall,HoughCirc_LoTrh,HoughCirc_HiTrh); results=cvHoughCircles(ImgCannyBall,storage,CV_HOUGH_GRADIENT,HoughCirc_dp,

HoughCirc_mindist,HoughCirc_LoTrh,HoughCirc_HiTrh,HoughCirc_MinRad,HoughCirc_MaxRad);

/* localizamos el balon por su color, aplicando momentos */ // estas coordenadas son provisionales,corresponden con el centro de masa // de todos los puntos que estan en el color correcto,


// si puntos de ese color estan dispersos por la pantalla, // las coordenadas no sirven para nada.

cvMoments(ImgBallErode,&BallMoment,1);

if (BallMoment.m00 !=0) { BallPosXm=BallMoment.m10/BallMoment.m00; BallPosYm=BallMoment.m01/BallMoment.m00; } else { BallPosXm=160; BallPosYm=120; } /* localizamos la porteria por su color, aplicando momentos */ cvMoments(ImgGoalErode,&GoalMoment,1);

if (GoalMoment.m00 !=0) { GoalPosXm=GoalMoment.m10/GoalMoment.m00; GoalPosYm=GoalMoment.m01/GoalMoment.m00; } else { GoalPosXm=160; GoalPosYm=120; } // busqueda de contornos en imagen del balon dilatada.

/* ********************************* informacion de la ventana status ********************************* */

/* borramos la imagen antes de refrescar la informacion */ cvZero(ImgStatus);

/* informacion modo servos */ if (ServoMode == 0) cvPutText(ImgStatus,"Servo MAN",cvPoint(0,15),Fuente,CV_RGB(255,0,0)); else cvPutText(ImgStatus,"Servo AUT",cvPoint(0,15),Fuente,CV_RGB(0,255,0));

/* informacion modo captura de color */ if (TeachBall ==0) cvPutText(ImgStatus,"TeachBall OFF",cvPoint(0,30),Fuente,CV_RGB(255,0,0)); else cvPutText(ImgStatus,"TeachBall ON",cvPoint(0,30),Fuente,CV_RGB(0,255,0));

/* informacion modo captura de color porteria goalkick */ if (TeachGoal ==0) cvPutText(ImgStatus,"TeachGoal OFF",cvPoint(0,45),Fuente,CV_RGB(255,0,0)); else cvPutText(ImgStatus,"TeachGoal ON",cvPoint(0,45),Fuente,CV_RGB(0,255,0));

/*informacion coordenadas mouse en pantalla live */ sprintf(StrCoordMouse,"XLive=%03i YLive=%03i",XMouseLive,YMouseLive); cvPutText(ImgStatus,StrCoordMouse,cvPoint(0,60),Fuente,CV_RGB(255,255,255));

/*Informacion color target bola */ cvPutText(ImgStatus," -- InfoBall -- ",cvPoint(0,75),Fuente,CV_RGB(255,255,

255)); sprintf(StrSalida,"R %03i G %03i B %03i",BallColorR,BallColorG,BallColorB); cvPutText(ImgStatus,StrSalida,cvPoint(20,90),Fuente,CV_RGB(255,255,255)); cvRectangle(ImgStatus,cvPoint(0,78),cvPoint(15,90),CV_RGB(255,255,255)); cvRectangle(ImgStatus,cvPoint(1,79),cvPoint(14,89),AvgBallColor,CV_FILLED);

/* informacion posicion bola */ sprintf(StrBallData,"XBall=%03i YBall=%03i",(int)BallPosXm,(int)BallPosYm); cvPutText(ImgStatus,StrBallData,cvPoint(0,105),Fuente,CV_RGB(255,255,255));


/* Infomacion color target porteria */ cvPutText(ImgStatus," -- InfoGoal -- ",cvPoint(0,120),Fuente,CV_RGB(255,255

,255)); sprintf(StrSalida,"R %03i G %03i B %03i",GoalColorR,GoalColorG,GoalColorB); cvPutText(ImgStatus,StrSalida,cvPoint(20,135),Fuente,CV_RGB(255,255,255)); cvRectangle(ImgStatus,cvPoint(0,123),cvPoint(15,135),CV_RGB(255,255,255)); cvRectangle(ImgStatus,cvPoint(1,124),cvPoint(14,134),AvgGoalColor,CV_FILLED);

/* informacion capa de color activa para HoughCircles en el balon */ cvPutText(ImgStatus,"HoughCircSrc: ",cvPoint(0,150),Fuente,CV_RGB(255,255,255))

; switch (HoughCircSrcImg) { case 0: cvPutText(ImgStatus,"GREY",cvPoint(125,150),Fuente,CV_RGB(128,128,128))

; break; case 1: cvPutText(ImgStatus,"RED",cvPoint(125,150),Fuente,CV_RGB(255,0,0)); break; case 2: cvPutText(ImgStatus,"GREEN",cvPoint(125,150),Fuente,CV_RGB(0,255,0)); break; case 3: cvPutText(ImgStatus,"BLUE",cvPoint(125,150),Fuente,CV_RGB(0,0,255)); break; default: cvPutText(ImgStatus,"invalid",cvPoint(125,150),Fuente,CV_RGB(255,255,

255)); break; }

/* informacion sobreimpresa de la imagen LIVE */

// info pos toma muestra color balon if (TeachBall==1) { cvRectangle(ImgLive,cvPoint(XMouseLive-10,YMouseLive-10),cvPoint(XMouseLive

+10,YMouseLive+10),CV_RGB(255,255,255)); cvPutText(ImgLive,"TeachBall",cvPoint(XMouseLive-30,YMouseLive-15),Fuente,

CV_RGB(255,0,0)); }

// info pos toma muestra color porteria if (TeachGoal==1) { cvRectangle(ImgLive,cvPoint(XMouseLive-2,YMouseLive-2),cvPoint(XMouseLive+4

,YMouseLive+4),CV_RGB(255,255,255)); cvPutText(ImgLive,"TeachGoal",cvPoint(XMouseLive-30,YMouseLive-15),Fuente,

CV_RGB(255,0,0)); }

// indicador posicion estimada balon cvCircle(ImgLive,cvPoint(BallPosXm,BallPosYm),10,CV_RGB(255,0,0)); cvPutText(ImgLive,"Ball",cvPoint(BallPosXm-15,BallPosYm-15),Fuente,CV_RGB(255,0

,0));

// indicador posicion estimada porteria cvRectangle(ImgLive,cvPoint(GoalPosXm-10,GoalPosYm-10),cvPoint(GoalPosXm+10,

GoalPosYm+10),CV_RGB(0,0,255)); cvPutText(ImgLive,"GoalKick",cvPoint(GoalPosXm-15,GoalPosYm-15),Fuente,CV_RGB

(255,0,0));

// circulos encontrados con HoughCircles

for (int i=0;i<results->total;i++) { float *p=(float*)cvGetSeqElem(results,i); CvPoint pt=cvPoint(cvRound(p[0]),cvRound(p[1])); cvCircle(ImgLive,pt,cvRound(p[2]),CV_RGB(0,255,0)); }


/* refresco de la ventana de navegacion */

if (ServoMode==1) { SteerAngle=SteerAngleAut; CameraAngle=CameraAngleAut; DrivePower=DrivePowerAut; GunTriggerPos=GunTriggerPosAut;

//cvSetTrackbarPos("A_Steer","WTrackJog",SteerAngle); //cvSetTrackbarPos("A_Camera","WTrackJog",CameraAngle); //cvSetTrackbarPos("PWRDrive","WTrackJog",DrivePower); } else { SteerAngle=SteerAngleJog; CameraAngle=CameraAngleJog; GunTriggerPos=GunTriggerPosJog; DrivePower=DrivePowerJog; GunDrive=GunDriveJog;

}

/* formacion de datos para control de motores D DriPWR accionamiento motor de avance Funciona con la salida C0.GP5 direccion W65 Admite valores entre 0xFF (10% potencia) y 0xF0 (100% potencia) Necesita la activacion de los bits 0 y 1 en las salidas, direccion W68 DriGUN accionamiento cargador de muelle Funciona con la salida C1.GP5 enn direccion W67 Admite valores entre 0xFF (10% potencia) y 0xF0 (100% potencia) Necesita la activacion de los bits 2 y 3 en las salidas, direcccion W68

DriOUT Activacion de las salidas A3,A2,A1,A0 */

switch (DrivePower) { case 0: DriPWR=0xFF; // ponemos dutti minima potencia DriOUT=DriOUT & 0xFC; // borramos las 2 salidas A1 A0 break;

case 1: DriPWR=0xFE; // ponemos dutti media potencia DriOUT=DriOUT & 0xFC; // borramos las 2 salidas A1 A0 DriOUT=DriOUT | 0x01; // activamos la salida 0 break;

case 2: DriPWR=0xF0; // ponemos dutti maxima potencia DriOUT=DriOUT & 0xFC; // borramos las 2 salidas A1 A0 DriOUT=DriOUT | 0x01; // activamos la salida 0 break;

default: DriPWR=0xFF; // ponemos dutti minima potencia DriOUT=DriOUT & 0xFC; // borramos las 2 salidas A1 A0 break; }

switch (GunDrive) { case 0: DriGUN=0xF0; // ponemos dutti maxima potencia DriOUT=DriOUT & 0xF3; // borramos las 2 salidas A2 A3 DriOUT=DriOUT | 0x04; // activo salida A2 break;

case 1: DriGUN=0xFF; // ponemos dutti minima potencia DriOUT=DriOUT & 0xF3; // borramos las 2 salidas A2 A3


break;

case 2: DriGUN=0xF0; // ponemos dutti maxima potencia DriOUT=DriOUT & 0xF3; // borramos las 2 salidas A2 A3 DriOUT=DriOUT | 0x08; // activo salida A3 break;

default: DriGUN=0xFF; // ponemos dutti minima potencia DriOUT=DriOUT & 0xF3; // borramos las 2 salidas A2 A3 break; }

BitA3=(DriOUT & 8)/8; // A3 SALIDA DIGITAL CARGADOR MUELLE LANZADOR ADELANTE)

BitA2=(DriOUT & 4)/4; // A2 SALIDA DIGITAL CARGADOR MUELLE LANZADOR ATRAS)

BitA1=(DriOUT & 2)/2; // A1 SALIDA DIGITAL AVANCE ATRAS BitA0=(DriOUT & 1); // A0 SALIDA DIGITAL AVANCE ADELANTE

/* Informacion comandos a los servos */ cvPutText(ImgStatus,"-- InfoServoCmd --",cvPoint(0,300),Fuente,CV_RGB(255,255,

255));

sprintf(StrSalida,"A_Steer =%03i 0x%02X",SteerAngle,SteerAngle); cvPutText(ImgStatus,StrSalida,cvPoint(0,315),Fuente,CV_RGB(255,255,255));

sprintf(StrSalida,"A_Camera=%03i 0x%02X",CameraAngle,CameraAngle); cvPutText(ImgStatus,StrSalida,cvPoint(0,330),Fuente,CV_RGB(255,255,255));

sprintf(StrSalida,"GunTrg =%03i 0x%02X",50+(90*GunTriggerPos),50+(90*GunTriggerPos));

cvPutText(ImgStatus,StrSalida,cvPoint(0,345),Fuente,CV_RGB(255,255,255));

//sprintf(StrSalida,"DrivePWR =%03i 0x%02X",DrivePower,DrivePower); sprintf(StrSalida,"DrivePWR =%03i 0x%02X",DriPWR,DriPWR); cvPutText(ImgStatus,StrSalida,cvPoint(0,360),Fuente,CV_RGB(255,255,255));

//sprintf(StrSalida,"GUNDrive =%03i 0x%02X",GunDrive,GunDrive); sprintf(StrSalida,"GUNDrive =%03i 0x%02X",DriGUN,DriGUN); cvPutText(ImgStatus,StrSalida,cvPoint(0,375),Fuente,CV_RGB(255,255,255));

sprintf(StrSalida,"B5B4B3B2B0 %i,%i,%i,%i,%i",BitB5,BitB4,BitB3,BitB2,BitB0); cvPutText(ImgStatus,StrSalida,cvPoint(0,390),Fuente,CV_RGB(255,255,255));

sprintf(StrSalida,"A3A2A1A0 %i,%i,%i,%i",BitA3,BitA2,BitA1,BitA0); cvPutText(ImgStatus,StrSalida,cvPoint(0,405),Fuente,CV_RGB(255,255,255));

cvZero(ImgCamaraRot); angle=(double)CameraAngle-90; cv2DRotationMatrix(center,angle,scale,RotMat); cvWarpAffine(ImgCamara,ImgCamaraRot,RotMat);

cvZero(ImgSteerRot); angle=(double)SteerAngle-90; cv2DRotationMatrix(center,angle,scale,RotMat); cvWarpAffine(ImgSteer,ImgSteerRot,RotMat);

cvZero(ImgNavigator); cvZero(ImgAccu1); cvZero(ImgAccu2); cvZero(ImgAccu3); cvZero(ImgNull);

switch (DrivePower) { case 0: cvOr(ImgDrivePwr0,ImgNull,ImgAccu1); break;

case 1: cvOr(ImgDrivePwr1,ImgNull,ImgAccu1);


break;

case 2: cvOr(ImgDrivePwr2,ImgNull,ImgAccu1); break;

default: break; }

switch (GunDrive) { case 0: cvOr(ImgGunDriveBW,ImgAccu1,ImgAccu2); break;

case 1: cvOr(ImgGunDriveST,ImgAccu1,ImgAccu2); break;

case 2: cvOr(ImgGunDriveFW,ImgAccu1,ImgAccu2); break;

default: break; }

switch (GunTriggerPos) { case 0: cvOr(ImgGunTriggerClosed,ImgAccu2,ImgAccu3); break;

case 1: cvOr(ImgGunTriggerOpen,ImgAccu2,ImgAccu3); break;

default: break; }

cvOr(ImgSteerRot,ImgCamaraRot,ImgAccu2); cvOr(ImgAccu3,ImgAccu2,ImgNavigator); /* Volcado de consignas al COM1 */ if (DuttiC2GP4!=CameraAngle) { RefreshC2GP4=true; DuttiC2GP4=CameraAngle; }

if (DuttiC2GP5!=SteerAngle) { RefreshC2GP5=true; DuttiC2GP5=SteerAngle; }

if (DuttiC3GP4!=GunTriggerPos) { RefreshC3GP4=true; DuttiC3GP4=GunTriggerPos; }

if (DuttiC0GP5!=DriPWR) { RefreshC0GP5=true; DuttiC0GP5=DriPWR; }

if (DuttiC1GP5!=DriGUN) { RefreshC1GP5=true;


DuttiC1GP5=DriGUN; }

if (DirA3A2A1A0 !=DriOUT) { RefreshA3A2A1A0=true; DirA3A2A1A0=DriOUT; }

DuttiC3GP5=100; DuttiC0GP4=100; DuttiC1GP4=100; sprintf(StrServoC2GP4,"w60%02X",DuttiC2GP4); cvPutText(ImgNavigator,StrServoC2GP4,cvPoint(0,155),Fuente,CV_RGB(255,255,255));

sprintf(StrServoC2GP5,"w61%02X",DuttiC2GP5); cvPutText(ImgNavigator,StrServoC2GP5,cvPoint(0,170),Fuente,CV_RGB(255,255,255));

sprintf(StrServoC3GP4,"w62%02X",50+(90*DuttiC3GP4)); cvPutText(ImgNavigator,StrServoC3GP4,cvPoint(0,185),Fuente,CV_RGB(255,255,255));



sprintf(StrDirA3A2A1A0,"w68%02X",DirA3A2A1A0); cvPutText(ImgNavigator,StrDirA3A2A1A0,cvPoint(0,230),Fuente,CV_RGB(255,255,255));/* if (RefreshC2GP4) { SerialSendByte(hPort,StrServoC2GP4[0]); // w waitKey(1); SerialSendByte(hPort,StrServoC2GP4[1]); // DIR waitKey(1); SerialSendByte(hPort,StrServoC2GP4[2]); // DIR waitKey(1); SerialSendByte(hPort,StrServoC2GP4[3]); // DAT waitKey(1); SerialSendByte(hPort,StrServoC2GP4[4]); // DAT waitKey(1); SerialSendByte(hPort,0x0D); // CR waitKey(1); RefreshC2GP4=false; }

if (RefreshC2GP5) { SerialSendByte(hPort,StrServoC2GP5[0]); // w waitKey(1); SerialSendByte(hPort,StrServoC2GP5[1]); // DIR waitKey(1); SerialSendByte(hPort,StrServoC2GP5[2]); // DIR waitKey(1); SerialSendByte(hPort,StrServoC2GP5[3]); // DAT waitKey(1); SerialSendByte(hPort,StrServoC2GP5[4]); // DAT waitKey(1); SerialSendByte(hPort,0x0D); // CR waitKey(1); RefreshC2GP5=false; }*/ if (RefreshA3A2A1A0) { SerialSendByte(hPort,StrDirA3A2A1A0[0]); // w waitKey(1); SerialSendByte(hPort,StrDirA3A2A1A0[1]); // DIR waitKey(1); SerialSendByte(hPort,StrDirA3A2A1A0[2]); // DIR waitKey(1); SerialSendByte(hPort,StrDirA3A2A1A0[3]); // DAT


waitKey(1); SerialSendByte(hPort,StrDirA3A2A1A0[4]); // DAT waitKey(1); SerialSendByte(hPort,0x0D); // CR waitKey(5); RefreshA3A2A1A0=false; }

if ((RefreshC2GP4)||(RefreshC2GP5)||(RefreshC3GP4)||(RefreshC0GP5)||(RefreshC1GP5)||(RefreshA3A2A1A0))

{ SerialSendByte(hPort,'P'); // P de Poke seguido de direccion

base y 8 datos waitKey(1); SerialSendByte(hPort,'6'); // DIR waitKey(1); SerialSendByte(hPort,'0'); // DIR waitKey(1);

SerialSendByte(hPort,StrServoC2GP4[3]); // DAT C2GP4 waitKey(1); SerialSendByte(hPort,StrServoC2GP4[4]); // DAT waitKey(1);



SerialSendByte(hPort,'0'); // DAT C3GP5 waitKey(1); SerialSendByte(hPort,'0'); // DAT waitKey(1);





SerialSendByte(hPort,0x0D); // CR waitKey(1);

RefreshC2GP4=false; RefreshC2GP5=false; RefreshC3GP4=false; RefreshC0GP5=false; RefreshC1GP5=false; }

/* informacion correspondiente al estado de entradas en ventana Navegador */

sprintf (StrSalida,"%s",StrRec06); cvPutText(ImgNavigator,StrSalida,cvPoint(0,2015),Fuente,CV_RGB(255,255,255));


/* iconos correspondientes al estado de las entradas ventana navegador*/

cvCopy(ImgNavigator,ImgAccu1);

// bit2 indicador de muelle reposo cvSetImageROI(ImgAccu1,cvRect(82,289,31,31)); cvSetImageROI(ImgNavigator,cvRect(82,289,31,31)); if (BitB2==1) cvAdd(ImgAccu1,ImgMuelleRepON,ImgNavigator); else cvAdd(ImgAccu1,ImgMuelleRepOFF,ImgNavigator); // bit3 indicador de muelle comprimido cvSetImageROI(ImgAccu1,cvRect(113,289,31,31)); cvSetImageROI(ImgNavigator,cvRect(113,289,31,31)); if (BitB3==1) cvAdd(ImgAccu1,ImgMuelleComON,ImgNavigator); else cvAdd(ImgAccu1,ImgMuelleComOFF,ImgNavigator);

// bit0 indicador de balon cvSetImageROI(ImgAccu1,cvRect(145,289,31,31)); cvSetImageROI(ImgNavigator,cvRect(145,289,31,31)); if (BitB0==1) cvAdd(ImgAccu1,ImgBalonVerde,ImgNavigator); else cvAdd(ImgAccu1,ImgBalonRojo,ImgNavigator);

// bit4 indicador obstaculo delante cvSetImageROI(ImgAccu1,cvRect(177,289,31,31)); cvSetImageROI(ImgNavigator,cvRect(177,289,31,31)); if (BitB4==1) cvAdd(ImgAccu1,ImgFeedFwd,ImgNavigator); else cvAdd(ImgAccu1,ImgHoldFeedFwd,ImgNavigator);

// bit5 indicador obstaculo delante cvSetImageROI(ImgAccu1,cvRect(209,289,31,31)); cvSetImageROI(ImgNavigator,cvRect(209,289,31,31)); if (BitB5==1) cvAdd(ImgAccu1,ImgFeedBwd,ImgNavigator); else cvAdd(ImgAccu1,ImgHoldFeedBwd,ImgNavigator);

cvResetImageROI(ImgAccu1); cvResetImageROI(ImgNavigator);

/* volcado de imagenes en ventanas */

cvShowImage("Live", ImgLive); cvShowImage("Status",ImgStatus); cvShowImage("Navegador",ImgNavigator);

switch (ShowFilter) { case 0: cvPutText(ImgByW,"B&W",cvPoint(0,15),Fuente,CV_RGB(128,128,128)); cvShowImage("Filter",ImgByW); break; case 1: cvPutText(ImgRedShow,"ChanR",cvPoint(0,15),Fuente,CV_RGB(128,128,128)); cvShowImage("Filter",ImgRedShow); break; case 2: cvPutText(ImgBlueShow,"ChanB",cvPoint(0,15),Fuente,CV_RGB(128,128,128))

; cvShowImage("Filter",ImgBlueShow); break; case 3: cvPutText(ImgGreenShow,"ChanG",cvPoint(0,15),Fuente,CV_RGB(128,128,

128)); cvShowImage("Filter",ImgGreenShow); break; case 4: cvPutText(ImgRangeBall,"InRangeBall",cvPoint(0,15),Fuente,CV_RGB(128,

128,128)); cvShowImage("Filter",ImgRangeBall); break; case 5: cvPutText(ImgBallErode,"InRangeBall & Erode",cvPoint(0,15),Fuente,

CV_RGB(128,128,128)); cvShowImage("Filter",ImgBallErode); break; case 6:


cvPutText(ImgRangeGoal,"InRangeGoal",cvPoint(0,15),Fuente,CV_RGB(128,128,128));

cvShowImage("Filter",ImgRangeGoal); break; case 7: cvPutText(ImgGoalErode,"InRangeGoal & Erode",cvPoint(0,15),Fuente,

CV_RGB(128,128,128)); cvShowImage("Filter",ImgGoalErode); break; case 8: cvPutText(ImgShowContours,"InRangeBall > Contours",cvPoint(0,15),Fuente

,CV_RGB(128,128,128)); cvShowImage("Filter",ImgShowContours); break; case 9: cvPutText(ImgPatronC,"Contorno Patron",cvPoint(0,15),Fuente,CV_RGB(128,

128,128)); cvShowImage("Filter",ImgPatronC); break; case 10: cvPutText(ImgBallDilate,"ImgBallDilate",cvPoint(0,15),Fuente,CV_RGB(255

,255,255)); cvShowImage("Filter",ImgBallDilate); break; case 11: cvPutText(ImgByWCanny,"ByW & Canny",cvPoint(0,15),Fuente,CV_RGB(128,128

,128)); cvShowImage("Filter",ImgByWCanny); break; case 12: cvPutText(ImgCannyBall,"Img CannyBall",cvPoint(0,15),Fuente,CV_RGB(128,

128,128)); cvShowImage("Filter",ImgCannyBall); break;

default: break; }

/* consulta fin de aplicacion */ if(waitKey(1)==27) break; }

/* liberando recursos */

cvDestroyAllWindows(); cvReleaseImage(&ImgStatus); cvReleaseImage(&ImgToolBar1); cvReleaseImage(&ImgColor); cvReleaseImage(&ImgLive); CloseSerialPort(hPort); cap.release();

return 0;}

Documents

TrackBolaV3 OpenCV