#include<stdio.h>
#include<GRAPHICS.h>
#include<math.h>
double L1=110;
double L2=140;
double lx[3],ly[3];
double radian(int val)
{
return (M_PI*val)/180;
}
void rotate(double x_c,double y_c,double a,double b,double *x_p,double *y_p,int degree)
{
double rad=radian(degree);
*x_p=((a-x_c)*cos(rad))-((b-y_c)*sin(rad))+x_c;
*y_p=((a-x_c)*sin(rad))+((b-y_c)*cos(rad))+y_c;
}
int isreachable(int x,int y)
{
int xc,yc,radius;
if( ((x-lx[0])*(x-lx[0])+ (y-ly[0])*(y-ly[0])) > (L1+L2)*(L1+L2) )
return 0;
/* Case 2: */
if(y>0)
{
xc=L1+lx[0];
yc=ly[0];
//radius=L2;
if( ((x-xc)*(x-xc) + (y-yc)*(y-yc)) < L2*L2 )
return 0;
}
/* Case 3: */
if(y<=0)
{
xc=lx[0];
yc=ly[0];
radius=abs(L1)-abs(L2);
if( ((x-lx[0])*(x-lx[0]) + (y-ly[0])*(y-ly[0])) < radius*radius )
return 0;
}
/*Case 4: */
if( y<=0 && x>=0)
{
xc=lx[0]-L1;
yc=ly[0];
radius=L2;
if( ((x-xc)*(x-xc) + (y-yc)*(y-yc)) > radius*radius )
return 0;
}
/*Case 5: */
if( y<0 && x<0 )
{
xc=lx[0]-L1;
yc=0;
radius=L2;
if( ((x-xc)*(x-xc) + (y-yc)*(y-yc)) > L2*L2 )
return 0;
}
return 1;
}
double calculateAngle(double a,double b,double c)
{
double val=(180/M_PI);
double res=acos( ((a*a)+(b*b)-(c*c)) / (2*b*a)) * val;
return res;
}
int main()
{
double dx,dy,a,b;
double a_rad,b_rad,tan_rad;
int degree1,degree2;
/* request auto detection */
int gdriver = DETECT, gmode, errorcode;
/* initialize graphics mode */
initgraph(&gdriver, &gmode, "C:\\TurboC3\\BGI");
lx[0]=getmaxx()/2;
ly[0]=getmaxy()/2;
printf("Origin-X: %lf Origin-Y: %lf\n",lx[0],ly[0]);
lx[1]=lx[0]+L1;
ly[1]=ly[0];
lx[2]=lx[1];
ly[2]=ly[1]-L2;
setcolor(BLUE);
line(lx[0],ly[0],lx[1],ly[1]);
setcolor(RED);
line(lx[1],ly[1],lx[2],ly[2]);
printf("Enter dx :");
scanf("%lf",&dx);
printf("Enter dy :");
scanf("%lf",&dy);
// dx=lx[0]+dx;
// dy=ly[0]-dy;
if(isreachable(lx[0]+dx,ly[0]-dy))
{
double dist;
// setcolor(YELLOW);
// line(lx[0],ly[0],dx,dy);
dist=sqrt( (dx)*(dx) + (dy)*(dy) );
b_rad=calculateAngle(L1,L2,dist);
a_rad=calculateAngle(L1,dist,L2);
printf("%lf ",a_rad);
b_rad=(180-b_rad);
tan_rad=((atan2(dy,dx))*(180/M_PI));
printf("%lf\n",tan_rad);
a_rad=(tan_rad-a_rad);
printf("\nangle Alpha=%lf",a_rad);
printf("\nangle Beta=%lf",b_rad);
rotate(lx[0],ly[0],lx[1],ly[0],&a,&b,(0-a_rad));
lx[1]=a;
ly[1]=b;
rotate(lx[0],ly[0],lx[0]+L1+L2,ly[0],&a,&b,(0-a_rad));
lx[2]=a;
ly[2]=b;
// printf("\n %lf %lf %lf %lf",lx[1],ly[1],lx[2],ly[2]);
rotate(lx[1],ly[1],lx[2],ly[2],&a,&b,(0-b_rad));
lx[2]=a;
ly[2]=b;
// printf("\n%lf %lf",lx[2],ly[2]);
clrscr();
setcolor(BLUE);
line(lx[0],ly[0],lx[1],ly[1]);
setcolor(RED);
line(lx[1],ly[1],lx[2],ly[2]);
//setcolor(YELLOW);
//line(lx[0],ly[0],lx[2],ly[2]);
}
else
{
printf("Destination is unreachable");
}
getch();
return 0;
}
Inverse Kinematics Problem - Check Robotic Arm Reachability
Subscribe to:
Posts (Atom)



No comments:
Post a Comment