Tuesday, 11 August 2015

Implementation of inverse kinematics - single link arm

#include<stdio.h>
#include<conio.h>
#include<stdlib.h>
#include<math.h>
#include<dos.h>
#include<graphics.h>


void main(){
 int gd=DETECT, gm;
 float x1,y1,x2,y2;
 float l1,l2,l3,angle;
 int midx=320;
 int midy=240;
 initgraph(&gd,&gm,"C:\\TurboC3\\BGI");
   x2=midx+100;y2=midy;
 line(midx,midy,midx+100,midy);
 printf("Enter destination:");
scanf("%f %f",&x1,&y1);
// x1=390;y1=310;
 l1=sqrt((midx-x2)*(midx-x2)+(midy-y2)*(midy-y2));
// printf("%f\n",l1);
 l2=sqrt((midx-x1)*(midx-x1)+(midy-y1)*(midy-y1));
 l3=sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
  //printf("%f\n",l2);
 //printf("%f\n",l3);

  angle=acos((l1*l1+l2*l2-l3*l3)/(2*l1*l2));

 // x1=(midx+100-midx)*cos(angle) - (midy-midy)*sin(angle) + midx;
 //y1=(midx+100-midx)*sin(angle) + (midy-midy)*cos(angle) + midy;

  delay(200);

  // l1 < l2 is also not reachable but do we have to consider it or not ?
  if(l1>l2){
  printf("not reachable");
  getch();
  exit(0);
  }
  else{
  line(midx,midy,x1,y1);
  printf("\n%f",180*angle/M_PI);
       }

getch();

}

No comments:

Post a Comment