void REPOUSO(void){
ufrn_header();
pbase = 1500;
pombro = 1500;
pcotovelo = 1500;
ppunho = 1500;
//Execução do comando para o braço robótico:
memset(comando, 0, BUFSIZE);
sprintf(comando,"#%dP%dT3000",BAS_SERVO,trava(BAS_SERVO,pbase));
enviar_comando(comando,serial_fd);
memset(comando, 0, BUFSIZE);
sprintf(comando,"#%dP%dT3000",SHL_SERVO,pombro);
enviar_comando(comando,serial_fd);
memset(comando, 0, BUFSIZE);
sprintf(comando,"#%dP%dT3000",ELB_SERVO,trava(ELB_SERVO,pcotovelo));
enviar_comando(comando,serial_fd);
memset(comando, 0, BUFSIZE);
sprintf(comando,"#%dP%dT3000",WRI_SERVO,trava(WRI_SERVO,ppunho));
enviar_comando(comando,serial_fd);
}
void repouso(){
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);
memset(comando, 0, BUFSIZE);
printf("Pressione enter para acordar...");
getchar();
sprintf(comando, "%s", WAKEUP);
// sprintf(comando, "#1P1900S%d#2P2276S%d#3P1024S%d#4P1870S%dT%d", s, s, s, s, t);
enviar_comando(comando, serial_fd);
memset(comando, 0, BUFSIZE);
}
int main(int argc, char** argv){
char in, *comando;
int serial_fd;
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;
}
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;
}
sprintf(comando,"%s",HOME_POS);
while(in != 'x'){
memset(comando, 0, BUFSIZE);
scanf("%s", &in);
switch (in){
case 'a':
sprintf(comando,"#%dP%dT%d",3,trava(3,2000),2000);
enviar_comando(comando,serial_fd);
break;
case 's':
sprintf(comando,"#%dP%dT%d",2,trava(2,1900),2000);
enviar_comando(comando,serial_fd);
break;
case 'd':
sprintf(comando,"#%dP%dT%d",1,trava(1,1000),2000);
enviar_comando(comando,serial_fd);
break;
case 'w':
sprintf(comando,"#%dP%dT%d",4,trava(4,1700),1000);
enviar_comando(comando,serial_fd);
break;
}
}
return 0;
}
//ENVIAR UM COMANDO PRA UM DETERMINADO SERVO
int enviarComando(int servo, int pos){
memset(comando, 0, BUFSIZE);
sprintf(comando,"#%dP%d",servo,trava(servo,pos));
if(enviar_comando(comando,serial_fd)==-1){
printf("Problema no envio do comando\nAbortando o programa...");
return -1;
}
return 1;
}
void SOLTA (void){
ufrn_header();
pgarra = 1500;
sprintf(comando,"#%dP%dT3000",GRI_SERVO,trava(GRI_SERVO,pgarra));
enviar_comando(comando,serial_fd);
memset(comando, 0, BUFSIZE);
}
void MOVE (void){
ufrn_header();
//Posição inicial.
//sprintf(comando,"#%dP%d",BAS_SERVO,trava(BAS_SERVO,));
int i=1;
while(i!=0)
{
//Captura dos valores:
printf("Digite x: ");
scanf("%lf",&x);
printf("Digite y: ");
scanf("%lf",&y);
printf("Digite z: ");
scanf("%lf",&z);
printf("Digite PHI: ");
scanf("%lf",&phi);
inversa(x, y, z);
//Transformação de rad para grau:
o1= o1*180/(M_PI);
o2= o2*180/(M_PI);
o3= o3*180/(M_PI);
o4= o4*180/(M_PI);
//Inicializando o Programa:
serial_fd = abrir_porta();
comando = (char*) malloc(sizeof(char)*BUFSIZE);
//Execução do comando para o braço robótico:
memset(comando, 0, BUFSIZE);
//pbase=(-800*o1/90)+ 620;
pbase = (o1/0.1) + 1500;
sprintf(comando,"#%dP%dT3000",BAS_SERVO,trava(BAS_SERVO,pbase));
enviar_comando(comando,serial_fd);
memset(comando, 0, BUFSIZE);
// pombro=(9.01*o2/90)+ 635.3;
pombro = (o2/(-0.1)) + 530;
sprintf(comando,"#%dP%dT3000",SHL_SERVO,pombro);
enviar_comando(comando,serial_fd);
memset(comando, 0, BUFSIZE);
// pcotovelo=(-8.72*o3)+893.3;
pcotovelo = (o3/0.1) + 770;
sprintf(comando,"#%dP%dT3000",ELB_SERVO,trava(ELB_SERVO,pcotovelo));
enviar_comando(comando,serial_fd);
memset(comando, 0, BUFSIZE);
// ppunho=(9.9479*o4+1471.1);
ppunho = (phi/0.1) + 540;
sprintf(comando,"#%dP%dT3000",WRI_SERVO,trava(WRI_SERVO,ppunho));
enviar_comando(comando,serial_fd);
printf("\np1: %d", pbase);
printf("\np2: %d", pombro);
printf("\np3: %d", pcotovelo);
printf("\np4: %d", ppunho);
printf("\n\nPara continuar digite 1, para sair digite 0: ");
scanf("%d",&i);
}
}
void solta(){
sprintf(comando, "#4P1320");
enviar_comando(comando, serial_fd);
memset(comando, 0, BUFSIZE);
}
void pega(){
sprintf(comando, "#4P1950");
enviar_comando(comando, serial_fd);
memset(comando, 0, BUFSIZE);
}
void send_command(void) {
enviar_comando(comando,serial_fd);
sprintf(last_comando, "%s", comando);
memset(comando, 0, BUFSIZE);
}
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;
}
