/*void robot_motion() { float r,w,vc; w=(vright-vleft)/(2*ra[0][5]); vc=(vleft+vright)/2; // ra[0][1] = ra[0][1] + (vc/w)*sin(w+ra[0][0]) - (vc/w)*sin(ra[0][0]); // ra[0][2] = ra[0][2] + (vc/w)*cos(ra[0][0]) - (vc/w)*cos(w+ra[0][0]); ra[0][0] = ra[0][0]+w; }*/ int update(float dest) { printf("updating\n"); int i,j,collision; float the_sum=dest,the_difference=dest,factor=M_PI/180,the_sum1,the_difference1,angle,angle1,temp; int h=1,count_sum=0,state_sum=0,loop=1; while(1) { h=1; count_sum=0; while(count_sum<numberofobjects) { the_sum1=tan_inv((v*sin(the_sum)-ra[h][4]),(v*cos(the_sum)-ra[h][3])); if((the_sum1 < M_PI/2 && ra[h][2] < omnicy))// && obj[h].xcent > obj[0].xcent) the_sum1=2*M_PI + the_sum1; if((the_sum1 > M_PI*3/2 && the_sum1 < 2*M_PI && ra[h][2] > omnicy))//&& obj[h].xcent > obj[0].xcent) the_sum1= the_sum1-2*M_PI; temp = sqrt((omnicx-ra[h][1])*(omnicx-ra[h][1])+(omnicy-ra[h][2])*(omnicy-ra[h][2])); if(temp < ra[h][5] + 2*ra[0][5]) { printf("collision occurred with object sum : %d\n",h); } angle = tan_inv((ra[h][2]-omnicy),(ra[h][1]-omnicx)); angle1 = asin((ra[h][5]+2*ra[0][5])/temp); // state_sum=0; // printf("angle=%f angle1=%f the_sum1=%f h=%d\n",angle*180/M_PI,angle1*180/M_PI,the_sum1*180/M_PI,h); if((the_sum1 >= angle+angle1 || the_sum1 <= angle-angle1)) { printf("enter\n"); count_sum++; h++; // state_sum = 1; ra[0][3]=v*cos(the_sum); ra[0][4]=v*sin(the_sum); } else break; } if(count_sum>=numberofobjects) { printf("fucksum\n"); return 1; } int count_diff=0,state_diff=0; h=1; while(count_diff<numberofobjects) { the_difference1=tan_inv((v*sin(the_difference)-ra[h][4]),(v*cos(the_difference)-ra[h][3])); if((the_difference1 < M_PI/2 && ra[h][2] <omnicy))// && obj[h].xcent > obj[0].xcent) the_difference1=2*M_PI + the_difference1; if((the_difference1 > M_PI*3/2 && the_difference1 < 2*M_PI && ra[h][2] > omnicy))// && obj[h].xcent > obj[0].xcent) the_difference1= the_difference1-2*M_PI; temp = sqrt((omnicx-ra[h][1])*(omnicx-ra[h][1])+(omnicy-ra[h][2])*(omnicy-ra[h][2])); if(temp < ra[h][5] + 2*ra[0][5]) { printf("collision occurred with object diff: %d\n",h); } angle = tan_inv((ra[h][2]-omnicy),(ra[h][1]-omnicx)); angle1 = asin((ra[h][5]+2*ra[0][5])/temp); // printf("angle=%f angle1=%f the_difference1=%f h=%d\n",angle*180/M_PI,angle1*180/M_PI,the_difference1*180/M_PI,h); if((the_difference1 <= angle-angle1 || the_difference1 >= angle+angle1)) { printf("enterdiff %d\n",h); count_diff++; h++; ra[0][3]=v*cos(the_difference); ra[0][4]=v*sin(the_difference); } else break; } if(count_diff>=numberofobjects) { printf("fuckdiff\n"); return 1; } if(count_sum<numberofobjects&&count_diff<numberofobjects) { the_sum=the_sum+M_PI/180; the_difference=the_difference-M_PI/180; loop++; printf("angleupdate\n"); } if(loop>180) { v=v+100; printf("f****d\n"); loop =1; the_sum = dest; the_difference = dest; // count_sum=0; // count_diff=0; // h=1; } } }
int main(int argc, char **argv) { dest_x=0; dest_y=6000; v = 200; numberofobjects = 2; v1 = 100; ra[0][5] = 400; ra[1][5] = 400; ra[2][5] = 400; ra[3][5] = 400; //argv[1]="-rh"; //argv[2]="10.2.36.7"; //argc=3; BotConnector bc(argc, argv); if( bc.connect() ) { printf( "Connected to Robot\n" ); } else { printf( "Connection attempt to Robot failed, Sorry :(\n" ); return EXIT_SUCCESS; } argv[1]="-rrtp"; argv[2]="8102"; argc=3; BotConnector bc1(argc, argv); if( bc1.connect() ) { printf( "Connected to Robot1\n" ); } else { printf( "Connection attempt to Robot failed, Sorry :(\n" ); return EXIT_SUCCESS; } // bc.setRobotVelocity_idle(100,100); // initialising vr and vl bc.setRobotVelocity(100,100, 10000); bc.getAngles(0);// updating ra[][]. // omnicx = ra[0][1]+ra[0][5]*cos(ra[0][0]); // omnicy = ra[0][2]+ra[0][5]*sin(ra[0][0]); //bc1.moveRobot(0,90); //bc1.setRobotVelocity(100,100, 10000); bc1.moveRobot(6000,90); //bc1.setRobotVelocity(100,100, 30000); bc1.moveRobot(0,180); argv[1]="-rrtp"; argv[2]="8103"; argc=3; BotConnector bc2(argc, argv); if( bc2.connect() ) { printf( "Connected to Robot2\n" ); } else { printf( "Connection attempt to Robot failed, Sorry :(\n" ); return EXIT_SUCCESS; } bc2.moveRobot(2000,90); //bc2.setRobotVelocity(100,100, 20000); bc2.moveRobot(2000,-45); //bc2.setRobotVelocity(200,200, 25000); bc2.moveRobot(0,135); /* argv[1]="-rrtp"; argv[2]="8104"; argc=3; BotConnector bc3(argc, argv); if( bc3.connect() ) { printf( "Connected to Robot\n" ); } else { printf( "Connection attempt to Robot failed, Sorry :(\n" ); return EXIT_SUCCESS; } //bc.setVelocity1(-100,-100, 10000); bc3.moveRobot(0,90); bc3.setRobotVelocity(150,150, 30000); bc3.moveRobot(0,-125); bc3.setRobotVelocity(100,100, 20000); bc3.moveRobot(0,180);*/ /*argv[1]="-rrtp"; argv[2]="8105"; argc=3; BotConnector bc4(argc, argv); if( bc4.connect() ) { printf( "Connected to Robot\n" ); } else { printf( "Connection attempt to Robot failed, Sorry :(\n" ); return EXIT_SUCCESS; } //bc.setVelocity1(-100,-100, 10000); bc4.getAngles(4); bc4.moveRobot(0,90); bc4.setRobotVelocity(100,100, 40000); bc4.moveRobot(0,0); bc4.setRobotVelocity(200,200, 25000); bc4.moveRobot(0,180); argv[1]="-rrtp"; argv[2]="8106"; argc=3; BotConnector bc5(argc, argv); if( bc5.connect() ) { printf( "Connected to Robot\n" ); } else { printf( "Connection attempt to Robot failed, Sorry :(\n" ); return EXIT_SUCCESS; } //bc.setVelocity1(-100,-100, 10000); bc5.getAngles(5); bc5.moveRobot(0,90); bc5.setRobotVelocity(200,200, 25000); bc5.moveRobot(0,-80); bc5.setRobotVelocity(300,300, 15000); bc5.moveRobot(0,180);*/ bc.setRobotVelocity(-100,-100, 10000); bc.moveRobot(0,90); bc.getAngles(0); bc1.getAngles(1); bc2.getAngles(2); omnicx = ra[0][1]; omnicy = ra[0][2]+300; //bc3.getAngles(3); /*bc4.getAngles(4); bc5.getAngles(5);*/ ArUtil::sleep(5000); int f; for(f=0;f<200;f++) { float dest,dest1; int j; dest = tan_inv((dest_y-omnicy),(dest_x-omnicx)); j=update(dest); right_left(); // robot_motion(); bc.setRobotVelocity_idle(vright,vleft); bc1.setRobotVelocity_idle(100,100); bc2.setRobotVelocity_idle(100,100); // bc3.setRobotVelocity_idle(250,250); /*bc4.setRobotVelocity_idle(250,250); bc5.setRobotVelocity_idle(250,250);*/ ArUtil::sleep(300); bc.getAngles(0); bc1.getAngles(1); bc2.getAngles(2); //bc3.getAngles(3); omnicx = omnicx+ra[0][3]*0.3; omnicy = omnicy+ra[0][4]*0.3; /*bc.setRobotVelocity_idle(0,0); bc1.setRobotVelocity_idle(0,0); bc2.setRobotVelocity_idle(0,0); bc3.setRobotVelocity_idle(0,0);*/ // omnicx = ra[0][1]+ra[0][5]*cos(ra[0][0]); //omnicy = ra[0][2]+ra[0][5]*sin(ra[0][0]); printf("omnicx = %f, omnicy = %f\n",omnicx,omnicy); printf("x = %d",f); /* bc3.getAngles(3); bc4.getAngles(4); bc5.getAngles(5);*/ } return bc.disconnect(); }
/*Moving cursor through keys To exit the calculator press q*/ void mvcurs() { int input; curs_set(2); int y=11,x=50; int b; float a,p,q,w; mvprintw(22,0,"Press 'q'to exit"); move(11,50); while(input != 'q') { cbreak(); input = getch(); switch(input) { case KEY_UP : if(y >= 14) move(y=y-3,x); break; case KEY_DOWN : if(y <= 17) move(y=y+3,x); break; case KEY_LEFT : if(x <= 70 && x>50) move(y,x=x-5); else if(x>70) move(y,x=x-10); break; case KEY_RIGHT : if(x < 70) move(y,x=x+5); if(x>=70 && x<100) move(y,x=x+10); break; case '=': getyx(stdscr,y,x); if(y==20 && x==90){ clrscr(); mvprintw(7,70,"sqrt"); scanw("%d",&b); mvprintw(7,74,"%d",b); mvprintw(8,76,"%f",powerfun(b,0.5)); } if(y==20 && x==80){ clrscr(); mvprintw(7,70,"factorial"); scanw("%d",&b); mvprintw(7,80,"%d",b); mvprintw(8,76,"%d",fact(b)); } if(y==17 && x==100){ clrscr(); mvprintw(7,70,"powerfunction:",b); scanw("%f%f",&p,&q); mvprintw(7,80,"%f ^ %f",p,q); mvprintw(8,76,"%f",powerfun(p,q)); } if(y==17 && x==90){ clrscr(); mvprintw(7,70,"tan inverse",b); scanw("%f",&p); if(p > -1 && p<=1) a=tan_inv(p); else if(p>1){ w=1/p; a= (PI/2)-tan_inv(w); } else if(p<(-1)){ w=1/p; a= (PI/2)-tan_inv(w); a=a*(-1); } mvprintw(7,80,"%f",p); mvprintw(8,76,"%f",a*180/PI); } if(y==14 && x==70){ clrscr(); mvprintw(7,70,"sine"); scanw("%f",&p); mvprintw(7,75,"%f=",p); mvprintw(8,76,"%f",sine(p*(PI/180))); } if(y==17 && x==70){ clrscr(); mvprintw(7,70,"sineinv"); scanw("%f",&p); mvprintw(7,77,"%f",p); mvprintw(8,76,"%f",(asin(p)*(180/PI))); } if(y==14 && x==80){ clrscr(); mvprintw(7,70,"cos"); scanw("%f",&p); mvprintw(7,77,"%f",p); mvprintw(8,76,"%f",cosine(p*(PI/180))); } if(y==20 && x==70){ clrscr(); for(a=50;a<85;a++){ mvprintw(7,a," "); } mvprintw(8,76,"%f",ans); } if(y==11 && x==80){ clrscr(); mvprintw(7,70,"ln"); scanw("%f",&p); mvprintw(7,73,"%f",p); mvprintw(8,76,"%f",ln(p)); } if(y==20 && x==50){ clrscr(); } if(y==14 && x==100){ clrscr(); mvprintw(7,70,"e^"); scanw("%f",&p); mvprintw(7,72,"%f",p); mvprintw(8,76,"%f",expo(10,p)); } if(y==14 && x==90){ clrscr(); mvprintw(7,70,"tan"); scanw("%f",&p); w=sine(p*PI/180)/cos(p*PI/180); mvprintw(7,74,"%f",p); mvprintw(8,76,"%f",w); } if(y==11 && x==70){ clrscr(); mvprintw(7,70,"log"); scanw("%f",&p); mvprintw(7,74,"%f",p); mvprintw(8,76,"%f",(ln(p)/2.303)); } if(y==11 && x==90){ clrscr(); mvprintw(7,70,"reciprocal"); scanw("%f",&p); mvprintw(7,78,"%f",p); mvprintw(8,76,"%f",(1/p)); } if(y==11 && x==100){ clrscr(); mvprintw(8,76,"3.1428"); } if(y==20 && x==100){ endcurses(); exit(1); } if(y==17 && x== 80) { clrscr(); mvprintw(7,70,"cosinv"); scanw("%f",&p); mvprintw(7,77,"%f",p); mvprintw(8,76,"%f",acos(p)*(180/PI)); } break; case 'A' : /*for numerical calculation press'A'*/ noecho(); int i = 0; int y=65; while(1) { data[i] = getch(); mvprintw(6,y,"%c",data[i]); y++; switch(data[i]) { case 'C': move(20,50); goto reset; break; case '1': move(17,50); break; case '2': move(17,55); break; case '3': move(17,60); break; case '4': move(14,50); break; case '5': move(14,55); break; case '6': move(14,60); break; case '7': move(11,50); break; case '8': move(11,55); break; case '9': move(11,60); break; case '+': move(20,65); break; case '-': move(17,65); break; case '*': move(14,65); break; case '/': move(11,65); break; case '=' : goto terminate; break; } i++; } terminate: data[i]='\0'; int j; int k; i=0; while(data[i]!='\0') { if(data[i]=='.'){ i=j; k=10; int num=data[i-1]-'0'; while(isdigit(data[j+1])){ num=num+(data[j+1]-'0')/k; k=k*10; j++; } data[i-1] = (char)(((int)'0')+num); k=i; while(data[j+1]!='\0'){ data[k]=data[j]; k++; j++; } data[k]=data[j]; data[k+1]='\0'; } i++; reset : for(a=65;a<75;a++) mvprintw(6,a," "); } float x=calculateinfix(data); ans = x; mvprintw(6,65,"%s",data); mvprintw(8,76,"%f",x); refresh(); break; } if(input == 'q') { endcurses(); exit(1); } } }