/** * =================================================================================================== * * \file RobCommClient.c * \brief Ficheiro que estabelece a comunicação TCP/IP com o robot fanuc * * =================================================================================================== * */ #ifndef _RobotComm_C_ #define _RobotComm_C_ #include "RobCommClient.h" #include "header.h" int RobotComunication(void) { char NameKey_robEnv[20]="RobCommFunc.c"; char NameKeyEnv[20]="RS232Comm.c"; shm_env = GetSharedMem(NameKeyEnv, &shm_id_env); shm_robEnv = GetSharedMem(NameKey_robEnv, &shm_id_robEnv); //Criação da Sharememory shm_robEnv int i, menu=1, sock, x_move, y_move , r4, r5, r6, ObjDetectX, ObjDetectY; char continuar[20]; char r1, r2, r3; double x, y,z,rx,ry,rz; double**ArrayRob=(double**)malloc(200*sizeof(double*)); for(i=0;i<200;i++) ArrayRob[i]=(double*)malloc(20*sizeof(double)); double**ArrayTemp=(double**)malloc(200*sizeof(double*)); for(i=0;i<200;i++) ArrayTemp[i]=(double*)malloc(20*sizeof(double)); double**MatrizTransf=(double**)malloc(10*sizeof(double*)); for(i=0;i<10;i++) MatrizTransf[i]=(double*)malloc(10*sizeof(double)); signal(SIGUSR1,Kill_RobotComm); signal(SIGINT,Kill_RobotComm); RobValues* DataRob; DataRob=malloc(sizeof(RobValues)); InicializeRobotProgram(ArrayRob, ArrayTemp, DataRob, &sock); //******************************************************************************* // SetSpeedRobot(sock, 100); // // MovimentoAbsoluto( sock, 640, 0, -250, -180, 0, 0); // // SetSpeedRobot(sock, 400); // // MovimentoAbsoluto( sock, 640, 300, -250, -180, 0, 0); // // SetSpeedRobot(sock, 600); // // MovimentoAbsoluto( sock, 640, 0, -250, -180, 0, 0); // // SetSpeedRobot(sock, 20); // // MovimentoAbsoluto( sock, 640, 300, -250, -180, 0, 0); // // printf("\nPrima enter para continuar ...\t"); // scanf("%s",continuar); //******************************************************************************* do { //Caso 1 - O robot fica em ciclo infinito se não reconhecer nenhum objecto no espaço de trabalho if ( (DataRob->NTotObj==0) && (DataRob->NObjTrab==0) ) { /*Testa de novo 5 vezes*/ for(i=0;i<5;i++) { usleep(150000); GetDataSherlock(ArrayRob, DataRob); TrackingObjects( ArrayRob, ArrayTemp, DataRob); printf("X: %d\tY: %d\tObjTr: %d\tTot: %d\tElipRatio: %lf\n", DataRob->ObjTrabX, DataRob->ObjTrabY, DataRob->NObjTrab, DataRob->NTotObj,DataRob->ElipAxisRatio); if ((DataRob->NTotObj!=0) && (DataRob->NObjTrab!=0)) { continue; } } printf("Não há objectos encontrados!\n"); GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6); if(z <= (DataRob->RefTrabZ-40)){ SetSpeedRobot(sock, DataRob->VelocMedia); MovimentoRelativo(sock,0,0,20,0,0,0); SetSpeedRobot(sock, DataRob->VelocTrab); } else if (z > (DataRob->RefTrabZ-40)){ SetSpeedRobot(sock, DataRob->VelocMedia); MovimentoAbsoluto( sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0); SetSpeedRobot(sock, DataRob->VelocTrab); } /*Dados do sherlock*/ GetDataSherlock(ArrayRob, DataRob); TrackingObjects( ArrayRob, ArrayTemp, DataRob); printf("X: %d\tY: %d\tObjTr: %d\tTot: %d\tElipRatio: %lf\n", DataRob->ObjTrabX, DataRob->ObjTrabY, DataRob->NObjTrab, DataRob->NTotObj,DataRob->ElipAxisRatio); continue; } //Caso2 - Derruba objectos que não estejam em condições de serem apanhados if ( (DataRob->NTotObj>0) && (DataRob->ElipAxisRatio<0.25) ) { int x_relative, y_relative; int x_temp=DataRob->ObjGlobX; /*Diferenca X entre o centro da imagem e o objecto*/ int y_temp=DataRob->ObjGlobY; /*Diferenca Y entre o centro da imagem e o objecto*/ printf("BlackObjX:%d\tBlackObjY:%d\n",x_temp, y_temp); Aproximacao_Inicial_Objecto( sock, DataRob, x_temp, y_temp); GetDataSherlock(ArrayRob, DataRob); TrackingObjects( ArrayRob, ArrayTemp, DataRob); x_temp=DataRob->ObjGlobX; y_temp=DataRob->ObjGlobY; Aproximacao_ao_Objecto(x_temp,y_temp, &x_relative, &y_relative); if((x_relative == 0) && (y_relative == 0)) { printf("\nObjecto desconhecido encontrado ...\n"); int largura_x=DataRob->ObjGlobLarguraX; int largura_y=DataRob->ObjGlobLarguraY; if ( largura_x > largura_y ) DerrubaObjectoSegundoYY( sock, DataRob, largura_x, largura_y); else DerrubaObjectoSegundoXX( sock, DataRob, largura_x, largura_y); } else { printf("\nMovimento de aproximação ao black object!!\n"); MovimentoRelativo(sock,x_relative,y_relative,0,0,0,0); } } //Caso3 - Apanha objectos independentemente da sua orientação if( (DataRob->NObjTrab>0) && (DataRob->LostState!=1) && (DataRob->ElipAxisRatio>=0.25) ) { printf("...CASO 3...\n"); int X_Value = DataRob->ObjTrabX; int Y_Value = DataRob->ObjTrabY; printf("...aqui!!!...\n"); Aproximacao_Inicial_Objecto( sock, DataRob, X_Value, Y_Value); /*Dados do sherlock*/ GetDataSherlock(ArrayRob, DataRob); TrackingObjects( ArrayRob, ArrayTemp, DataRob); printf("X: %d\tY: %d\tObjTr: %d\tTot: %d\tElipRatio: %lf\n", DataRob->ObjTrabX, DataRob->ObjTrabY, DataRob->NObjTrab, DataRob->NTotObj,DataRob->ElipAxisRatio); Aproximacao_ao_Objecto(DataRob->ObjTrabX, DataRob->ObjTrabY, &x_move, &y_move); if((x_move == 0) && (y_move == 0)) { printf("\nPneu encontrado!!\n"); GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6); ObjDetectX=(int)x+75; ObjDetectY=(int)y-15; if (DataRob->AproxObj==0) { printf("\n...Vai fazer aproximação...\n"); printf("\nValor de ZZ_Ref: %d\n", DataRob->RefTrabZ); SetSpeedRobot(sock, DataRob->VelocMedia); MovimentoAbsoluto( sock, x, y, DataRob->RefTrabZ-75, -180, -1, 0); SetSpeedRobot(sock, DataRob->VelocTrab); sleep(1); //Com esta condição só faz a aproximação uma vez DataRob->AproxObj=1; //Dados do sherlock GetDataSherlock(ArrayRob, DataRob); TrackingObjects( ArrayRob, ArrayTemp, DataRob); printf("X: %d\tY: %d\tObjTr: %d\tTot: %d\tElipRatio: %lf\n", DataRob->ObjTrabX, DataRob->ObjTrabY, DataRob->NObjTrab, DataRob->NTotObj,DataRob->ElipAxisRatio); } else if ( (DataRob->AproxObj==1) && (DataRob->ElipAxisRatio>0.85) ){ //Apanha objectos totalmente na horizontal ApanhaObjectoHoriz( sock, DataRob, ObjDetectX, ObjDetectY); } else if ( (DataRob->AproxObj==1) && (DataRob->ElipAxisRatio<=0.85) ) { //Apanha objectos num plano inclinado Ajuste_Orientacao_GarraObj( sock, DataRob, ArrayRob, ArrayTemp, MatrizTransf); //Se DataRob->NovaTentativa tiver o valor 1 o robot não encontrou nenhum objecto apos a rotação if( DataRob->NovaTentativa==0 ) Apanha_Obj_Inclinado( sock, DataRob, ArrayRob, ArrayTemp, MatrizTransf); } } else MovimentoRelativo(sock,x_move,y_move,0,0,0,0); } //Dados do sherlock GetDataSherlock(ArrayRob, DataRob); TrackingObjects( ArrayRob, ArrayTemp, DataRob); printf("X: %d\tY: %d\tObjTr: %d\tTot: %d\tElipRatio: %lf\n", DataRob->ObjTrabX, DataRob->ObjTrabY, DataRob->NObjTrab, DataRob->NTotObj,DataRob->ElipAxisRatio); }while(menu==1); //Termina programa shmdt (shm_robEnv); //Eliminar a Sharememory de recepção shm_robEnv if(shmctl(shm_id_robEnv, IPC_RMID, NULL)==0) printf("Foi limpa a sharememory de shm_robEnv !\n"); close(sock); exit(0); } #endif