Revision: 7972
http://playerstage.svn.sourceforge.net/playerstage/?rev=7972&view=rev
Author: rtv
Date: 2009-07-10 02:53:00 +0000 (Fri, 10 Jul 2009)
Log Message:
-----------
reduced stuckness of flocking demo
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/pioneer_flocking.cc
Modified: code/stage/trunk/examples/ctrl/pioneer_flocking.cc
===================================================================
--- code/stage/trunk/examples/ctrl/pioneer_flocking.cc 2009-07-10 02:46:27 UTC
(rev 7971)
+++ code/stage/trunk/examples/ctrl/pioneer_flocking.cc 2009-07-10 02:53:00 UTC
(rev 7972)
@@ -107,7 +107,13 @@
if( robot->closest )
turn_speed += FLOCK_WGAIN * robot->closest_heading_error;
}
-
+ else
+ {
+ // front not clear. we might be stuck, so wiggle a bit
+ if( fabs(turn_speed) < 0.1 )
+ turn_speed = drand48();
+ }
+
robot->position->SetSpeed( forward_speed, side_speed, turn_speed );
return 0;
@@ -117,9 +123,7 @@
int FiducialUpdate( ModelFiducial* fid, robot_t* robot )
{
// find the closest teammate
-
- //puts( "fiducial update" );
-
+
double dist = 1e6; // big
robot->closest = NULL;
@@ -137,36 +141,10 @@
if( robot->closest ) // if we saw someone
{
- //printf( "model %s see closest %s\n", fid->Token(),
robot->closest->mod->Token() );
-
robot->closest_bearing = robot->closest->bearing;
robot->closest_range = robot->closest->range;
robot->closest_heading_error = robot->closest->geom.a;
}
-
-// if( (dx == 0) || (dy == 0) )
-// return 0;
-
-// double resultant_angle = atan2( dy, dx );
-// double forward_speed = 0.0;
-// double side_speed = 0.0;
-// double turn_speed = WGAIN * resultant_angle;
-
-// //printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed
);
-
-// // if the front is clear, drive forwards
-// if( (rgr->sensors[3].range > SAFE_DIST) && // forwards
-// (rgr->sensors[4].range > SAFE_DIST) &&
-// (rgr->sensors[5].range > SAFE_DIST/2.0) && //
-// (rgr->sensors[6].range > SAFE_DIST/4.0) &&
-// (rgr->sensors[2].range > SAFE_DIST/2.0) &&
-// (rgr->sensors[1].range > SAFE_DIST/4.0) &&
-// (fabs( resultant_angle ) < SAFE_ANGLE) )
-// {
-// forward_speed = VSPEED;
-// }
-
-// robot->position->SetSpeed( forward_speed, side_speed, turn_speed );
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Enter the BlackBerry Developer Challenge
This is your chance to win up to $100,000 in prizes! For a limited time,
vendors submitting new applications to BlackBerry App World(TM) will have
the opportunity to enter the BlackBerry Developer Challenge. See full prize
details at: http://p.sf.net/sfu/Challenge
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
