void main() {
//("inversa inicia\n");
leerMatriz1();
leerMatriz2();
inversa();
exit(0);
}
myrabot_arm_base::WriteServos acercar(geometry_msgs::Point cerca)
{
double x = cerca.x;
double z = cerca.z;
cerca.z = cerca.z - 70*cos(atan2(x,z));
cerca.x = cerca.x - 70*sin(atan2(x,z));
myrabot_arm_base::WriteServos move = inversa(cerca, ::inclinacion_pinza, ::pg, 0);
return move;
}
myrabot_arm_base_b::WriteServos separar(geometry_msgs::Point lejos)
{
double x = lejos.x;
double z = lejos.z;
lejos.z = lejos.z - 70*cos(atan2(x,z));
lejos.x = lejos.x - 70*sin(atan2(x,z));
myrabot_arm_base_b::WriteServos move = inversa(lejos, ::inclinacion_pinza, ::pg, 0);
return move;
}
void punto(const geometry_msgs::Point& point)
{
ros::NodeHandle n;
ros::Publisher move_pub_=n.advertise<myrabot_arm_base_b::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<myrabot_arm_base_b::WriteServos>("hand_arm", 1);
::lambda = 0;
::punto_0 = directa(::p, ::inclinacion_pinza);
::punto_1 = point;
::destino = inversa(::punto_1, ::inclinacion_pinza, ::p, 0);
}
myrabot_arm_base::WriteServos levantarAcercar(geometry_msgs::Point la)
{
la.x = 0;
la.y = la.y + 50;
la.z = 60;
if (la.y < 75) {
la.y = 75;
}
myrabot_arm_base::WriteServos move = inversa(la, ::inclinacion_pinza, ::pg, 0);
::punto_0 = la;
return move;
}
void posicionEstadoCorriente(const myrabot_arm_base_b::ReadServos& pec)
{
ros::NodeHandle n;
ros::Publisher move_pub_=n.advertise<myrabot_arm_base_b::WriteServos>("move_arm", 1);
p = pec.posicion;
c = pec.corriente;
if (start == 0 || (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15))
&& ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15))
&& ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15))
&& ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15)))) {
if (::cont == 0)
{
::punto = home(p, c);
::move = inversa(::punto, ::inclinacion_pinza, p, 0);
::cont = 1;
::start = 1;
sleep(2);
}
else if (::cont_1 == 0)
{
::punto.x = 0;
::punto.z = 0;
::punto.y = 265;
::move = inversa(::punto, ::inclinacion_pinza, p, 0);
move_pub_.publish(::move);
::cont_1 = 1;
}
else if (::cont_2 == 0)
{
::punto.x = 260;
::punto.z = 0;
::punto.y = 50;
::move = inversa(::punto, ::inclinacion_pinza, p, 0);
move_pub_.publish(::move);
::cont_2 = 1;
sleep(2);
}
else if (::cont_3 == 0)
{
::punto.x = 0;
::punto.z = 205;
::punto.y = 60;
::move = inversa(::punto, ::inclinacion_pinza, p, 0);
move_pub_.publish(::move);
::cont_3 = 1;
sleep(2);
}
else if (::cont_4 == 0)
{
::punto.x = -260;
::punto.z = 0;
::punto.y = 50;
::move = inversa(::punto, ::inclinacion_pinza, p, 0);
move_pub_.publish(::move);
::cont_4 = 1;
sleep(2);
}
else
{
::punto.x = 0;
::punto.z = 0;
::punto.y = 265;
::move = inversa(::punto, ::inclinacion_pinza, p, 0);
move_pub_.publish(::move);
::cont = 0;
::cont_1 = 0;
::cont_2 = 0;
::cont_3 = 0;
::cont_4 = 0;
::start = 0;
sleep(2);
}
}
}
void posicionEstadoCorriente(const myrabot_arm_base::ReadServos& pec)
{
ros::NodeHandle n;
ros::Publisher move_pub_=n.advertise<myrabot_arm_base::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<myrabot_arm_base::WriteServos>("hand_arm", 1);
myrabot_arm_base::Servos p = pec.posicion;
::pg = pec.posicion;
::cg = pec.corriente;
myrabot_arm_base::Servos e = pec.estado;
myrabot_arm_base::Servos c = pec.corriente;
if (::cont == 0)
{
::punto_0 = home(p, c);
::punto_1 = ::punto_0;
::cont = ::cont+1;
::pinza.pinza = p.pinza;
}
if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {
if (::cont_1 == 0) {
::move = acercar(::punto_0);
move_pub_.publish(::move);
::cont_1 = ::cont_1+1;
}
if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15))
&& ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15))
&& ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15))
&& ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {
if (::cont_2 == 0) {
::move = inversa(::punto_0, ::inclinacion_pinza, p, 0);
move_pub_.publish(::move);
::cont_2 = ::cont_2+1;
}
else {
if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15))
&& ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15))
&& ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15))
&& ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {
if(::pin.posicion.pinza != p.pinza) {
std::cout<<c.pinza<<std::endl;
::pinza.pinza = ::pinza.pinza + 20;
::pin = controlPinza(::pinza, p, c);
hand_pub_.publish(::pin);
}
else {
std::cout<<"Agarrado"<<std::endl;
::move = levantarAcercar(::punto_0);
move_pub_.publish(::move);
::punto_1 = ::punto_0;
::cont_1 = 0;
::cont_2 = 0;
}
}
}
}
else {
std::cout<<"En movimiento"<<std::endl;
}
}
}
void posicion(const myrabot_arm_base_b::ReadServos& pose)
{
ros::NodeHandle n;
ros::Publisher move_pub_=n.advertise<myrabot_arm_base_b::WriteServos>("move_arm", 1);
::p = pose.posicion;
::e = pose.estado;
::c = pose.corriente;
if (cont == 0)
{
::punto_0 = home(::p, ::c);
::mover = inversa(::punto_0, ::inclinacion_pinza, ::p, 0);
cont = 1;
}
//std::cout<<::punto_0<<" - "<<::punto_1<<std::endl;
//std::cout<<::p<<" - "<<::mover<<std::endl;
if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z) && ((::p.base-15) < ::mover.posicion.base && ::mover.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::mover.posicion.arti1 && ::mover.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::mover.posicion.arti2 && ::mover.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::mover.posicion.arti3 && ::mover.posicion.arti3 < (::p.arti3+15)))
{
if (((::p.base-15) < ::destino.posicion.base && ::destino.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::destino.posicion.arti1 && ::destino.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::destino.posicion.arti2 && ::destino.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::destino.posicion.arti3 && ::destino.posicion.arti3 < (::p.arti3+15)))
{
::punto_0 = ::punto_1;
::lambda = 0;
}
else
{
geometry_msgs::Point u;
//ecuación de la recta p1 = p0+lambda.u
u.x = ::punto_1.x - ::punto_0.x;
u.y = ::punto_1.y - ::punto_0.y;
u.z = ::punto_1.z - ::punto_0.z;
double paso = abs(sqrt(pow(u.x,2)+pow(u.y,2)+pow(u.z,2))/10);
//std::cout<<paso<<std::endl;
::lambda = ::lambda + 1/paso;
::punto_i.x = ::punto_0.x + (::lambda)*u.x;
::punto_i.y = ::punto_0.y + (::lambda)*u.y;
::punto_i.z = ::punto_0.z + (::lambda)*u.z;
//std::cout<<punto_0<<std::endl;
//std::cout<<punto_i<<" : "<<lambda<<std::endl;
::mover = inversa(::punto_i, ::inclinacion_pinza, ::p, 0);
move_pub_.publish(::mover);
//std::cout<<::p<<std::endl;
//std::cout<<::destino<<std::endl;
}
}
}
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 menu(matriz A, matriz B)
{int op; limpiar();double x;
gotoxy(10,5); cout<<"[1] OBTENER UN ELEMENTO DE A..";
gotoxy(10,6); cout<<"[2] OBTENER UN ELEMENTO DE B..";
gotoxy(10,7); cout<<"[3] ASIGNAR UN ELEMENTO DE A..";
gotoxy(10,8); cout<<"[4] ASIGNAR UN ELEMENTO DE B..";
gotoxy(10,9); cout<<"[5] IGUALAR A<-B..............";
gotoxy(10,10); cout<<"[6] IGUALAR B<-A..............";
gotoxy(10,11); cout<<"[7] SUMAR A + B...............";
gotoxy(10,12); cout<<"[8] NEGATIVA DE A.............";
gotoxy(10,13); cout<<"[9] NEGATIVA DE B.............";
gotoxy(10,14); cout<<"[10] RESTAR A - B.............";
gotoxy(10,15); cout<<"[11] RESTAR B - A.............";
gotoxy(10,16); cout<<"[12] ESCALAR PARA A...........";
gotoxy(10,17); cout<<"[13] ESCALAR PARA B...........";
gotoxy(10,18); cout<<"[14] PRODUCTO A * B...........";
gotoxy(10,19); cout<<"[15] PRODUCTO B * A...........";
gotoxy(10,20); cout<<"[16] TRANSPUESTA DE A.........";
gotoxy(10,21); cout<<"[17] TRANSPUESTA DE B.........";
gotoxy(10,22); cout<<"[18] DETERMINANTE DE A........";
gotoxy(10,23); cout<<"[19] DETERMINANTE DE B........";
gotoxy(10,24); cout<<"[20] INVERSA DE A.............";
gotoxy(10,25); cout<<"[21] INVERSA DE B.............";
gotoxy(10,26); cout<<"[22] SALIR....................";
do{gotoxy(22,28); cout<<"Seleccione una opcion (1-22): ";cin>>op;
if(op<1 || op>22)
{gotoxy(20,28);cout<<"Opci¢n incorrecta vuelva a intentar";}
}while(op<1 || op>22);
switch(op)
{case 1: {limpiar();gotoxy(28,4); cout<<"[1] OBTENER UN ELEMENTO DE A..";
obtener(A);menu(A,B);break;}
case 2: {limpiar(); gotoxy(28,4); cout<<"[2] OBTENER UN ELEMENTO DE B..";
obtener(B);menu(A,B);break;}
case 3: {limpiar(); gotoxy(28,4); cout<<"[3] ASIGNAR UN ELEMENTO DE A..";
imprimir(asignar(A));getchar();menu(A,B);break;}
case 4: {limpiar(); gotoxy(28,4); cout<<"[4] ASIGNAR UN ELEMENTO DE B..";
imprimir(asignar(B));getchar();menu(A,B);break;}
case 5: {limpiar(); gotoxy(28,4); cout<<"[5] IGUALAR A<-B..............";
imprimir(igual(A,B));menu(A,B);break;}
case 6: {limpiar(); gotoxy(28,4); cout<<"[6] IGUALAR B<-A..............";
imprimir(igual(B,A));menu(A,B);break;}
case 7: {limpiar(); gotoxy(28,4); cout<<"[7] SUMAR A + B...............";
imprimir(sumar(A,B));menu(A,B);break;}
case 8: {limpiar(); gotoxy(28,4); cout<<"[8] NEGATIVA DE A.............";
imprimir(negativa(A));menu(A,B);break;}
case 9: {limpiar(); gotoxy(28,4); cout<<"[9] NEGATIVA DE B.............";
imprimir(negativa(B));menu(A,B);break;}
case 10: {limpiar(); gotoxy(28,4); cout<<"[10] RESTAR A - B.............";
imprimir(restar(A,B));menu(A,B);break;}
case 11: {limpiar(); gotoxy(28,4); cout<<"[11] RESTAR B - A.............";
imprimir(restar(B,A));menu(A,B);break;}
case 12: {limpiar(); gotoxy(28,4); cout<<"[12] ESCALAR PARA A...........";
imprimir(escalar(A));menu(A,B);break;}
case 13: {limpiar(); gotoxy(28,4); cout<<"[13] ESCALAR PARA B...........";
imprimir(escalar(B));menu(A,B);break;}
case 14: {limpiar(); gotoxy(28,4); cout<<"[14] PRODUCTO A * B...........";
imprimir(producto(A,B));menu(A,B);break;}
case 15: {limpiar(); gotoxy(28,4); cout<<"[15] PRODUCTO B * A...........";
imprimir(producto(B,A));menu(A,B);break;}
case 16: {limpiar(); gotoxy(28,4); cout<<"[16] TRANSPUESTA DE A.........";
imprimir(transpuesta(A));menu(A,B);break;}
case 17: {limpiar(); gotoxy(28,4); cout<<"[17] TRANSPUESTA DE B.........";
imprimir(transpuesta(B));menu(A,B);break;}
case 18: {limpiar(); gotoxy(28,4); cout<<"[18] DETERMINANTE DE A........";
x=det(A);gotoxy(5,5);cout<<"Determinante = "<<x;getchar();menu(A,B);break;}
case 19: {limpiar(); gotoxy(28,4); cout<<"[19] DETERMINANTE DE B........";
x=det(B);gotoxy(5,5);cout<<"Determinante = "<<x;getchar();menu(A,B);break;}
case 20: {limpiar(); gotoxy(28,4); cout<<"[20] INVERSA DE A.............";
inversa(A);menu(A,B);break;}
case 21: {limpiar(); gotoxy(28,4); cout<<"[21] INVERSA DE B.............";
inversa(B);menu(A,B);break;}
case 22: {limpiar(); gotoxy(28,4); cout<<"[22] SALIR....................";
gotoxy(10,10); cout<<"Saliendo...";
gotoxy(25,23); cout<<"Presione Enter Para Salir...";break;
}
}
}
/*
* Retorna el cuaternion resultante entre la division de dos cuaterniones.
*/
cuaternion division(cuaternion a, cuaternion b) {
return producto(a, inversa(b));
}
