int main (){
char c;
do{
printf("(1) Mover\n(2) Pegar\n(3) Solta\n(4) Repouso\n(0) Finalizar programa ");
c = getchar();
if( c == '1'){
MOVE();
}
else if( c == '2'){
PEGA();
}
else if( c == '3'){
SOLTA();
}
else if( c == '4'){
REPOUSO();
}
else if( c == '0'){
printf("Finalizando o programa!");
}
}while( c != '0' );
// FIM DO PROGRAMA //
printf("\nAcesso a porta serial /dev/ttyS0 finalizado\n");
fechar_porta(serial_fd);
printf("\nPROGRAMA FINALIZADO\n\n");
return 0;
}
int main(int argc, char** argv){
char x0[50], x1[50], x2[50], x3[50], x4[50], y0[50], y1[50], y2[50], y3[50], y4[50];
int retorno = 0, pos;
char cmd;
comando = (char*) malloc(sizeof(char)*BUFSIZE);
serial_fd = abrir_porta();
if(serial_fd == -1)
{
printf("Erro abrindo a porta serial /dev/ttyS0\nAbortando o programa...");
return -1;
}
else
{
if(configurar_porta(serial_fd) == -1){
printf("Erro inicializando a porta\n");
close(serial_fd);
return -1;
}
Mat image;
image= imread("branco.png",CV_LOAD_IMAGE_GRAYSCALE);
if(!image.data)
printf("Não foi possível carregar a imagem\n");
namedWindow("janela",WINDOW_AUTOSIZE);
pos = theta[0]*2000/180 + 500;
retorno = enviarComando(BAS_SERVO,pos);
pos = theta[1]*2000/180 + 500;
retorno = enviarComando(SHL_SERVO,pos);
pos = theta[2]*2000/180 + 500;
retorno = enviarComando(ELB_SERVO,pos);
pos = theta[3]*2000/180 + 500;
retorno = enviarComando(WRI_SERVO,pos);
pos = 500;
retorno = enviarComando(GRI_SERVO,pos);
while(terminar != -1){
// PRIMEIRO COMANDO - POSICAL INICIAL
printf("\n\n\nESCOLHA ENTRE AS OPCOES DE MOVIMENTO\n");
printf("a - INCREMENTAR BASE\n");
printf("z - DECREMENTAR BASE\n");
printf("s - IMCREMENTAR OMBRO\n");
printf("x - DECREMENTAR OMBRO\n");
printf("d - IMCREMENTAR COTOVELO\n");
printf("c - DECREMENTAR COTOVELO\n");
printf("f - INCREMENTAR PUNHO\n");
printf("v - DECREMENTAR PUNHO\n");
printf("g - ABRIR GARRA\n");
printf("b - FECHAR GARRA\n");
printf("1 - SALVAR VALOR 1\n");
printf("2 - SALVAR VALOR 2\n");
printf("3 - SALVAR VALOR 3\n");
printf("4 - SALVAR VALOR 4\n");
printf("5 - SALVAR VALOR 5\n");
printf("p - CRIAR IMAGEM\n");
printf("l - FINALIZAR APLICACAO\n \n");
retorno = system("/bin/stty raw");
cmd = getchar();
retorno = system("/bin/stty cooked");
if(retorno == -1)
break;
switch(cmd){
case 'a':
if(theta[0] < 180)
theta[0] = theta[0] + 1;
pos = theta[0]*2000/180 + 500;
retorno = enviarComando(BAS_SERVO,pos);
break;
case 'z':
if(theta[0] > 0)
theta[0] = theta[0] - 1;
pos = theta[0]*2000/180 + 500;
retorno = enviarComando(BAS_SERVO,pos);
break;
case 's':
if(theta[1] < 180)
theta[1] = theta[1] + 1;
pos = theta[1]*2000/180 + 500;
retorno = enviarComando(SHL_SERVO,pos);
break;
case 'x':
if(theta[1] > 0)
theta[1] = theta[1] - 1;
pos = theta[1]*2000/180 + 500;
retorno = enviarComando(SHL_SERVO,pos);
break;
case 'c':
if(theta[2] < 180)
theta[2] = theta[2] + 1;
pos = theta[2]*2000/180 + 500;
retorno = enviarComando(ELB_SERVO,pos);
break;
case 'd':
if(theta[2] > 0)
theta[2] = theta[2] - 1;
pos = theta[2]*2000/180 + 500;
retorno = enviarComando(ELB_SERVO,pos);
break;
case 'f':
if(theta[3] < 180)
theta[3] = theta[3] + 1;
pos = theta[3]*2000/180 + 500;
retorno = enviarComando(WRI_SERVO,pos);
break;
case 'v':
if(theta[3] > 0)
theta[3] = theta[3] - 1;
pos = theta[3]*2000/180 + 500;
retorno = enviarComando(WRI_SERVO,pos);
break;
case 'g':
theta[4] = 0;
pos = 1500;
retorno = enviarComando(GRI_SERVO,pos);
break;
case 'b':
theta[4] = 1;
pos = 2400;
retorno = enviarComando(GRI_SERVO,pos);
break;
case 'l':
terminar = -1;
break;
case '1':
definirPosicao(0);
printf("\nX[0]: %.1f Y[0]: %.1f", x[0], y[0]);
break;
case '2':
definirPosicao(1);
printf("\nX[1]: %.1f Y[1]: %.1f", x[1], y[1]);
break;
case '3':
definirPosicao(2);
printf("\nX[2]: %.1f Y[2]: %.1f", x[2], y[2]);
break;
case '4':
definirPosicao(3);
printf("\nX[3]: %.1f Y[3]: %.1f", x[3], y[3]);
break;
case '5':
definirPosicao(4);
printf("\nX[4]: %.1f Y[4]: %.1f", x[4], y[4]);
break;
case 'p':
x[0] *= 50; x[1] *= 50; x[2] *= 50; x[3] *= 50; x[4] *= 50;
y[0] *= 50; y[1] *= 50; y[2] *= 50; y[3] *= 50; y[4] *= 50;
int xInt[5], yInt[5];
xInt = x[0]; yInt = y[0];
xInt = x[1]; yInt = y[1];
xInt = x[2]; yInt = y[2];
xInt = x[3]; yInt = y[3];
xInt = x[4]; yInt = y[4];
linha(xInt[0], yInt[0], xInt[1], yInt[1], image); //Ponto 1 -> Ponto 2
linha(xInt[0], yInt[0], xInt[2], yInt[2], image); //Ponto 1 -> Ponto 3
linha(xInt[1], yInt[1], xInt[2], yInt[2], image); //Ponto 2 -> Ponto 3
linha(xInt[1], yInt[1], xInt[3], yInt[3], image); //Ponto 2 -> Ponto 4
linha(xInt[2], yInt[2], xInt[4], yInt[4], image); //Ponto 3 -> Ponto 5
linha(xInt[3], yInt[3], xInt[4], yInt[4], image); //Ponto 4 -> Ponto 5
imshow("janela", image);
waitKey();
break;
default:
printf("\nOPÇÃO INVALIDA\n");
break;
}
if(retorno == -1)
break;
}
fechar_porta(serial_fd);
printf("\nPROGRAMA FINALIZADO\n");
}
return 0;
}
int main()
{
ufrn_header();
// INICIO DO PROGRAMA DEMO //
printf("PROGRAMA INTOLERANTS INICIADO\n\n");
serial_fd = abrir_porta();
if (serial_fd == -1)
{
printf("Erro abrindo a porta serial /dev/ttyS0\nAbortando o programa...");
return -1;
}
else
{
printf("Porta serial /dev/ttyS0 aberta com sucesso\n");
if (configurar_porta(serial_fd) == -1)
{
printf("Erro inicializando a porta\n");
close(serial_fd);
return -1;
}
comando = (char*) malloc(sizeof(char) * BUFSIZE);
last_comando = (char*) malloc(sizeof(char) * BUFSIZE);
//////////////////////
// PRIMEIRO COMANDO //
//////////////////////
printf("\nPRIMEIRO COMANDO - POSICAL INICIAL\n");
sprintf(comando, "%s", STANDBY);
//Escrevendo com teste de escrita
if (enviar_comando(comando, serial_fd) != -1)
{
printf("Enviando de comando com teste de envio: %s\n", STANDBY);
}
else
{
printf("Problema no envio do comando\nAbortando o programa...");
return -1;
}
printf("Pressione enter para continuar...");
getchar();
memset(comando, 0, BUFSIZE);
sprintf(comando, "%s", HOME_POS);
//Escrevendo com teste de escrita
if (enviar_comando(comando, serial_fd) != -1)
{
printf("Enviando de comando com teste de envio: %s\n", HOME_POS);
}
else
{
printf("Problema no envio do comando\nAbortando o programa...");
return -1;
}
memset(comando, 0, BUFSIZE);
/////////////////////
// SEGUNDO COMANDO //
/////////////////////
//system("rm input.txt");
//system("cat /dev/null > input.txt");
//system("rm pos.txt");
//system("cat /dev/null > pos.txt");
do {
if(system ("clear"));
t1 = angSenseBas*PI/180;
t2 = angSenseShl*PI/180;
t3 = angSenseElb*PI/180;
t4 = angSenseWri*PI/180;
X = cos(t1)*(L3*cos(t2 + t3) + L2*cos(t2) + L4*cos(t2 + t3 + t4));
Y = sin(t1)*(L3*cos(t2 + t3) + L2*cos(t2) + L4*cos(t2 + t3 + t4));
Z = L1 + L3*sin(t2 + t3) + L2*sin(t2) + L4*sin(t2 + t3 + t4);
calc_tetas(X, Y, Z, phi);
printf("A coordenada x do ponto : %.2f \n", X);
printf("A coordenada y do ponto : %.2f \n", Y);
printf("A coordenada z do ponto : %.2f \n", Z);
printf("phi da ferramenta : %.2f \n", angSenseShl+angSenseElb+angSenseWri);
//printf("#0P%dS%d#1P%dS%d#2P%dS%d#3P%dS%d#4P%dS%dT%d\n", (int)sense[0], s, (int)sense[1], s, (int)sense[2], s, (int)sense[3], s, (int)sense[4], s, t);
printf("%s\n", last_comando);
printf("BASE -> (Q)ESQUERDA ; (A)DIREITA | LARGURA DO PULSO: %.2f \t | ANGULO: %.2f \t | ANGULOC: %.2f \n", sense[0], angSenseBas, teta[0]);
printf("OMBRO -> (W)CIMA ; (S)BAIXO | LARGURA DO PULSO: %.2f \t | ANGULO: %.2f \t | ANGULOC: %.2f \n", sense[1], angSenseShl, teta[1]);
printf("COTOVELO -> (E)CIMA ; (D)BAIXO | LARGURA DO PULSO: %.2f \t | ANGULO: %.2f \t | ANGULOC: %.2f \n", sense[2], angSenseElb, teta[2]);
printf("PUNHO -> (R)CIMA ; (F)BAIXO | LARGURA DO PULSO: %.2f \t | ANGULO: %.2f \t | ANGULOC: %.2f \n", sense[3], angSenseWri, teta[3]);
printf("GARRA -> (T)FECHAR ; (G)ABRIR | LARGURA DO PULSO: %d \t \n", (int)sense[4]);
printf("Digite space para sair\n");
if(system("/bin/stty raw"));
c = getchar();
if(system("/bin/stty cooked"));
if (c == 'q') {
sense[0] = sense[0] + 2;
pos = sense[0];
angSenseBas = (offsetBas - sense[0]) * gainBas;
sprintf(comando, "#%dP%d", BAS_SERVO, trava(BAS_SERVO, pos));
} else if (c == 'a') {
sense[0] = sense[0] - 2;
pos = sense[0];
angSenseBas = (offsetBas - sense[0]) * gainBas;
sprintf(comando, "#%dP%d", BAS_SERVO, trava(BAS_SERVO, pos));
} else if (c == 'w') {
sense[1] = sense[1] + 2;
pos = sense[1];
// angSenseShl = (sense[1] - offsetShl) * gainShl;
angSenseShl = 762.684+7.948*sense[1];
sprintf(comando, "#%dP%d", SHL_SERVO, pos * (pos < 1850) + 1850 * (pos >= 1850));
} else if (c == 's') {
sense[1] = sense[1] - 2;
pos = sense[1];
// angSenseShl = (sense[1] - offsetShl) * gainShl;
angSenseShl = 762.684+7.948*sense[1];
sprintf(comando, "#%dP%d", SHL_SERVO, pos * (pos < 1850) + 1850 * (pos >= 1850));
} else if (c == 'e') {
sense[2] = sense[2] - 2;
pos = sense[2];
// angSenseElb = (offsetElb - sense[2]) * gainElb - 180;
angSenseElb = 854.377-9.231*sense[2];
sprintf(comando, "#%dP%d", ELB_SERVO, pos);
} else if (c == 'd') {
sense[2] = sense[2] + 2;
pos = sense[2];
// angSenseElb = (offsetElb - sense[2]) * gainElb - 180;
angSenseElb = 854.377-9.231*sense[2];
sprintf(comando, "#%dP%d", ELB_SERVO, pos);
} else if (c == 'r') {
sense[3] = sense[3] + 2;
pos = sense[3];
// angSenseWri = (sense[3] - offsetWri) * gainWri - 90;
angSenseWri = 1465.639+10.114*sense[3];
sprintf(comando, "#%dP%d", WRI_SERVO, trava(WRI_SERVO, pos));
} else if (c == 'f') {
sense[3] = sense[3] - 2;
pos = sense[3];
// angSenseWri = (sense[3] - offsetWri) * gainWri - 90;
angSenseWri = 1465.639+10.114*sense[3];
sprintf(comando, "#%dP%d", WRI_SERVO, trava(WRI_SERVO, pos));
} else if (c == 't') {
sense[4] = sense[4] + 10;
pos = sense[4];
sprintf(comando, "#%dP%d", GRI_SERVO, trava(GRI_SERVO, pos));
} else if (c == 'g') {
sense[4] = sense[4] - 10;
pos = sense[4];
sprintf(comando, "#%dP%d", GRI_SERVO, trava(GRI_SERVO, pos));
} else if (c == 'p') {
if(system("gnome-terminal -x /home/aluno/intolerants/meta3/meta3"));
} else if (c == '=') {
FILE *f = fopen("input.txt", "a");
if (f == NULL)
{
printf("Error opening file!\n");
exit(1);
}
/* print some text */
fprintf(f, "%d %d\n", (int)X+OFFSETX, (int)Y+OFFSETY);
fclose(f);
FILE *f2 = fopen("pos.txt", "a");
if (f2 == NULL)
{
printf("Error opening file!\n");
exit(1);
}
/* print some text */
fprintf(f2, "#0P%uT1000#1P%uT1000#2P%uT1000#3P%uT1000#4P%uT1000\n", (unsigned int)sense[0], (unsigned int)sense[1], (unsigned int)sense[2], (unsigned int)sense[3], (unsigned int)sense[4]);
fclose(f2);
} else if (c == 'b') {
int *bolas, i, n;
printf("\nDigite quantas bolinhas:\n");
scanf("%d", &n);
bolas = malloc(n*sizeof(int));
printf("\nDigite a ordem das bolinhas:\n");
for (i = 0; i < n; i++)
scanf("%d", &bolas[i]);
//bolas[i] = i+1;
desenha(bolas, n);
} else if (c == '0') {
// sense[0] = 1528;
// sense[1] = 1796;
// sense[2] = 2072;
// sense[3] = 844;
// sense[4] = 1870;
// // send_command(1528,1796,2072,844,1870);
// int delay = 2000;
// sprintf(comando,"#0P%dT%d#1P%dT%d#2P%dT%d#3P%dT%d#4P%dT%d", sense[0], s, sense[1], s, sense[2], s, sense[3], s, sense[4], s, t);
repouso();
} else if (c == '1') {
move(2.97,-22.55,9,-5);
sleep(3);
move(6.02,-27.16,9.63,-10.38);
sleep(3);
} else if (c == 'h') {
printf("Insira s e t: ");
if(scanf("%d", &s));
if(scanf("%d", &t));
sprintf(comando, "#0P1528S%d#1P1468S%d#2P1672S%d#3P1504S%d#4P1870S%dT%d", s, s, s, s, s, t);
} else if (c == 'z') {
pega();
// getchar();
} else if (c == 'x') {
solta();
// printf("solta\n");
// getchar();
} else if (c == 'm'){
printf("\n\n");
printf("x: ");
if(scanf("%f", &x));
printf("y: ");
if(scanf("%f", &y));
printf("z: ");
if(scanf("%f", &z));
printf("phi: ");
if(scanf("%f", &phi));
move(x,y,z,phi);
printf("%s\n", comando);
}
if (enviar_comando(comando, serial_fd) != -1)
{
printf("\nEnviando de comando com teste\n");
}
else
{
printf("Problema no envio do comando\nAbortando o programa...");
return -1;
}
// while ( (c = getchar()) != '\n');
memset(comando, 0, BUFSIZE);
} while ( c != ' ');
sprintf(comando, "%s", STANDBY);
enviar_comando(comando, serial_fd);
memset(comando, 0, BUFSIZE);
printf("Pressione enter para relaxar...");
getchar();
sprintf(comando, "%s", RELAX);
enviar_comando(comando, serial_fd);
// FIM DO PROGRAMA DEMO //
fechar_porta(serial_fd);
printf("\nAcesso a porta serial /dev/ttyS0 finalizado\n");
}
printf("\nPROGRAMA FINALIZADO\n\n");
return 0;
}