Tuesday 11 August 2015

Progam to find the workspace of a two link robotic arm

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



float d2r(float deg){

 return (M_PI*deg)/180;
}
void main(){
 int gd=DETECT,gm;
 int midx,midy;
 float x1,x2,x3,y1,y3,y2,x4,y4,temp1,temp2,deg,rad,k=0,ang;
 initgraph(&gd,&gm,"C:\\TurboC3\\BGI");

 midx=320;
 midy=240;
 setcolor(BLUE);
 line(midx,midy,midx+100,midy);
 setcolor(GREEN);
 line(midx+100,midy,midx+100,midy-100);
 delay(500);
 cleardevice();
 setcolor(BLUE);
 line(midx,midy,midx+100,midy);
 setcolor(GREEN);
 line(midx+100,midy,midx+150,midy);
 x1=midx+100;
 x2=midx+150;
 y1=midy;
 y2=midy;
 //outer range
 while(k<360 && !kbhit()){
 k++;


    ang=d2r(1);
  x3=(x1-midx)*cos(ang) - (y1 - midy)*sin(ang) + midx;
  y3=(x1-midx)*sin(ang) + (y1 - midy)*cos(ang) + midy;
x4=(x2-midx)*cos(ang) - (y2 - midy)*sin(ang) + midx;
 y4=(x2-midx)*sin(ang) + (y2 - midy)*cos(ang) + midy;
  x1=x3;
  y1=y3;
  x2=x4;
  y2=y4;
 setcolor(BLUE);
 //if(k==180){
 line(midx,midy,x1,y1);
 setcolor(GREEN);
 line(x1,y1,x2,y2);
 //setcolor(RED);
 //}
 putpixel(x4,y4,RED);
       delay(10);

       }

 setcolor(RED);
 line(midx,midy,midx+100,midy);
 setcolor(GREEN);
 line(midx+100,midy,midx+50,midy);
 x1=midx+100;
 x2=midx+50;
 y1=midy;
 y2=midy;


 //inner range
 while(k<720 && !kbhit()){
 k++;


    ang=d2r(1);
  x3=(x1-midx)*cos(ang) - (y1 - midy)*sin(ang) + midx;
  y3=(x1-midx)*sin(ang) + (y1 - midy)*cos(ang) + midy;
x4=(x2-midx)*cos(ang) - (y2 - midy)*sin(ang) + midx;
 y4=(x2-midx)*sin(ang) + (y2 - midy)*cos(ang) + midy;
  x1=x3;
  y1=y3;
  x2=x4;
  y2=y4;
 setcolor(RED);
 //if(k==180){
 line(midx,midy,x1,y1);
 setcolor(GREEN);
 line(x1,y1,x2,y2);
 //setcolor(RED);
 //}
 putpixel(x4,y4,RED);
       delay(10);

       }

 getch();
}

No comments:

Post a Comment