Revision: 7975
http://playerstage.svn.sourceforge.net/playerstage/?rev=7975&view=rev
Author: rtv
Date: 2009-07-10 06:26:58 +0000 (Fri, 10 Jul 2009)
Log Message:
-----------
faster
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 06:09:46 UTC
(rev 7974)
+++ code/stage/trunk/examples/ctrl/pioneer_flocking.cc 2009-07-10 06:26:58 UTC
(rev 7975)
@@ -1,9 +1,8 @@
/////////////////////////////////
-// File: stest.c
-// Desc: Stage library test program
-// Created: 2004.9.15
+// File: pioneer_flocking.cc
+// Desc: Flocking behaviour, Stage controller demo
+// Created: 2009.7.8
// Author: Richard Vaughan <[email protected]>
-// CVS: $Id: stest.cc,v 1.3 2008-02-01 03:11:02 rtv Exp $
// License: GPL
/////////////////////////////////
@@ -29,7 +28,7 @@
} robot_t;
-const double VSPEED = 0.3; // meters per second
+const double VSPEED = 0.4; // meters per second
const double EXPAND_WGAIN = 0.3; // turn speed gain
const double FLOCK_WGAIN = 0.3; // turn speed gain
const double SAFE_DIST = 1.0; // meters
@@ -59,11 +58,6 @@
robot->fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate,
robot );
robot->fiducial->Subscribe();
- // subscribe to the laser, though we don't use it for navigating
- //robot->laser = (ModelLaser*)mod->GetModel( "laser:0" );
- //assert( robot->laser );
- //robot->laser->Subscribe();
-
return 0; //ok
}
@@ -78,8 +72,6 @@
ModelRanger::Sensor& s = rgr->sensors[i];
dx += s.range * cos( s.pose.a );
dy += s.range * sin( s.pose.a );
-
- //printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.a
);
}
if( (dx == 0) || (dy == 0) )
@@ -90,32 +82,30 @@
double side_speed = 0.0;
double turn_speed = EXPAND_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/1.0) && //
- (rgr->sensors[6].range > SAFE_DIST/2.0) &&
- (rgr->sensors[2].range > SAFE_DIST/1.0) &&
- (rgr->sensors[1].range > SAFE_DIST/2.0) &&
- (fabs( resultant_angle ) < SAFE_ANGLE) )
- {
- forward_speed = VSPEED;
-
- // and steer to match the heading of the nearest robot
- if( robot->closest )
- turn_speed += FLOCK_WGAIN * robot->closest_heading_error;
- }
+ (rgr->sensors[4].range > SAFE_DIST) &&
+ (rgr->sensors[5].range > SAFE_DIST ) && //
+ (rgr->sensors[6].range > SAFE_DIST/2.0) &&
+ (rgr->sensors[2].range > SAFE_DIST ) &&
+ (rgr->sensors[1].range > SAFE_DIST/2.0) &&
+ (fabs( resultant_angle ) < SAFE_ANGLE) )
+ {
+ forward_speed = VSPEED;
+
+ // and steer to match the heading of the nearest robot
+ 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();
- }
-
+ {
+ // 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;
}
@@ -129,22 +119,22 @@
robot->closest = NULL;
FOR_EACH( it, fid->GetFiducials() )
- {
- ModelFiducial::Fiducial* other = &(*it);
-
- if( other->range < dist )
- {
- dist = other->range;
- robot->closest = other;
- }
- }
+ {
+ ModelFiducial::Fiducial* other = &(*it);
+
+ if( other->range < dist )
+ {
+ dist = other->range;
+ robot->closest = other;
+ }
+ }
if( robot->closest ) // if we saw someone
- {
+ {
robot->closest_bearing = robot->closest->bearing;
robot->closest_range = robot->closest->range;
robot->closest_heading_error = robot->closest->geom.a;
- }
-
+ }
+
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
