From: Lana E. <l.t...@st...> - 2011-04-17 15:43:43
|
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); } } _____________________________________________________________ |