Sonar Proxy readings

Dear all,

I was wondering if you could help me with a sonar proxy problem. I
have a UAV robot that has 3 sonars on it and uses the command
Adrian.Read() to read in the data from these sonars. Originally it was
reading in the data fine, however recently i've noticed that on
starting the simulation, one of my sonars doesn't read at all but the
other 2 work fine (when all 3 should be detecting an obstacle),
however later in the simulation it starts working which I find very
odd. Secondly, it seems to be clearing out the values randomly
sometimes too, and I have no idea why that is. I tried putting in
extra .Read() functions, but I found that that corrupted all the data
being read in if I tried running that very soon after the original
read command. I will tag the code on the end of this post if it helps.

Any help with this would be very very much appreciated :)

Thanks,

Lana

____________________________________________________________
Code:

#include <iostream>
#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <libplayerc++/playerc++.h>

using namespace PlayerCc;

double forwardSpeed, sideSpeed, turnSpeed;

//--------------------------------------------
// WANDER
//--------------------------------------------

//function for making the UAV fly in a random direction
void Wander(double *forwardSpeed, double *sideSpeed, double *turnSpeed)
{
  printf("wandering\n");
  double fspeed, sspeed, tspeed;

  fspeed = 1.5;
  sspeed = 0;
  tspeed = 0;
  *forwardSpeed = fspeed;
  *turnSpeed = tspeed;
  *sideSpeed = sspeed;

}

//--------------------------------------------
// AVOID
//--------------------------------------------
//function for detecting obstacles and avoiding if necessary
void AvoidObstacles(double *forwardSpeed, double *turnSpeed,	\
		    SonarProxy &sp)
{
  //will avoid obstacles closer than 4.5m;
  double avoidDistance = 4.5;
  //will turn away at 40 degrees/sec
  int avoidTurnSpeed = 40;
  int criticalAvoidDistance = 2.7;


  //sp[0] right sonar
  //sp[1] left sonar
  //sp[2] centre sonar
  //if there is something in front of the uav
  printf("0: %f, 1: %f, 2: %f \n", sp[0], sp[1], sp[2]);

  if(sp[2] < criticalAvoidDistance || sp[1] < criticalAvoidDistance ||
sp[0] < criticalAvoidDistance)
    {
      printf("Slowing\n");
      *forwardSpeed = 0.5;

    }


  if(sp[2] < avoidDistance || sp[1] < avoidDistance || sp[0] < avoidDistance)
    {
      printf("entered normal avoid\n");
       if(sp[2] < avoidDistance && sp[1] < avoidDistance && sp[0] <
avoidDistance)
	{
	  //reverse
	  *forwardSpeed = -0.3;
	  printf("reversing\n");
	  //*turnSpeed = avoidTurnSpeed;
	  printf("avoiding obstacle\n");
	  return;
	  }
       //if just something on left and in front
       else  if(sp[1] < avoidDistance && sp[2] < avoidDistance)
	 {
	   printf("Turning right\n");
	   //turn right
	   *turnSpeed = (-1)*avoidTurnSpeed;
	   //*forwardSpeed = 0.5;
	   printf("avoiding obstacle\n");
	   return;
	 }
       //if something on right, and front
       else if(sp[0] < avoidDistance && sp[2] < avoidDistance)
	 {
	   printf("turning left\n");
	   //turn left
	   *turnSpeed = avoidTurnSpeed;
	   //*forwardSpeed = 0.5;
	   printf("avoiding obstacle\n");
	   return;
	 }
     //if just something on left
       else  if(sp[1] < avoidDistance)
	{
	  printf("turning right\n");
	  //turn right
	  *turnSpeed = (-1)*avoidTurnSpeed;
	  printf("avoiding obstacle\n");
	  return;
	}
     //else if just something on right or front
       else  if(sp[0] < avoidDistance || sp[2] < avoidDistance)
	{
	  printf("Turning left\n");
	  //turn left
	  *turnSpeed = avoidTurnSpeed;
	  printf("avoiding obstacle\n");
	  return;
	}

    }

  return; //do nothing
}


//----------------------------------------
// MAIN
//----------------------------------------
int main(int argc, char *argv[])
{
  fflush(stdout);
  using namespace PlayerCc;
  PlayerClient Adrian("localhost",6665);

  Position2dProxy p2dProxy(&Adrian,0);
  SonarProxy sonarProxy(&Adrian,0);


  //shared Simultion proxy...
  SimulationProxy simProxy(&Adrian,0);

  double forwardSpeed, turnSpeed, sideSpeed;
  srand(time(NULL));

  p2dProxy.RequestGeom();
  sonarProxy.RequestGeom();

  while(true)
   {
      //read from proxies
      Adrian.Read();
      fflush(stdout);
      forwardSpeed = 1.5;
      turnSpeed = 0;
      sideSpeed = 0;

      //wander
      Wander(&forwardSpeed, &sideSpeed, &turnSpeed);

      if (sonarProxy[0] < 4.5 || sonarProxy[1] < 4.5 || sonarProxy[2] < 4.5)
	//avoid obstacles
	AvoidObstacles(&forwardSpeed, &turnSpeed, sonarProxy);

      //set motors
      p2dProxy.SetSpeed(forwardSpeed, sideSpeed, dtor(turnSpeed));
      // sleep(1);
    }
}

_____________________________________________________________

------------------------------------------------------------------------------
Benefiting from Server Virtualization: Beyond Initial Workload 
Consolidation -- Increasing the use of server virtualization is a top
priority.Virtualization can reduce costs, simplify management, and improve 
application availability and disaster protection. Learn more about boosting 
the value of server virtualization. http://p.sf.net/sfu/vmware-sfdev2dev