|
Hi,
Yes, result near zero, but my robot go around in the world the value is
changed very little same near changes. I still have this problems.
Thanks
Dong
--- On Mon, 20/9/10, Rich Mattes <jpgr87@gmail.com>
wrote:
From: Rich Mattes <jpgr87@gmail.com>
Subject: RE: Hello All, please help me this PlayerStage problems
To: "'dong nguyenhung'" <hungdong1984@yahoo.com>
Date: Monday, 20 September, 2010, 8:46 PM
Are you sure that’s the wrong
value? 1e-17 is effectively zero anyway; it is probably just a difference
in how printf and cout handle very small values. If you mess with the
cout precision (i.e. limit it to 6 digits after the decimal point) it should
display 0.
Rich
From: dong nguyenhung
[mailto:hungdong1984@yahoo.com]
Sent: Monday, September 20, 2010 3:47 AM
To: jpgr87@gmail.com
Cc: player stage
Subject: Hello All, please help me this PlayerStage problems
|
Hi,
I have a problems with Player stage 2.1.1, function GetYPos when
simulation Robot. GetYPos is wrong value. How can i solve it.Thanks. I
wish to solve urgently.
My code here:
/*g++ `pkg-config --cflags playerc++` -o VFF VFF.cc `pkg-config
--libs playerc++`*/
#define USAGE \
"USAGE: sonarobstacleavoid [-h <host>] [-p
<port>] [-m]\n" \
" -h <host>:
connect to Player on this host\n" \
" -p <port>:
connect to Player on this TCP port\n" \
"
-m : turn on motors (be
CAREFUL!)"
#include <libplayerc++/playerc++.h>
#include <iostream>
#include <cstdlib>
#include <libplayerc/playerc.h>
#include "args.h"
#include "time.h"
#include "stdio.h"
#include "stdlib.h"
using namespace std;
using namespace PlayerCc;
void AvoidObstacles(double *forwardSpeed, double *turnSpeed, \
SonarProxy &sp);
double x_init=-1.5;
double y_init=-1.5;
double Yaw_init=3.1416/4;
double tetar;
double xr=0;
double yr=0;
int main(int argc, char **argv)
{
double min_front_dist = 0.300;
double really_min_front_dist = 0.100;//10cm
char avoid;
char i;
double forwardSpeed=0.05,turnSpeed=0;
double xgoal=1;
double ygoal=-1;
double x=0;
double y=0;
double Yawr=45;
double distance;
parse_args(argc,argv);
// we throw exceptions on creation if we fail
try
{
using namespace PlayerCc;
PlayerClient robot (gHostname, gPort);
Position2dProxy pp (&robot, gIndex);
SonarProxy sp (&robot, gIndex);
std::cout << robot << std::endl;
forwardSpeed=0.05;
for(;;)
{
robot.Read();
for(int i=0;i<6;i++){
std::cout<<"Sonar:
"<<i<<"="<<sp[i]<<std::endl;
}
//turnSpeed=0;
//AvoidObstacles(&forwardSpeed, &turnSpeed,
sp);
xr=pp.GetXPos();
yr=pp.GetYPos();
printf("y postion=%f\n",yr);
Yawr=pp.GetYaw();
std::cout <<"vitriX = "<< xr
<< std::endl;
std::cout <<"vitriY = "<< yr
<< std::endl;
//while(1);
printf("xr=%f\n",xr+x_init);
printf("yr=%f\n",yr+y_init);
printf("Yawr=%f\n",Yawr+Yaw_init);
printf("GetXPos=%f\n",xr+x_init);
printf("GetYPos=%f\n",yr+y_init);
printf("GetYaw=%f\n",Yawr+Yaw_init);
// compute the potential field
x=xgoal-(pp.GetXPos()+x_init);
y=ygoal-(pp.GetYPos()+y_init);
printf("x=%f\n",x);
printf("y=%f\n",y);
if((x<0.02)&&(y<0.02))
{
pp.SetSpeed(0,0, 0);
while(1);
}
distance= sqrt(x*x+y*y);
printf("distance=%f\n",distance);
tetar= atan2(x,y);//(x,y)
printf("atan2=%f\n",tetar);//gia tri la
radian
//pp.SetSpeed(forwardSpeed,-forwardSpeed,
dtor(turnSpeed));
robot.Read();
pp.SetSpeed(forwardSpeed, dtor(turnSpeed));
sleep(1);
}
}
catch (PlayerCc::PlayerError e)
{
std::cerr << e << std::endl;
return -1;
}
}
void AvoidObstacles(double *forwardSpeed, double *turnSpeed, \
SonarProxy &sp)
{
//will avoid obstacles closer than 40cm
double avoidDistance_side = 0.2;
double avoidDistance_front = 0.3;
//will turn away at 60 degrees/sec
int avoidTurnSpeed = 30;
//left corner is sonar no. 0
//right corner is sonar no. 2
/*
*/
if((sp[1]<avoidDistance_front) &&
(sp[2]<avoidDistance_side) && \
(sp[0]<avoidDistance_side))
{
//back off a
little bit
*forwardSpeed = -0.05;
*turnSpeed = 10;
printf("avoiding obstacle
front, left, right\n");
return;
}
if(sp[0] < avoidDistance_side)
{
*forwardSpeed = 0;
//turn right
*turnSpeed = (-1)*avoidTurnSpeed;
printf("avoiding obstacle
right\n");
return;
}
else if(sp[2] < avoidDistance_side)
{
*forwardSpeed = 0;
//turn left
*turnSpeed = avoidTurnSpeed;
printf("avoiding obstacle
left\n");
return;
}
return; //do nothing
}
/// Result:
Sonar: 0=2
Sonar: 1=1.35406
Sonar: 2=2
Sonar: 3=0.537083
Sonar: 4=0.681506
Sonar: 5=0.537083
yposition=0.000000
vitriX = 0.162
vitriY = 1.2719e-17
xr=-1.338000
yr=-1.500000
Yawr=0.785400
GetXPos=-1.338000
GetYPos=-1.500000
GetYaw=0.785400
x=2.338000
y=0.500000
distance=2.390867
atan2=1.360112
Sincerely,
Dong
|
|
|