/*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);
}
}
}
