trackbolav3 opencv
Post on 14-Apr-2015
74 Views
Preview:
TRANSCRIPT
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;
2D:\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 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;
3D:\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 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) {
4D:\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 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; }
5D:\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 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;
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(
6D:\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 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 */
7D:\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);
8D:\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);
9D:\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);
10D:\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; }
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
11D:\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; }
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,
12D:\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;
}
// 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
13D:\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;
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,
14D:\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.
/* ********************************* 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));
15D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp
/* 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)); }
16D:\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; // 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
17D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp
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);
18D:\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;
19D:\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]); // 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
20D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp
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,StrServoC2GP5[3]); // DAT C2GP5 waitKey(1); SerialSendByte(hPort,StrServoC2GP5[4]); // DAT waitKey(1);
SerialSendByte(hPort,StrServoC3GP4[3]); // DAT C3GP4 waitKey(1); SerialSendByte(hPort,StrServoC3GP4[4]); // DAT waitKey(1);
SerialSendByte(hPort,'0'); // DAT C3GP5 waitKey(1); SerialSendByte(hPort,'0'); // DAT waitKey(1);
SerialSendByte(hPort,'0'); // DAT C0GP4 waitKey(1); SerialSendByte(hPort,'0'); // DAT waitKey(1);
SerialSendByte(hPort,StrServoC0GP5[3]); // DAT C0GP5 waitKey(1); SerialSendByte(hPort,StrServoC0GP5[4]); // DAT waitKey(1);
SerialSendByte(hPort,'0'); // DAT C1GP4 waitKey(1); SerialSendByte(hPort,'0'); // DAT waitKey(1);
SerialSendByte(hPort,StrServoC1GP5[3]); // DAT C1GP5 waitKey(1); SerialSendByte(hPort,StrServoC1GP5[4]); // 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));
21D:\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);
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:
22D:\VisualStudioProjects\TrackBolaV3\TrackBolaV3\TrackBolaV3_20121119_1625.cpp
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;}
top related