[Playerstage-users] Hello All, please help me this PlayerStage problems

[dong n. <hun...@ya...>]

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