/** * =================================================================================================== * * \file RobCommFunc.c * \brief Ficheiro que contém todas as funções necessárias para executar os movimentos do robot * * =================================================================================================== */ #include #include #include "RobCommClient.h" #include "header.h" /** * @brief Função para alterar a velocidade linear do robot para adquirir dados com o sensor * * @param Widget and user data * @return void */ void SetLinearSpeedRobot(int sock, int SpeedInput) { char string_send[BUFSIZE]; // printf("Speed:%d",SpeedInput); sprintf(string_send,"SETREG\n1 50 %d 0 0 0 0 0 0 1 1 0 0 0\n",SpeedInput); printf("String a enviar :\n %s\n",string_send); size_t echoStringLen = strlen(string_send); ssize_t numBytes = send(sock, string_send, echoStringLen, 0); if(numBytes < 0) ExitWithSystemMessage("send() failed"); else if(numBytes != echoStringLen) ExitWithUserMessage("send()", "sent unexpected number of bytes"); printf("\nEspera pela resposta do robot...\n"); sleep(1); WaitRobotComm(sock); printf("\nRobot pronto para novo movimento...\n\n"); } /** * @brief Função para alterar a velocidade dos movimentos do robot * * @param Widget and user data * @return void */ void SetSpeedRobot(int sock, int SpeedInput) { char inputStr[1024], string_send[BUFSIZE]; /* strcpy(inputStr,"SETLSPEED\n"); printf("Speed:%d",SpeedInput); sprintf(string_send,"%s\n%d\n",inputStr,777);*/ // sprintf(string_send,"SETLSPEED\n777\n"); shm_robEnv->veloc_robot=SpeedInput; // size_t echoStringLen = strlen(string_send); // ssize_t numBytes = send(sock, string_send, echoStringLen, 0); // // if(numBytes < 0) ExitWithSystemMessage("send() failed"); // else if(numBytes != echoStringLen) ExitWithUserMessage("send()", "sent unexpected number of bytes"); // // printf("\nEspera pela resposta do robot...\n"); // // WaitRobotComm(sock); // printf("\nRobot pronto para novo movimento...\n\n"); } /** * @brief Função para pedir a posicao do robot * * @param Widget and user data * @return void */ void GetPosition(int sock, double* x,double* y,double* z,double* rx,double* ry,double* rz,char* r1,char* r2,char* r3,int* r4,int* r5,int* r6) { char r1_in, r2_in, r3_in, inputStr[1024], buffer[BUFSIZE]="", PartBuffer[BUFSIZE], temp[BUFSIZE], continuar[20]; double x_in, y_in,z_in,rx_in,ry_in,rz_in; int r4_in, r5_in, r6_in, size_str; strcpy(inputStr,"GETCRCPOS\n"); // printf("Envia pedido: %s\n",inputStr); // Determine input length size_t echoStringLen = strlen(inputStr); ssize_t numBytes = send(sock, inputStr, echoStringLen, 0); if(numBytes < 0) ExitWithSystemMessage("send() failed"); else if(numBytes != echoStringLen) ExitWithUserMessage("send()", "sent unexpected number of bytes"); do{ numBytes = recv(sock, PartBuffer, BUFSIZE - 1, 0 ); if(numBytes < 0) ExitWithSystemMessage("recv() failed"); else if(numBytes == 0) ExitWithUserMessage("recv()", "connection closed prematurely"); PartBuffer[numBytes] = '\0'; // Terminate the string for propor manipulation! strcat(buffer,PartBuffer); size_str=strlen(buffer); int i; if((strncmp(buffer,"\n",1)==0) & (size_str >95)) { printf("Barra N ............ Encontrado\n"); for(i=0; i 95)//93 { printf("String encontrada\n"); break; } }while(1); if(strncmp(buffer,"Current position",16)==0) { sscanf(buffer,"%*s %*s %lf %lf %lf %lf %lf %lf %c %c %c %*c %d %*c %d %*c %d",&x_in,&y_in,&z_in ,&rx_in,&ry_in,&rz_in ,&r1_in,&r2_in,&r3_in, &r4_in,&r5_in,&r6_in); printf("x:%lf\ny:%lf\nz:%lf\nrx:%lf\nry:%lf\nrz:%lf\nr1:%c\nr2:%c\nr3:%c\nr4:%d\nr5:%d\nr6:%d\n",x_in,y_in,z_in,rx_in,ry_in,rz_in,r1_in,r2_in,r3_in,r4_in,r5_in,r6_in); } else { printf("\nNão foi encontrada a posição!... \t"); scanf("%s",continuar); } *x=x_in; *y=y_in; *z=z_in; *rx=rx_in; *ry=ry_in; *rz=rz_in; *r1=r1_in; *r2=r2_in; *r3=r3_in; *r4=r4_in; *r5=r5_in; *r6=r6_in; } /** * @brief Função para esperar pela resposta do robot após ser feito um pedido * * @param Widget and user data * @return void */ void WaitRobotComm(int sock) { char BufferWait[BUFSIZE]="", PartBuffer[BUFSIZE], TempBuffer[BUFSIZE], continuar[20]; int size_str, i; for(;;) { ssize_t numBytes = recv(sock, PartBuffer, BUFSIZE - 1, 0 ); if(numBytes < 0) ExitWithSystemMessage("recv() failed"); else if(numBytes == 0) ExitWithUserMessage("recv()", "connection closed prematurely"); PartBuffer[numBytes] = '\0'; // Terminate the string for propor manipulation! if( (strncmp(PartBuffer,"\n",1)==0) ) { /*Elimina o \n que aparece casualmente no inicio das mensagens */ for (i = 0 ; i < numBytes && PartBuffer[i]!='\0'; i++) TempBuffer[i] = PartBuffer[i+1]; TempBuffer[i+1]='\0'; strcpy(PartBuffer, TempBuffer); } strcat(BufferWait,PartBuffer); size_str=strlen(BufferWait); printf("\nMensagem de espera:_%s_\t%d\n", BufferWait, size_str); /*Movimento Relativo*/ if(strncmp(BufferWait,"Moving...OK!",12)==0) { printf("Robot terminou movimentação !! \n"); break; } else if(strncmp(BufferWait,"1",1)==0) { printf("Registo definido !! \n"); break; } else if(strncmp(BufferWait,"Running JSMM1...OK!",19)==0) { printf("Robot executou movimento linear !! \n"); break; } else if(strncmp(BufferWait,"Setting speed...1",17)==0) { printf("Robot definiu velocidade !! \n"); break; } else if(strncmp(BufferWait,"Setting TCP[ 1]...1",19)==0) { printf("Robot actualizou Tool Center Pointer \n"); break; } else if(strncmp(BufferWait,"Moving...ERROR!",15)==0) { printf("\n--------------------------------------------- \n "); printf("Nao e possivel mover o robot para esta posiçao!!...\n "); printf("\n--------------------------------------------- \n "); printf("Nao e possivel mover o robot para esta posiçao!!...\n "); scanf("%s",continuar); break; } } } /** * @brief Função para realizar movimentos incrementais com o robot * * @param Widget and user data * @return void */ void MovimentoRelativo(int sock,int x_coord,int y_coord,int z_coord,int x_rot,int y_rot,int z_rot) { char r1, r2, r3, string_send[BUFSIZE],continuar[20]; double x, y,z,rx,ry,rz; int r1_send, r2_send, r3_send, r4, r5, r6, wait=0; GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6); /*Soma com o valor da posicao actual do robot*/ x=x+x_coord; y=y+y_coord; z=z+z_coord; rx=rx+x_rot; ry=ry+y_rot; rz=rz+z_rot; //Verifica se o robot pode ir para as coordenadas dadas Condicao_Seguranca_Coordenadas( x, y, z, rx, ry, rz); if (r1=='F') r1_send=1; else r1_send=0; if (r2=='U') r2_send=1; else r2_send=0; if (r3=='T') r3_send=1; else r3_send=0; sprintf(string_send,"MOVTOCPOS\n%d %d %d\n%d %d %d\n%d %d %d\n%d %d %d\n%d %d %d\n", (int)x, (int)y, (int)z, (int)rx, (int)ry, (int)rz, r1_send, r2_send, r3_send,r4, r5, r6,wait,shm_robEnv->veloc_robot,0); printf("String a enviar :\n %s\n",string_send); /* printf("\nPrima enter para enviar... \t"); scanf("%s",continuar);*/ /* Envia as coordenadas*/ size_t string_sendLen = strlen(string_send); ssize_t numBytes = send(sock, string_send, string_sendLen, 0); if(numBytes < 0) ExitWithSystemMessage("send() failed"); else if(numBytes != string_sendLen) ExitWithUserMessage("send()", "sent unexpected number of bytes"); printf("\nEspera pela resposta do robot...\n"); /*Receive the a string of end movement*/ sleep(1); WaitRobotComm(sock); printf("\nRobot pronto para novo movimento...\n\n"); } /** * @brief Função para executar movimentos absolutos do robot * * @param Widget and user data * @return void */ void MovimentoAbsoluto(int sock,int x_coord,int y_coord,int z_coord,int x_rot,int y_rot,int z_rot) { char r1, r2, r3, str_send_begin[10]="MOVTOCPOS\n", string_send[BUFSIZE], continuar[20]; double x, y,z,rx,ry,rz; int r1_send, r2_send, r3_send,wait=0,r4,r5,r6; GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6); //Verifica se o robot pode ir para as coordenadas dadas Condicao_Seguranca_Coordenadas( x_coord, y_coord, z_coord, x_rot, y_rot, z_rot); if (r1=='F') r1_send=1; else r1_send=0; if (r2=='U') r2_send=1; else r2_send=0; if (r3=='T') r3_send=1; else r3_send=0; sprintf(string_send,"%s%d %d %d\n%d %d %d\n%d %d %d\n%d %d %d\n%d %d %d\n",str_send_begin,x_coord , y_coord, z_coord, x_rot, y_rot , z_rot, r1_send, r2_send, r3_send, r4, r5, r6,wait,shm_robEnv->veloc_robot,0); printf("String a enviar: %s",string_send); /* printf("\nPrima qualquer tecla para enviar... \t"); scanf("%s",continuar);*/ // Determine input length size_t string_sendLen = strlen(string_send); ssize_t numBytes = send(sock, string_send, string_sendLen, 0); if(numBytes < 0) ExitWithSystemMessage("send() failed"); else if(numBytes != string_sendLen) ExitWithUserMessage("send()", "sent unexpected number of bytes"); printf("\nEspera pela resposta do robot...\n"); WaitRobotComm(sock); printf("\nRobot pronto para novo movimento...\n\n"); } /** * @brief Função para executar movimentos relativos lineares do robot * * @param Widget and user data * @return void */ void MovimentoRelativoLinear(int sock, char* var, int x_coord, int y_coord, int z_coord, int x_rot, int y_rot, int z_rot) { int r1_send, r2_send, r3_send, r4, r5, r6; char r1, r2, r3, string_send[BUFSIZE], continuar[20], sendStr[1024]; double x, y,z,rx,ry,rz; //Retira a posicao inicial do varrimento GetPosition(sock, &x, &y, &z, &rx, &ry, &rz, &r1, &r2, &r3, &r4, &r5, &r6); if( (strcmp(var,"varrimento_x")==0) || (strcmp(var,"varrimento_y")==0) ){ shm_robEnv->data_int[0] =(int)x; shm_robEnv->data_int[1] =(int)y; shm_robEnv->data_int[2] =(int)z; printf("Retirei os valores das variáveis !\n"); } else if(strcmp(var,"varrimento_inclinado")==0){ //Considera-se um referencial local no centro do pneu shm_robEnv->data_int[0] =-20; } /*Soma com o valor da posicao actual do robot*/ x=x+x_coord; y=y+y_coord; z=z+z_coord; rx=rx+x_rot; ry=ry+y_rot; rz=rz+z_rot; if (r1=='F') r1_send=1; else r1_send=0; if (r2=='U') r2_send=1; else r2_send=0; if (r3=='T') r3_send=1; else r3_send=0; /*Envia string para o robot*/ sprintf(string_send,"SETREG\n1 90 %d %d %d %d %d %d 0 1 1 0 0 0\n", (int)x, (int)y, (int)z, (int)rx, (int)ry, (int)rz); printf("String a enviar :\n %s\n",string_send); /* printf("\nPrima enter para enviar... \t"); scanf("%s",continuar);*/ size_t string_sendLen = strlen(string_send); ssize_t numBytes = send(sock, string_send, string_sendLen, 0); /* Envia as coordenadas*/ if(numBytes < 0) ExitWithSystemMessage("send() failed"); else if(numBytes != string_sendLen) ExitWithUserMessage("send()", "sent unexpected number of bytes"); printf("\nEspera pela resposta do robot...\n"); /*Receive the a string of end movement*/ WaitRobotComm(sock); printf("\nRobot pronto para novo movimento...\n\n"); /*Envia string para realizar o movimento*/ strcpy(sendStr,"RUNTPP\nJSMM1\n"); printf("Envia string... \n"); /*Inicia a aquisicao de dados do sensor*/ if( (strcmp(var,"varrimento_x")==0) || (strcmp(var,"varrimento_inclinado")==0) ) shm_robEnv->RobotComm = INI_TICTOC_X; else if (strcmp(var,"varrimento_y")==0) shm_robEnv->RobotComm = INI_TICTOC_Y; else if (strcmp(var,"varrimento_null")==0) printf("Nao retira dados do sensor... \n"); size_t echoStringLen = strlen(sendStr); numBytes = send(sock, sendStr, echoStringLen, 0); if(numBytes < 0) ExitWithSystemMessage("send() failed"); else if(numBytes != echoStringLen) ExitWithUserMessage("send()", "sent unexpected number of bytes"); /*Receive the a string of end movement*/ WaitRobotComm(sock); printf("\nRobot pronto para novo movimento...\n\n"); /*Envia a posição final do varrimento*/ GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6); if( (strcmp(var,"varrimento_x")==0) || (strcmp(var,"varrimento_y")==0) ){ shm_robEnv->data_int[0] =(int)x; shm_robEnv->data_int[1] =(int)y; shm_robEnv->data_int[2] =(int)z; printf("Retirei os valores das variáveis !\n"); } else if(strcmp(var,"varrimento_inclinado")==0){ //Deslocamento em relação ao referencial local no centro do pneu shm_robEnv->data_int[0] =120; printf("Posicao final do varrimento inclinado !\n"); } /*Termina a aquisicao de dados do sensor*/ if( (strcmp(var,"varrimento_x")==0) || (strcmp(var,"varrimento_inclinado")==0) ) { shm_robEnv->RobotComm = CLOSE_TICTOC_X; printf("\nMuda ShareMemory value: CLOSE_TICTOC_X\n"); } else if(strcmp(var,"varrimento_y")==0) { shm_robEnv->RobotComm = CLOSE_TICTOC_Y; printf("\nMuda ShareMemory value: CLOSE_TICTOC_Y\n"); } else if (strcmp(var,"varrimento_null")==0) printf("... \n"); } /** * @brief Função para editar o Tool Center Pointer * * @param Widget and user data * @return void */ void SetToolCenterPointer(int sock, int NumberTool, int x, int y, int z, int rx, int ry, int rz) { char string_send[BUFSIZE]; sprintf(string_send,"SETTOOLFRM\n%d\n%d %d %d\n%d %d %d\n",NumberTool, x, y, z, rx, ry, rz); printf("\nEnvia string para Tool Center Pointer:%s\n", string_send); size_t echoStringLen = strlen(string_send); ssize_t numBytes = send(sock, string_send, echoStringLen, 0); if(numBytes < 0) ExitWithSystemMessage("send() failed"); else if(numBytes != echoStringLen) ExitWithUserMessage("send()", "sent unexpected number of bytes"); printf("\nEspera pela resposta do robot...\n"); WaitRobotComm(sock); printf("\nRobot pronto para novo movimento...\n\n"); } /** * @brief Função para especificar um user frame * * @param Widget and user data * @return void */ void SetUserFrame(int sock, int x, int y, int z, int rx, int ry, int rz) { char string_send[BUFSIZE]; sprintf(string_send,"SETUSERFRM\n%d %d %d\n%d %d %d\n", x, y, z, rx, ry, rz); size_t echoStringLen = strlen(string_send); ssize_t numBytes = send(sock, string_send, echoStringLen, 0); if(numBytes < 0) ExitWithSystemMessage("send() failed"); else if(numBytes != echoStringLen) ExitWithUserMessage("send()", "sent unexpected number of bytes"); printf("Mensagem enviada: \n%s",string_send); printf("\nEspera pela resposta do robot...\n"); WaitRobotComm(sock); printf("\nRobot pronto para novo movimento...\n\n"); } /** * @brief Função para executar a rotação da garra * * @param Widget and user data * @return void */ void SetOrientacaoGarra( int sock, double**MatrizTransf, int Xp, int Yp, int Zp, double c, double b, double a) { int Zg=-250, posX, posY, posZ; double angX, angY, angZ; a=(a * M_PI) / 180; b=(b * M_PI) / 180; c=(c * M_PI) / 180; MatrizTransf[0][0]=cos(a)*cos(b); MatrizTransf[0][1]=-sin(a)*cos(c)+cos(a)*sin(b)*sin(c); MatrizTransf[0][2]=sin(a)*sin(c)+cos(a)*sin(b)*cos(c); MatrizTransf[0][3]=(sin(a)*sin(c)+cos(a)*sin(b)*cos(c))*Zg+Xp; MatrizTransf[1][0]=sin(a)*cos(b); MatrizTransf[1][1]=cos(a)*cos(c)+sin(a)*sin(b)*sin(c); MatrizTransf[1][2]=-cos(a)*sin(c)+sin(a)*sin(b)*cos(c); MatrizTransf[1][3]=(-cos(a)*sin(c)+sin(a)*sin(b)*cos(c))*Zg+Yp; MatrizTransf[2][0]=-sin(b); MatrizTransf[2][1]=cos(b)*sin(c); MatrizTransf[2][2]=cos(b)*cos(c); MatrizTransf[2][3]=cos(b)*cos(c)*Zg+Zp; MatrizTransf[3][0]=0; MatrizTransf[3][1]=0; MatrizTransf[3][2]=0; MatrizTransf[3][3]=1; // int i; /* for(i=0;i<4;i++) printf("%lf\t%lf\t%lf\t%lf\n", MatrizTransf[i][0], MatrizTransf[i][1], MatrizTransf[i][2], MatrizTransf[i][3]);*/ angX=( atan2( MatrizTransf[2][1], MatrizTransf[2][2] ) ); angZ= ( atan2( MatrizTransf[1][0], MatrizTransf[0][0] ) ); angY= ( atan2(-MatrizTransf[2][0], MatrizTransf[0][0]*cos(angZ)+MatrizTransf[1][0]*sin(angZ)) ); angX=round(angX * 180 / M_PI); angY=round(angY * 180 / M_PI); angZ=round(angZ * 180 / M_PI); posX=round(MatrizTransf[0][3]); posY=round(MatrizTransf[1][3]); posZ=round(MatrizTransf[2][3]); printf("angX: %lf\tangY: %lf\tangZ: %lf\n\n", angX, angY, angZ); printf("posX: %d\tposY: %d\tposZ: %d\n", posX, posY, posZ); MovimentoAbsoluto( sock, posX, posY, posZ, (int)angX, (int)angY, (int)angZ); } /** * @brief Função para executar a rotação da garra * * @param Widget and user data * @return void */ void DeslocamentoRefGarra(int sock, double** MatrizTransf, RobValues* DataRob, int EixoMovimento, int ValorDeslocamento, char* TypeVarrimento) { double Ponto[5], vect_n[5], vect_s[5], vect_a[5]; int NovoPonto[5], r4, r5, r6, DifCoord[5]; char r1, r2, r3; double x, y,z,rx,ry,rz; DataRob->DistObjRefGarra=0; GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6); Ponto[0]=x; Ponto[1]=y; Ponto[2]=z; vect_n[0]=MatrizTransf[0][0]; vect_n[1]=MatrizTransf[1][0]; vect_n[2]=MatrizTransf[2][0]; vect_s[0]=MatrizTransf[0][1]; vect_s[1]=MatrizTransf[1][1]; vect_s[2]=MatrizTransf[2][1]; vect_a[0]=MatrizTransf[0][2]; vect_a[1]=MatrizTransf[1][2]; vect_a[2]=MatrizTransf[2][2]; int i; //Movimentos absolutos no referencial da garra for(i=0;i<3;i++) { if( EixoMovimento==EIXO_X ){ NovoPonto[i]=round( Ponto[i] + ValorDeslocamento*vect_n[i] ); } else if( EixoMovimento==EIXO_Y ){ NovoPonto[i]=round( Ponto[i] + ValorDeslocamento*vect_s[i] ); } else if( EixoMovimento==EIXO_Z ){ NovoPonto[i]=round( Ponto[i] + ValorDeslocamento*vect_a[i] ); } } if( EixoMovimento==EIXO_X || EixoMovimento==EIXO_Y || EixoMovimento==EIXO_Z ) MovimentoAbsoluto( sock, NovoPonto[0], NovoPonto[1], NovoPonto[2], rx, ry, rz); //Movimentos relativos no referencial da garra for(i=0;i<3;i++) { if( EixoMovimento==EIXO_X_LINEAR ){ NovoPonto[i]=round( Ponto[i] + ValorDeslocamento*vect_n[i] ); DifCoord[i]=NovoPonto[i]-Ponto[i]; printf("Ponto[i]: %lf\tNovoPonto[i]: %d\tDifCoord[i]: %d\n",Ponto[i],NovoPonto[i],DifCoord[i]); } else if( EixoMovimento==EIXO_Y_LINEAR ){ NovoPonto[i]=round( Ponto[i] + ValorDeslocamento*vect_s[i] ); DifCoord[i]=NovoPonto[i]-Ponto[i]; printf("Ponto[i]: %lf\tNovoPonto[i]: %d\tDifCoord[i]: %d\n",Ponto[i],NovoPonto[i],DifCoord[i]); } else if( EixoMovimento==EIXO_Z_LINEAR ){ NovoPonto[i]=round( Ponto[i] + ValorDeslocamento*vect_a[i] ); DifCoord[i]=NovoPonto[i]-Ponto[i]; printf("Ponto[i]: %lf\tNovoPonto[i]: %d\tDifCoord[i]: %d\n",Ponto[i],NovoPonto[i],DifCoord[i]); } } if( EixoMovimento==EIXO_X_LINEAR || EixoMovimento==EIXO_Y_LINEAR || EixoMovimento==EIXO_Z_LINEAR ) { int bordosPneu[4], media; char continuar[20]; //esta é a coordenada X do centro do pneu num referencial local definido em função desse mesmo centro shm_robEnv->CentroObj[0] = 0; shm_robEnv->DistSensorObj[0] = -1; MovimentoRelativoLinear(sock, TypeVarrimento, DifCoord[0], DifCoord[1], DifCoord[2], 0, 0, 0); if(strcmp(TypeVarrimento,"varrimento_inclinado")==0) { while(shm_robEnv->DistSensorObj[0] == -1) { usleep(1000); printf("A espera de actualizar o valor\n"); } bordosPneu[0]=shm_robEnv->DistSensorObj[0]; bordosPneu[1]=shm_robEnv->DistSensorObj[1]; printf("Distancia do Sensor >> MinAnteriorY:%d MinPosteriorY:%d\n",bordosPneu[0],bordosPneu[1]); //Rerira o valor da altura pelos dados obtidos if(bordosPneu[0] != 10000 && bordosPneu[1] != 10000) { media=( bordosPneu[0] + bordosPneu[1] ) / 2; printf("\nProfundidade em ZZ da garra... : %d\n\n",media); DataRob->DistObjRefGarra=media-110+35; //Tem de se retirar o comprimento da garra } else if(bordosPneu[0] == 10000 && bordosPneu[1] != 10000) { media=bordosPneu[1]; printf("\nProfundidade em ZZ da garra... : %d\n\n",media); DataRob->DistObjRefGarra=media-110+35; //Tem de se retirar o comprimento da garra } else if(bordosPneu[0] != 10000 && bordosPneu[1] == 10000) { media=bordosPneu[0]; printf("\nProfundidade em ZZ da garra... : %d\n\n",media); DataRob->DistObjRefGarra=media-110+35; //Tem de se retirar o comprimento da garra } else if(bordosPneu[0] == 10000 && bordosPneu[1] == 10000) { media=0; printf("Não encontrei valor para distancia... atraves do sensor\n"); DataRob->DistObjRefGarra=media; //Tem de se retirar o comprimento da garra } } else { printf("Nao precisa de esperar pela distancia ao pneu...\n"); } } } /** * @brief Função para ajustar a orientação da garra com o objecto a apanhar * * @param Widget and user data * @return void */ void Ajuste_Orientacao_GarraObj(int sock, RobValues* DataRob, double** ArrayRob, double** ArrayTemp, double** MatrizTransf) { int i, r4, r5, r6, ObjDetectX, ObjDetectY; char r1, r2, r3, continuar[20]; double x, y,z,rx,ry,rz; GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6); ObjDetectX=(int)x+75; ObjDetectY=(int)y-15; //Ajusta o Tool Center Poiter para realizar a rotação sengundo ZZ SetToolCenterPointer( sock, 1, 100, 0, 360, 0, 0, 0); //Roda segundo ZZ SetSpeedRobot(sock, 5);//VelocMedia printf("Velocidade Media:.................................... %d\n", DataRob->VelocMedia); MovimentoAbsoluto( sock, x+100, y, z, rx, ry, -DataRob->Angle); SetSpeedRobot(sock, DataRob->VelocTrab); //Estabiliza imagem sleep(0.5); //Faz ajuste fino da rotação segundo ZZ while( ( (DataRob->Angle) > 30 ) || ( (DataRob->Angle) < -30 ) ) { if( (DataRob->Angle) >=3 ) MovimentoRelativo(sock,0,0,0,0,0,-1); else if( (DataRob->Angle) <= -3 ) MovimentoRelativo(sock,0,0,0,0,0,1); GetDataSherlock(ArrayRob, DataRob); TrackingObjects( ArrayRob, ArrayTemp, DataRob); printf("Angulo do objecto: %lf\n", DataRob->Angle); } int rotX, rotY, rotZ; double ElipseAxRat=0; GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6); //-------------------------------------------------------------- for(i=0;i<5;i++) { GetDataSherlock(ArrayRob, DataRob); TrackingObjects( ArrayRob, ArrayTemp, DataRob); ElipseAxRat = ElipseAxRat + DataRob->ElipAxisRatio; printf("DataRob->ElipAxisRatio: %lf\n", DataRob->ElipAxisRatio); sleep(0.2); } printf("Roda para um lado: %d\n", DataRob->SentidoRotGarra); ElipseAxRat = ElipseAxRat / 5; printf("Roda para um lado: %d\n", DataRob->SentidoRotGarra); printf("ElipseAxRat: %lf\n", ElipseAxRat); printf("Valor do angulo: %lf\n", acos(ElipseAxRat)*180/M_PI); //-------------------------------------------------------------- //faz o calculo da rotação segundo YY pelas caracteristicas da elipse rotX= 180; rotZ= rz; printf("Rotacao YY: %d\n",rotY); printf("Rotacao ZZ: %d\n",rotZ); // printf("\nPrima enter para enviar... \t"); // scanf("%s",continuar); SetOrientacaoGarra( sock, MatrizTransf, x, y, z-250, 180, 0, rotZ); char varrimento_null[30]="varrimento_null", varrimento_incl[30]="varrimento_inclinado"; // DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X_LINEAR, 10, varrimento_null); DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X_LINEAR, 120, varrimento_incl); int BordoAnterior=shm_robEnv->DistSensorObj[2]; int BordoPosterior=shm_robEnv->DistSensorObj[3]; printf("\n\nBordo Anterior........................................ %d\n",BordoAnterior); printf("\n\nBordo Posterior........................................ %d\n",BordoPosterior); /* printf("\nPrima enter para enviar... \t"); scanf("%s",continuar); */ //Verifica o módulo da rotação segundo YY rotY= AbsValue( acos(ElipseAxRat)*180/M_PI); if( (rotY > - 50 && rotY < 50) && (rotZ > - 120 && rotZ < 120) ) { if( (BordoAnterior < BordoPosterior) && (rotZ >= 0) ) rotY= + acos(ElipseAxRat)*180/M_PI - 15; else if( (BordoAnterior < BordoPosterior) && (rotZ < 0) ) rotY= + acos(ElipseAxRat)*180/M_PI - 15; else if( (BordoAnterior > BordoPosterior) && (rotZ >= 0) ) rotY= - acos(ElipseAxRat)*180/M_PI + 15; else if( (BordoAnterior > BordoPosterior) && (rotZ < 0) ) rotY= - acos(ElipseAxRat)*180/M_PI + 15; SetSpeedRobot(sock, DataRob->VelocMedia-100); SetOrientacaoGarra( sock, MatrizTransf, x, y, z-220, rotX, rotY, rotZ); SetSpeedRobot(sock, DataRob->VelocTrab); for(i=0;i<4;i++) printf("%lf\t%lf\t%lf\t%lf\n", MatrizTransf[i][0], MatrizTransf[i][1], MatrizTransf[i][2], MatrizTransf[i][3]); } else if( (rotY <= - 50 || rotY >= 50) && (rotZ > - 120 && rotZ < 120) ) { printf("Não é possivel realizar uma rotação acima de 50 graus...\nRotacao maxima YY = 50 graus\n"); if( (BordoAnterior < BordoPosterior) && (rotZ >= 0) ) rotY= + 40; else if( (BordoAnterior < BordoPosterior) && (rotZ < 0) ) rotY= + 40; else if( (BordoAnterior > BordoPosterior) && (rotZ >= 0) ) rotY= - 40; else if( (BordoAnterior > BordoPosterior) && (rotZ < 0) ) rotY= - 40; SetSpeedRobot(sock, DataRob->VelocMedia-100); SetOrientacaoGarra( sock, MatrizTransf, x, y, z-220, rotX, rotY, rotZ); SetSpeedRobot(sock, DataRob->VelocTrab); for(i=0;i<4;i++) printf("%lf\t%lf\t%lf\t%lf\n", MatrizTransf[i][0], MatrizTransf[i][1], MatrizTransf[i][2], MatrizTransf[i][3]); } else { printf("ERRO - Ajuste_Orientacao_GarraObj: Rotação muito acentuada...\n"); printf("\nFaça Ctrl + C para sair... \t"); scanf("%s",continuar); } //Faz verificação apos rotação sleep(1.5); DataRob->TrackingState=0; //Esta propriedade é para o algritmo não combinar as propriedades da imagem DataRob->CombinacaoPropriedades=0; double ElipAxisRatio_Media=0, k; for(k=0;k<5;k++) { GetDataSherlock(ArrayRob, DataRob); TrackingObjects( ArrayRob, ArrayTemp, DataRob); ElipAxisRatio_Media = ElipAxisRatio_Media + DataRob->ElipAxisRatio; sleep(0.4); printf("DataRob->ElipAxisRatio: %lf\n", DataRob->ElipAxisRatio); } ElipAxisRatio_Media = ElipAxisRatio_Media/5; printf("ElipAxisRatio_Media: %lf\n", ElipAxisRatio_Media); if( ElipAxisRatio_Media < 0.50 ) { SetSpeedRobot(sock, DataRob->VelocMedia); MovimentoAbsoluto( sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0); DataRob->NovaTentativa=1; } sleep(1); DataRob->TrackingState=0; DataRob->CombinacaoPropriedades=1; } /** * @brief Função para verificar as coordenadas a enviar para o robot * * @param Widget and user data * @return void */ void Condicao_Seguranca_Coordenadas( double x, double y, double z, double rx, double ry, double rz) { char continuar[20]; if(x < 390) { printf("\n O robot vai para X=%lf",x); printf("\n--------------------------------------------- \n "); printf("É pegigoso o robot ir para esta posicao...(limitação XX) \n "); printf("--------------------------------------------- \n "); printf("\nTem a certeza que pretende realizar esta acção?\t"); scanf("%s",continuar); } if( (y < -450) || (y > 550) ) { printf("\n O robot vai para Y=%lf",y); printf("\n--------------------------------------------- \n "); printf("Limitação do robot segundo YY) \n "); printf("--------------------------------------------- \n "); printf("\nTem a certeza que pretende realizar esta acção?\t"); scanf("%s",continuar); } if(z < shm_robEnv->limiteZ) { printf("\n O robot vai para Z=%lf",z); printf("\n--------------------------------------------- \n "); printf("Robot vai colidir com o espaço de trabalho...(limitação ZZ) \n "); printf("--------------------------------------------- \n "); printf("\nTem a certeza que pretende realizar esta acção?\t"); scanf("%s",continuar); } if(ry > 50) { printf("\n Rotacao YY=%lf",ry); printf("\n--------------------------------------------- \n "); printf("\nRotação muito acentuada... \n "); printf("--------------------------------------------- \n "); printf("\nTem a certeza que pretende realizar esta acção?\t"); scanf("%s",continuar); } if(rz > 110) { printf("\n Rotacao ZZ=%lf",rz); printf("\n--------------------------------------------- \n "); printf("Rotação muito acentuada... \n "); printf("--------------------------------------------- \n "); printf("\nTem a certeza que pretende realizar esta acção?\t"); scanf("%s",continuar); } }