D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp // TRACK3.cpp: define el punto de entrada de la aplicacin de consola.

//TEST DE SEGUIMIENTO DE UNA PELOTA IDENTIFICANDO CIRCULOS CON HOUGH CIRCLES // Y CON MATCH /* ;B2 ;B3 ;B4 ;B5 */

ENTRADA ENTRADA ENTRADA ENTRADA

DIGITAL DIGITAL DIGITAL DIGITAL

DIRECTA DIRECTA DIRECTA DIRECTA

(MUELLE LANZADOR ADELANTE) (MUELLE LANZADOR DETRAS) (NO OBSTACULO DELANTE) (NO OBSTACULO DETRAS)

#undef _GLIBCXX_DEBUG #include #include #include #include #include #include "stdafx.h" "cv.h" "highgui.h" "serie.h" "AnsiUnicode.h" "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;

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp 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 char char char StrSalida[32]; StrCoordMouse[32]; StrBallData[32]; StrGoalData[32];

// variables globales posicion del raton en ventana Live int XMouseLive,YMouseLive; // variables globales movimiento del navegador int int int int int int int int int int int int int int int int int int int int SteerAngle = 90; SteerAngleJog = 90; SteerAngleAut = 90; SteerAngleTrack = 0; CameraAngle = 90; CameraAngleJog = 90; CameraAngleAut = 90; CameraAngleTrack = 0; GunTriggerPos =0; GunTriggerPosJog =0; GunTriggerPosAut =0; GunTriggerPosTrack =0; DrivePower = 0; DrivePowerJog = 0; DrivePowerAut = 0; DrivePowerTrack=0; // posiciones servo de direccion

// posiciones servo camara

// posiciones servo disparo lanzador

// potencia motor de traccion

GunDrive=1; // motor carga lanzador GunDriveJog=1; GunDriveAut=0; GunDriveTrack=0; // valores a enviar a la placa de E/S

int DriPWR = 0; int DriGUN = 0; int DriOUT = 0; int int int int BitA0; BitA1; BitA2; BitA3;

// estas variables estan relacionadas con DriOUT pues son sus bits

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp // 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 int int int int int int int int DuttiC2GP4=100; DuttiC2GP5=100; DuttiC3GP4=100; DuttiC3GP5=100; DuttiC0GP4=100; DuttiC0GP5=0xFF; DuttiC1GP4=100; DuttiC1GP5=0xFF; DirA3A2A1A0=0; //servo camara // servo direccion // servo gatillo chutador dirmemo 60 dirmemo 61 dirmemo 62

// motor PWM avance // motor PWM carg. muelle // salidas activar motores

dirmemo 65 dirmemo 67 dirmemo 68

bool RefreshC2GP4=false; bool RefreshC2GP5=false; bool RefreshC3GP4=false; bool RefreshC0GP5=false; bool RefreshC1GP5=false; bool RefreshA3A2A1A0=false; char char char char char char StrServoC2GP4[32]; StrServoC2GP5[32]; StrServoC3GP4[32]; StrServoC0GP5[32]; StrServoC1GP5[32]; 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) {

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp if (position >0) HoughCirc_MinRad=position; } void FctHoughCircNewMaxRad (int position) { if (position >0) HoughCirc_MaxRad=position; }

// funciones callback trackbars jog void 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 balon void 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 porteria void TrackGoalColorRP_CallBack (int pos) { GoalColorR_Plus=pos; } void TrackGoalColorGP_CallBack (int pos) { GoalColorG_Plus=pos; }

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp 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 derecha void 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; case 5: if (cvGetWindowHandle("WTrackBallFineColor")!=NULL) cvDestroyWindow ("WTrackBallFineColor"); if (cvGetWindowHandle("WTrackJog") !=NULL) cvDestroyWindow( "WTrackJog"); if (cvGetWindowHandle("WTrackGoalFineColor")!=NULL) cvDestroyWindow ("WTrackGoalFineColor"); if (cvGetWindowHandle("WTrackHoughCirc")!=NULL) cvDestroyWindow(

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp "WTrackHoughCirc"); GoalColorTrFltrCreate=true; break;

case 6: if (cvGetWindowHandle("WTrackBallFineColor")!=NULL) cvDestroyWindow ("WTrackBallFineColor"); if (cvGetWindowHandle("WTrackJog") !=NULL) cvDestroyWindow( "WTrackJog"); if (cvGetWindowHandle("WTrackGoalFineColor")!=NULL) cvDestroyWindow ("WTrackGoalFineColor"); if (cvGetWindowHandle("WTrackHoughCirc")!=NULL) cvDestroyWindow( "WTrackHoughCirc"); JogMovTrCreate=true; break; case 7: // boton comando cambiar de imagen fuente para el HoughCircles if (HoughCircSrcImg < 3) HoughCircSrcImg++; else HoughCircSrcImg=0; break; case 8: if (cvGetWindowHandle("WTrackBallFineColor")!=NULL) cvDestroyWindow ("WTrackBallFineColor"); if (cvGetWindowHandle("WTrackJog") !=NULL) cvDestroyWindow( "WTrackJog"); if (cvGetWindowHandle("WTrackGoalFineColor")!=NULL) cvDestroyWindow ("WTrackGoalFineColor"); if (cvGetWindowHandle("WTrackHoughCirc")!=NULL) cvDestroyWindow( "WTrackHoughCirc"); HoughCircTrCreate=true; break; default: break; } } sprintf(StrCoordMouse,"Fila %d",FilaToolBar); } // funcion callback raton en ventana live void 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 */

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp 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);

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp 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);

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp // 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);

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp } 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; }

10

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

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp 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; }

11

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,

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp 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; }

12

// 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 int int int int int int int BitB0; BitB1; BitB2; BitB3; BitB4; BitB5; BitB6; BitB7;

ValDat06=16*HexToInt(StrRec06[3])+HexToInt(StrRec06[4]); BitB5=(ValDat06 BitB4=(ValDat06 BitB3=(ValDat06 LANZADOR ATRAS) BitB2=(ValDat06 LANZADOR ADELANTE) BitB0=(ValDat06 & 32)/32; & 16)/16; & 8)/8; & 4)/4; & 1); // B5 ENTRADA DIGITAL DIRECTA (OBSTACULO DETRAS) // B4 ENTRADA DIGITAL DIRECTA (OBSTACULO DELANTE) // B3 ENTRADA DIGITAL DIRECTA (CARGADOR MUELLE // B2 ENTRADA DIGITAL DIRECTA (CARGADOR MUELLE // 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

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp 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;

13

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,

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp // 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.

14

/* ********************************* 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 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));

--

",cvPoint(0,75),Fuente,CV_RGB(255,255,

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp /* Infomacion color target porteria */ cvPutText(ImgStatus," -- InfoGoal -,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);

15

",cvPoint(0,120),Fuente,CV_RGB(255,255

/* 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)); }

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp /* 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; DriOUT=DriOUT & 0xFC; break; case 1: DriPWR=0xFE; DriOUT=DriOUT & 0xFC; DriOUT=DriOUT | 0x01; break; case 2: DriPWR=0xF0; DriOUT=DriOUT & 0xFC; DriOUT=DriOUT | 0x01; break; default: DriPWR=0xFF; DriOUT=DriOUT & 0xFC; break; } switch (GunDrive) { case 0: DriGUN=0xF0; DriOUT=DriOUT & 0xF3; DriOUT=DriOUT | 0x04; break; case 1: DriGUN=0xFF; DriOUT=DriOUT & 0xF3; // ponemos dutti minima potencia // borramos las 2 salidas A2 A3 // ponemos dutti minima potencia // borramos las 2 salidas A1 A0 // ponemos dutti maxima potencia // borramos las 2 salidas A1 A0 // activamos la salida 0 // ponemos dutti media potencia // borramos las 2 salidas A1 A0 // activamos la salida 0

16

// ponemos dutti minima potencia // borramos las 2 salidas A1 A0

// ponemos dutti maxima potencia // borramos las 2 salidas A2 A3 // activo salida A2

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp break; case 2: DriGUN=0xF0; DriOUT=DriOUT & 0xF3; DriOUT=DriOUT | 0x08; break; default: DriGUN=0xFF; DriOUT=DriOUT & 0xF3; break; } BitA3=(DriOUT ADELANTE) BitA2=(DriOUT ATRAS) BitA1=(DriOUT BitA0=(DriOUT & 8)/8; & 4)/4; & 2)/2; & 1); // A3 SALIDA DIGITAL CARGADOR MUELLE LANZADOR // A2 SALIDA DIGITAL CARGADOR MUELLE LANZADOR // A1 SALIDA DIGITAL AVANCE ATRAS // A0 SALIDA DIGITAL AVANCE ADELANTE // ponemos dutti maxima potencia // borramos las 2 salidas A2 A3 // activo salida A3

17

// ponemos dutti minima potencia // borramos las 2 salidas A2 A3

/* 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);

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp 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;

18

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp 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(StrServoC0GP5,"w65%02X",DuttiC0GP5); cvPutText(ImgNavigator,StrServoC0GP5,cvPoint(0,200),Fuente,CV_RGB(255,255,255)); sprintf(StrServoC1GP5,"w67%02X",DuttiC1GP5); cvPutText(ImgNavigator,StrServoC1GP5,cvPoint(0,215),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]); waitKey(1); SerialSendByte(hPort,StrServoC2GP4[1]); waitKey(1); SerialSendByte(hPort,StrServoC2GP4[2]); waitKey(1); SerialSendByte(hPort,StrServoC2GP4[3]); waitKey(1); SerialSendByte(hPort,StrServoC2GP4[4]); waitKey(1); SerialSendByte(hPort,0x0D); waitKey(1); RefreshC2GP4=false; } if (RefreshC2GP5) { SerialSendByte(hPort,StrServoC2GP5[0]); waitKey(1); SerialSendByte(hPort,StrServoC2GP5[1]); waitKey(1); SerialSendByte(hPort,StrServoC2GP5[2]); waitKey(1); SerialSendByte(hPort,StrServoC2GP5[3]); waitKey(1); SerialSendByte(hPort,StrServoC2GP5[4]); waitKey(1); SerialSendByte(hPort,0x0D); waitKey(1); RefreshC2GP5=false; } */ if (RefreshA3A2A1A0) { SerialSendByte(hPort,StrDirA3A2A1A0[0]); waitKey(1); SerialSendByte(hPort,StrDirA3A2A1A0[1]); waitKey(1); SerialSendByte(hPort,StrDirA3A2A1A0[2]); waitKey(1); SerialSendByte(hPort,StrDirA3A2A1A0[3]);

19

// w // DIR // DIR // DAT // DAT // CR

// w // DIR // DIR // DAT // DAT // CR

// w // DIR // DIR // DAT

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp waitKey(1); SerialSendByte(hPort,StrDirA3A2A1A0[4]); waitKey(1); SerialSendByte(hPort,0x0D); waitKey(5); RefreshA3A2A1A0=false; } // DAT // CR

20

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]); waitKey(1); SerialSendByte(hPort,StrServoC2GP4[4]); waitKey(1); SerialSendByte(hPort,StrServoC2GP5[3]); waitKey(1); SerialSendByte(hPort,StrServoC2GP5[4]); waitKey(1); SerialSendByte(hPort,StrServoC3GP4[3]); waitKey(1); SerialSendByte(hPort,StrServoC3GP4[4]); waitKey(1); SerialSendByte(hPort,'0'); waitKey(1); SerialSendByte(hPort,'0'); waitKey(1); SerialSendByte(hPort,'0'); waitKey(1); SerialSendByte(hPort,'0'); waitKey(1); SerialSendByte(hPort,StrServoC0GP5[3]); waitKey(1); SerialSendByte(hPort,StrServoC0GP5[4]); waitKey(1); SerialSendByte(hPort,'0'); waitKey(1); SerialSendByte(hPort,'0'); waitKey(1); SerialSendByte(hPort,StrServoC1GP5[3]); waitKey(1); SerialSendByte(hPort,StrServoC1GP5[4]); waitKey(1); SerialSendByte(hPort,0x0D); 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)); // DAT // DAT C2GP4

// DAT // DAT

C2GP5

// DAT // DAT

C3GP4

// DAT // DAT

C3GP5

// DAT // DAT

C0GP4

// DAT // DAT

C0GP5

// DAT // DAT

C1GP4

// DAT // DAT

C1GP5

// CR

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp /* 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);

21

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:

D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp cvPutText(ImgRangeGoal,"InRangeGoal",cvPoint(0,15),Fuente,CV_RGB(128, 128,128));

22

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; }