Browse files

working on charging model and demo

  • Loading branch information...
1 parent 8f23ef5 commit ff0dbdd6f6de4e2a4d7d4ddca77367c9aeb9908c rtv committed Feb 7, 2009
View
343 examples/ctrl/fasr.cc
@@ -11,38 +11,67 @@ const double minfrontdistance = 0.7;
const double stopdist = 0.5;
const int avoidduration = 10;
const int workduration = 20;
-const int payload = 4;
+const int payload = 1;
double have[4][4] = {
+ // { -120, -180, 180, 180 },
+ //{ -90, -120, 180, 90 },
{ 90, 180, 180, 180 },
- { 90, -90, 0, -90 },
+ { 90, -90, 180, 90 },
{ 90, 90, 180, 90 },
{ 0, 45, 0, 0}
};
double need[4][4] = {
{ -120, -180, 180, 180 },
- { -90, -120, 180, 180 },
+ { -90, -120, 180, 90 },
{ -90, -90, 180, 180 },
{ -90, -180, -90, -90 }
};
+double refuel[4][4] = {
+ { 0, 0, 45, 120 },
+ { 0,-90, -60, -160 },
+ { -90, -90, 180, 180 },
+ { -90, -180, -90, -90 }
+};
+
+typedef enum {
+ MODE_WORK=0,
+ MODE_DOCK,
+ MODE_UNDOCK
+} nav_mode_t;
class Robot
{
private:
ModelPosition* pos;
ModelLaser* laser;
ModelRanger* ranger;
- //ModelBlobfinder* blobfinder;
ModelFiducial* fiducial;
+ ModelBlobfinder* blobfinder;
Model *source, *sink;
int avoidcount, randcount;
int work_get, work_put;
-
+ bool charger_ahoy;
+ double charger_bearing;
+ double charger_range;
+ double charger_heading;
+ nav_mode_t mode;
+
static int LaserUpdate( ModelLaser* mod, Robot* robot );
static int PositionUpdate( ModelPosition* mod, Robot* robot );
static int FiducialUpdate( ModelFiducial* mod, Robot* robot );
+ static int BlobFinderUpdate( ModelBlobfinder* mod, Robot* robot );
+
+ void Dock();
+ void Work();
+ void UnDock();
+ bool ObstacleAvoid();
+
+ // predicate that indicates if we need to charge
+ bool Hungry();
+ bool Full();
public:
Robot( ModelPosition* pos,
@@ -51,25 +80,40 @@ class Robot
: pos(pos),
laser( (ModelLaser*)pos->GetUnusedModelOfType( MODEL_TYPE_LASER )),
ranger( (ModelRanger*)pos->GetUnusedModelOfType( MODEL_TYPE_RANGER )),
- fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL )),
+ fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL )),
+ blobfinder( (ModelBlobfinder*)pos->GetUnusedModelOfType( MODEL_TYPE_BLOBFINDER )),
source(source),
sink(sink),
avoidcount(0),
randcount(0),
work_get(0),
- work_put(0)
+ work_put(0),
+ charger_ahoy(false),
+ charger_bearing(0),
+ charger_range(0),
+ charger_heading(0),
+ mode(MODE_WORK)
{
// need at least these models to get any work done
// (pos must be good, as we used it in the initialization list)
assert( laser );
assert( source );
assert( sink );
-
+
+ // PositionUpdate() checks to see if we reached source or sink
pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this );
+
+ // LaserUpdate() controls the robot, by reading from laser and
+ // writing to position
laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this );
+ // trivial demos
+
if( fiducial ) // optional
fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate, this );
+
+ if( blobfinder ) // optional
+ blobfinder->AddUpdateCallback( (stg_model_callback_t)BlobFinderUpdate, this );
}
};
@@ -85,126 +129,241 @@ extern "C" int Init( Model* mod )
return 0; //ok
}
-// inspect the laser data and decide what to do
-int Robot::LaserUpdate( ModelLaser* laser, Robot* robot )
+
+
+void Robot::Dock()
{
-// if( laser->power_pack && laser->power_pack->charging )
-// printf( "model %s power pack @%p is charging\n",
-// laser->Token(), laser->power_pack );
-
- // Get the data
- uint32_t sample_count=0;
- stg_laser_sample_t* scan = laser->GetSamples( &sample_count );
+ if( charger_ahoy )
+ {
+ double a_goal = normalize( charger_bearing );
+
+// if( pos->Stalled() )
+// {
+// puts( "stalled. stopping" );
+// pos->Stop();
+// }
+// else
+
+ if( charger_range > 0.5 )
+ {
+ if( !ObstacleAvoid() )
+ {
+ pos->SetXSpeed( cruisespeed );
+ pos->SetTurnSpeed( a_goal );
+ }
+ }
+ else
+ {
+ pos->SetTurnSpeed( a_goal );
+ pos->SetXSpeed( 0.02 ); // creep towards it
- if( scan == NULL )
- return 0;
-
+ if( charger_range < 0.08 ) // close enough
+ pos->Stop();
+
+ if( pos->Stalled() ) // touching
+ pos->SetXSpeed( -0.01 ); // back off a bit
+
+ }
+ }
+ else
+ {
+ //printf( "docking but can't see a charger\n" );
+ pos->Stop();
+ mode = MODE_WORK; // should get us back on track eventually
+ }
+
+ // if the battery is charged, go back to work
+ if( Full() )
+ {
+ //printf( "fully charged, now back to work\n" );
+ mode = MODE_UNDOCK;
+ }
+}
+
+
+void Robot::UnDock()
+{
+ if( charger_range < 0.3 )
+ pos->SetXSpeed( -0.05 );
+ else
+ mode = MODE_WORK;
+}
+
+bool Robot::ObstacleAvoid()
+{
bool obstruction = false;
bool stop = false;
-
+
// find the closest distance to the left and right and check if
// there's anything in front
double minleft = 1e6;
double minright = 1e6;
-
- //return 0;
+ // Get the data
+ uint32_t sample_count=0;
+ stg_laser_sample_t* scan = laser->GetSamples( &sample_count );
+
for (uint32_t i = 0; i < sample_count; i++)
- {
-
+ {
if( verbose ) printf( "%.3f ", scan[i].range );
-
- if( (i > (sample_count/4))
+
+ if( (i > (sample_count/4))
&& (i < (sample_count - (sample_count/4)))
&& scan[i].range < minfrontdistance)
{
if( verbose ) puts( " obstruction!" );
obstruction = true;
}
- if( scan[i].range < stopdist )
+ if( scan[i].range < stopdist )
{
if( verbose ) puts( " stopping!" );
stop = true;
}
-
- if( i > sample_count/2 )
+
+ if( i > sample_count/2 )
minleft = MIN( minleft, scan[i].range );
- else
+ else
minright = MIN( minright, scan[i].range );
- }
+ }
if( verbose )
{
puts( "" );
printf( "minleft %.3f \n", minleft );
printf( "minright %.3f\n ", minright );
}
-
- if( obstruction || stop || (robot->avoidcount>0) )
- {
- if( verbose ) printf( "Avoid %d\n", robot->avoidcount );
-
- robot->pos->SetXSpeed( stop ? 0.0 : avoidspeed );
-
- /* once we start avoiding, select a turn direction and stick
- with it for a few iterations */
- if( robot->avoidcount < 1 )
- {
+
+ if( obstruction || stop || (avoidcount>0) )
+ {
+ if( verbose ) printf( "Avoid %d\n", avoidcount );
+
+ pos->SetXSpeed( stop ? 0.0 : avoidspeed );
+
+ /* once we start avoiding, select a turn direction and stick
+ with it for a few iterations */
+ if( avoidcount < 1 )
+ {
if( verbose ) puts( "Avoid START" );
- robot->avoidcount = random() % avoidduration + avoidduration;
+ avoidcount = random() % avoidduration + avoidduration;
if( minleft < minright )
{
- robot->pos->SetTurnSpeed( -avoidturn );
+ pos->SetTurnSpeed( -avoidturn );
if( verbose ) printf( "turning right %.2f\n", -avoidturn );
}
else
{
- robot->pos->SetTurnSpeed( +avoidturn );
+ pos->SetTurnSpeed( +avoidturn );
if( verbose ) printf( "turning left %2f\n", +avoidturn );
}
- }
+ }
- robot->avoidcount--;
- }
- else
- {
- if( verbose ) puts( "Cruise" );
+ avoidcount--;
- robot->avoidcount = 0;
- robot->pos->SetXSpeed( cruisespeed );
-
- Pose pose = robot->pos->GetPose();
+ return true; // busy avoding obstacles
+ }
+
+ return false; // didn't have to avoid anything
+}
- int x = (pose.x + 8) / 4;
- int y = (pose.y + 8) / 4;
+void Robot::Work()
+{
+ if( ! ObstacleAvoid() )
+ {
+ if( verbose ) puts( "Cruise" );
+
+ //avoidcount = 0;
+ pos->SetXSpeed( cruisespeed );
+
+ Pose pose = pos->GetPose();
+
+ int x = (pose.x + 8) / 4;
+ int y = (pose.y + 8) / 4;
+
// oh what an awful bug - 5 hours to track this down. When using
// this controller in a world larger than 8*8 meters, a_goal can
// sometimes be NAN. Causing trouble WAY upstream.
- if( x > 3 ) x = 3;
- if( y > 3 ) y = 3;
- if( x < 0 ) x = 0;
- if( y < 0 ) y = 0;
-
- double a_goal =
- dtor( robot->pos->GetFlagCount() ? have[y][x] : need[y][x] );
-
+ if( x > 3 ) x = 3;
+ if( y > 3 ) y = 3;
+ if( x < 0 ) x = 0;
+ if( y < 0 ) y = 0;
+
+ double a_goal =
+ dtor( pos->GetFlagCount() ? have[y][x] : need[y][x] );
+
+ // if we are low on juice - find the direction to the recharger instead
+ if( Hungry() )
+ {
+ //puts( "hungry - using refuel map" );
+
+ // use the refuel map
+ a_goal = dtor( refuel[y][x] );
+
+ if( charger_ahoy ) // I see a charger while hungry!
+ mode = MODE_DOCK;
+ }
+
assert( ! isnan(a_goal ) );
assert( ! isnan(pose.a ) );
+
+ double a_error = normalize( a_goal - pose.a );
+
+ assert( ! isnan(a_error) );
+
+ pos->SetTurnSpeed( a_error );
+ }
+}
- double a_error = normalize( a_goal - pose.a );
- assert( ! isnan(a_error) );
+// inspect the laser data and decide what to do
+int Robot::LaserUpdate( ModelLaser* laser, Robot* robot )
+{
+// if( laser->power_pack && laser->power_pack->charging )
+// printf( "model %s power pack @%p is charging\n",
+// laser->Token(), laser->power_pack );
+
+ if( laser->GetSamples(NULL) == NULL )
+ return 0;
- robot->pos->SetTurnSpeed( a_error );
- }
-
-
- return 0;
+ switch( robot->mode )
+ {
+ case MODE_DOCK:
+ //puts( "DOCK" );
+ robot->Dock();
+ break;
+
+ case MODE_WORK:
+ //puts( "WORK" );
+ robot->Work();
+ break;
+
+ case MODE_UNDOCK:
+ //puts( "UNDOCK" );
+ robot->UnDock();
+ break;
+
+ default:
+ printf( "unrecognized mode %u\n", robot->mode );
+ }
+
+ //if( robot->charger_ahoy )
+ //return 1;
+ //else
+ return 0;
}
+bool Robot::Hungry()
+{
+ return( pos->FindPowerPack()->ProportionRemaining() < 0.25 );
+}
+
+bool Robot::Full()
+{
+ return( pos->FindPowerPack()->ProportionRemaining() > 0.95 );
+}
+
int Robot::PositionUpdate( ModelPosition* pos, Robot* robot )
{
Pose pose = pos->GetPose();
@@ -255,26 +414,40 @@ int Robot::PositionUpdate( ModelPosition* pos, Robot* robot )
int Robot::FiducialUpdate( ModelFiducial* mod, Robot* robot )
{
+ robot->charger_ahoy = false;
+
for( unsigned int i = 0; i < mod->fiducial_count; i++ )
{
stg_fiducial_t* f = &mod->fiducials[i];
//printf( "fiducial %d is %d at %.2f m %.2f radians\n",
- // i, f->id, f->range, f->bearing );
+ // i, f->id, f->range, f->bearing );
- // if( 0 )
- if( f->range < 1 )
+ if( f->id == 2 ) // I see a charging station
{
- printf( "attempt to grab model @%p %s\n",
- f->mod, f->mod->Token() );
-
- // working on picking up models
- robot->pos->BecomeParentOf( f->mod );
- f->mod->SetPose( Pose(0,0,0,0) );
- f->mod->Disable();
+ // record that I've seen it and where it is
+ robot->charger_ahoy = true;
+ robot->charger_bearing = f->bearing;
+ robot->charger_range = f->range;
+ robot->charger_heading = f->geom.a;
+
+ //printf( "charger at %.2f radians\n", robot->charger_bearing );
+ break;
}
-
}
-
+
return 0; // run again
}
+
+int Robot::BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot )
+{
+ unsigned int blob_count = 0;
+ stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count );
+
+ if( blobs && (blob_count>0) )
+ {
+ printf( "%s sees %u blobs\n", blobmod->Token(), blob_count );
+ }
+
+ return 0;
+}
View
2 examples/ctrl/source.cc
@@ -1,7 +1,7 @@
#include "stage.hh"
using namespace Stg;
-const int INTERVAL = 50;
+const int INTERVAL = 200;
int Update( Model* mod, void* dummy );
View
36 libstage/block.cc
@@ -22,17 +22,13 @@ Block::Block( Model* mod,
inherit_color( inherit_color ),
rendered_cells( g_ptr_array_sized_new(32) ),
candidate_cells( g_ptr_array_sized_new(32) )
- // _gpts( NULL )
{
assert( mod );
assert( pt_count > 0 );
assert( pts );
local_z.min = zmin;
local_z.max = zmax;
-
- // add this block's global coords array to a global list
- //g_ptr_array_add( global_verts, this );
}
/** A from-file constructor */
@@ -46,16 +42,12 @@ Block::Block( Model* mod,
inherit_color(true),
rendered_cells( g_ptr_array_sized_new(32) ),
candidate_cells( g_ptr_array_sized_new(32) )
- // _gpts( NULL )
{
assert(mod);
assert(wf);
assert(entity);
Load( wf, entity );
-
- // add this block's global coords array to a global list
- //g_ptr_array_add( global_verts, this );
}
@@ -67,17 +59,34 @@ Block::~Block()
g_ptr_array_free( rendered_cells, TRUE );
g_ptr_array_free( candidate_cells, TRUE );
-
- //free( _gpts );
- //g_ptr_array_remove( global_verts, this );
}
stg_color_t Block::GetColor()
{
return( inherit_color ? mod->color : color );
}
+GList* Block::AppendTouchingModels( GList* list )
+{
+ // for every cell we are rendered into
+ for( unsigned int i=0; i<rendered_cells->len; i++ )
+ {
+ Cell* cell = (Cell*)g_ptr_array_index( rendered_cells, i);
+
+ // for every block rendered into that cell
+ for( GSList* it = cell->list; it; it=it->next )
+ {
+ Block* testblock = (Block*)it->data;
+ Model* testmod = testblock->mod;
+
+ if( !mod->IsRelated( testmod ))
+ if( ! g_list_find( list, testmod ) )
+ list = g_list_append( list, testmod );
+ }
+ }
+ return list;
+}
Model* Block::TestCollision()
{
@@ -86,8 +95,9 @@ Model* Block::TestCollision()
// find the set of cells we would render into given the current global pose
GenerateCandidateCells();
- // for every cell we may be rendered into
- for( unsigned int i=0; i<candidate_cells->len; i++ )
+ if( mod->vis.obstacle_return )
+ // for every cell we may be rendered into
+ for( unsigned int i=0; i<candidate_cells->len; i++ )
{
Cell* cell = (Cell*)g_ptr_array_index(candidate_cells, i);
View
8 libstage/blockgroup.cc
@@ -50,6 +50,14 @@ void BlockGroup::SwitchToTestedCells()
LISTMETHOD( blocks, Block*, SwitchToTestedCells );
}
+GList* BlockGroup::AppendTouchingModels( GList* list )
+{
+ for( GList* it=blocks; it; it = it->next )
+ list = ((Block*)it->data)->AppendTouchingModels( list );
+
+ return list;
+}
+
Model* BlockGroup::TestCollision()
{
//printf( "blockgroup %p test collision...\n", this );
View
2 libstage/canvas.cc
@@ -139,7 +139,7 @@ void Canvas::InitGl()
GLuint stall_id = TextureManager::getInstance().loadTexture( fullpath.c_str() );
TextureManager::getInstance()._stall_texture_id = stall_id;
- fullpath = FileManager::findFile( "assets/mains.png" );
+ fullpath = FileManager::findFile( "assets/mainspower.png" );
if ( fullpath == "" )
{
PRINT_DEBUG( "Unable to load mains texture.\n" );
View
111 libstage/model.cc
@@ -1050,13 +1050,13 @@ void Model::DrawStatusTree( Camera* cam )
void Model::DrawStatus( Camera* cam )
{
// quick hack
- if( power_pack && power_pack->stored < 0.0 )
- {
- glPushMatrix();
- glTranslatef( 0.3, 0, 0.0 );
- DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 );
- glPopMatrix();
- }
+// if( power_pack && power_pack->stored < 0.0 )
+// {
+// glPushMatrix();
+// glTranslatef( 0.3, 0, 0.0 );
+// DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 );
+// glPopMatrix();
+// }
if( say_string || power_pack )
{
@@ -1081,7 +1081,7 @@ void Model::DrawStatus( Camera* cam )
//if( ! parent )
// glRectf( 0,0,1,1 );
- if( power_pack->stored > 0.0 )
+ //if( power_pack->stored > 0.0 )
power_pack->Visualize( cam );
if( say_string )
@@ -1169,7 +1169,8 @@ stg_meters_t Model::ModelHeight()
return geom.size.z + m_child;
}
-void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha )
+
+void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width, double height )
{
float yaw, pitch;
pitch = - cam->pitch();
@@ -1192,16 +1193,17 @@ void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha )
//draw a square, with the textured image
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f );
- glTexCoord2f(1.0f, 0.0f); glVertex3f( 0.25f, 0, -0.25f );
- glTexCoord2f(1.0f, 1.0f); glVertex3f( 0.25f, 0, 0.25f );
- glTexCoord2f(0.0f, 1.0f); glVertex3f(-0.25f, 0, 0.25f );
+ glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f );
+ glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f );
+ glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f );
glEnd();
glPopMatrix();
glBindTexture( GL_TEXTURE_2D, 0 );
glDisable(GL_TEXTURE_2D);
}
+
void Model::DrawFlagList( void )
{
if( flag_list == NULL )
@@ -1605,47 +1607,72 @@ int Model::SetParent( Model* newparent)
void Model::PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax,
stg_meters_t ymin, stg_meters_t ymax )
{
- while( TestCollision() )
+ while( TestCollisionTree() )
SetPose( Pose::Random( xmin,xmax, ymin, ymax ));
}
+GList* Model::AppendTouchingModels( GList* list )
+{
+ return blockgroup.AppendTouchingModels( list );
+}
+
Model* Model::TestCollision()
{
- //printf( "mod %s test collision...\n", token );
-
- Model* hitmod = blockgroup.TestCollision();
+ //printf( "mod %s test collision...\n", token );
+ return( blockgroup.TestCollision() );
+}
+
+Model* Model::TestCollisionTree()
+{
+ Model* hitmod = TestCollision();
if( hitmod == NULL )
for( GList* it = children; it; it=it->next )
{
- hitmod = ((Model*)it->data)->TestCollision();
+ hitmod = ((Model*)it->data)->TestCollisionTree();
if( hitmod )
break;
}
- if( hitmod && (watts_take > 0.0) )
- {
- PowerPack* pp = FindPowerPack();
-
- if( pp )
- {
- if( hitmod->FindPowerPack() && (hitmod->watts_give > 0.0) )
- {
- stg_watts_t rate = MIN( watts_take, hitmod->watts_give );
- stg_joules_t amount = rate * (world->interval_sim * 1e-6);
-
- // move some joules from him to me
- hitmod->FindPowerPack()->TransferTo( FindPowerPack(), amount );
- }
- else
- pp->charging = false;
- }
- }
-
-
//printf( "mod %s test collision done.\n", token );
return hitmod;
+}
+
+void Model::UpdateCharge()
+{
+ //printf( "model %s is a charger. testing to see if anyone is touching\n",
+ // token );x
+
+ assert( watts_give > 0 );
+
+ PowerPack* mypp = FindPowerPack();
+ assert( mypp );
+
+ for( GList* touchers = AppendTouchingModels( NULL );
+ touchers;
+ touchers = touchers->next )
+ {
+ Model* toucher = (Model*)touchers->data;
+
+ PowerPack* hispp =toucher-> FindPowerPack();
+ if( hispp && toucher->watts_take > 0.0)
+ {
+ //printf( " toucher %s can take up to %.2f wats\n",
+ // toucher->Token(), toucher->watts_take );
+
+ stg_watts_t rate = MIN( watts_give, toucher->watts_take );
+ stg_joules_t amount = rate * (world->interval_sim * 1e-6);
+
+ //printf ( "moving %.2f joules from %s to %s\n",
+ // amount, token, toucher->token );
+
+ // move some joules from me to him
+ mypp->TransferTo( hispp, amount );
+ hispp->charging = true;
+ }
+ }
+
}
void Model::CommitTestedPose()
@@ -1665,7 +1692,7 @@ Model* Model::ConditionalMove( Pose newpose )
Pose startpose = pose;
pose = newpose; // do the move provisionally - we might undo it below
- Model* hitmod = TestCollision();
+ Model* hitmod = TestCollisionTree();
if( hitmod )
pose = startpose; // move failed - put me back where I started
@@ -1683,13 +1710,17 @@ Model* Model::ConditionalMove( Pose newpose )
return hitmod;
}
+
void Model::UpdatePose( void )
{
if( disabled )
return;
- if( velocity.x == 0 && velocity.y == 0 && velocity.a == 0 && velocity.z == 0 )
- return;
+ if( velocity.IsZero() )
+ {
+ PRINT_WARN1( "model %s has velocity zero but its pose is being updated", token );
+ return;
+ }
// TODO - control this properly, and maybe do it faster
//if( 0 )
View
34 libstage/model_fiducial.cc
@@ -175,14 +175,14 @@ void ModelFiducial::AddModelIfVisible( Model* him )
max_range_anon,
fiducial_raytrace_match,
NULL,
- false );
+ true );
//range = ray.range;
Model* hitmod = ray.mod;
-// printf( "ray hit %s and was seeking LOS to %s\n",
-// hitmod ? hitmod->Token() : "null",
-// him->Token() );
+ //printf( "ray hit %s and was seeking LOS to %s\n",
+ // hitmod ? hitmod->Token() : "null",
+ // him->Token() );
// if it was him, we can see him
if( hitmod == him )
@@ -262,23 +262,23 @@ void ModelFiducial::DataVisualize( Camera* cam )
if ( !showFiducialData )
return;
- // draw the FOV
- GLUquadric* quadric = gluNewQuadric();
+// // draw the FOV
+// GLUquadric* quadric = gluNewQuadric();
- PushColor( 0,0,0,0.2 );
+// PushColor( 0,0,0,0.2 );
- gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
+// gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
- gluPartialDisk( quadric,
- 0,
- max_range_anon,
- 20, // slices
- 1, // loops
- rtod( M_PI/2.0 + fov/2.0), // start angle
- rtod(-fov) ); // sweep angle
+// gluPartialDisk( quadric,
+// 0,
+// max_range_anon,
+// 20, // slices
+// 1, // loops
+// rtod( M_PI/2.0 + fov/2.0), // start angle
+// rtod(-fov) ); // sweep angle
- gluDeleteQuadric( quadric );
- PopColor();
+// gluDeleteQuadric( quadric );
+// PopColor();
if( data->len == 0 )
return;
View
10 libstage/model_load.cc
@@ -41,7 +41,7 @@ void Model::Load()
power_pack = new PowerPack( this );
power_pack->capacity =
- wf->ReadFloat( wf_entity, "joules_stored", power_pack->capacity );
+ wf->ReadFloat( wf_entity, "joules_capacity", power_pack->capacity );
}
/** if the capacity has been specified, limit the store to the capacity */
@@ -53,7 +53,7 @@ void Model::Load()
power_pack->stored,
power_pack->capacity );
}
-
+
// use my own pack or an ancestor's for the other energy properties
PowerPack* pp = FindPowerPack();
@@ -65,11 +65,15 @@ void Model::Load()
if( (watts_give > 0.0) && !pp)
PRINT_WARN1( "Model %s: Setting \"watts_give\" has no effect unless \"joules\" is specified for this model or a parent", token );
+ if( watts_give ) // need to get the world to test this model for charging
+ if( ! g_list_find( world->charge_list, this ) )
+ world->charge_list = g_list_append( world->charge_list, this );
+
watts_take = wf->ReadFloat( wf_entity, "take_watts", watts_take );
if( (watts_take > 0.0) & !pp )
PRINT_WARN1( "Model %s: Setting \"watts_take\" has no effect unless \"joules\" is specified for this model or a parent", token );
-
+
if( wf->PropertyExists( wf_entity, "debug" ) )
{
PRINT_WARN2( "debug property specified for model %d %s\n",
View
5 libstage/model_position.cc
@@ -475,6 +475,11 @@ void ModelPosition::Shutdown( void )
Model::Shutdown();
}
+void ModelPosition::Stop()
+{
+ SetSpeed( 0,0,0 );
+}
+
void ModelPosition::SetSpeed( double x, double y, double a )
{
assert( ! isnan(x) );
View
70 libstage/powerpack.cc
@@ -27,14 +27,35 @@ void PowerPack::Visualize( Camera* cam )
const double height = 0.5;
const double width = 0.2;
+ // draw an electric zap
+// glPolygonMode( GL_FRONT, GL_LINE );
+// glBegin( GL_POLYGON );
+// glVertex2i( 0, 0 );
+// glVertex2i( 3, 2 );
+// glVertex2i( 1, 2 );
+// glEnd();
+
+// glVertex2i( 1, 3 );
+// glVertex2i( 0, 3 );
+// glVertex2i( 1, 5 );
+// glVertex2i( 3, 5 );
+// glVertex2i( 4, 3 );
+// glVertex2i( 5, 3 );
+// glVertex2i( 4, 4 );
+// glVertex2i( 5, 4);
+// glEnd();
+ //}
+
double percent = stored/capacity * 100.0;
-
+
+ const double alpha = 0.5;
+
if( percent > 50 )
- glColor4f( 0,1,0, 0.7 ); // green
+ glColor4f( 0,1,0, alpha ); // green
else if( percent > 25 )
- glColor4f( 1,0,1, 0.7 ); // magenta
+ glColor4f( 1,0,1, alpha ); // magenta
else
- glColor4f( 1,0,0, 0.7 ); // red
+ glColor4f( 1,0,0, alpha ); // red
static char buf[6];
snprintf( buf, 6, "%.0f", percent );
@@ -59,6 +80,36 @@ void PowerPack::Visualize( Camera* cam )
glVertex2f( width, fullness );
glEnd();
+ if( stored < 0.0 ) // inifinite supply!
+ {
+ // draw an arrow toward the top
+ glBegin( GL_LINES );
+ glVertex2f( width/3.0, height/3.0 );
+ glVertex2f( 2.0 * width/3, height/3.0 );
+
+ glVertex2f( width/3.0, height/3.0 );
+ glVertex2f( width/3.0, height - height/5.0 );
+
+ glVertex2f( width/3.0, height - height/5.0 );
+ glVertex2f( 0, height - height/5.0 );
+
+ glVertex2f( 0, height - height/5.0 );
+ glVertex2f( width/2.0, height );
+
+ glVertex2f( width/2.0, height );
+ glVertex2f( width, height - height/5.0 );
+
+ glVertex2f( width, height - height/5.0 );
+ glVertex2f( 2.0 * width/3.0, height - height/5.0 );
+
+ glVertex2f( 2.0 * width/3.0, height - height/5.0 );
+ glVertex2f( 2.0 * width/3, height/3.0 );
+
+ glEnd();
+
+ }
+
+
if( charging )
{
glLineWidth( 6.0 );
@@ -85,8 +136,6 @@ stg_joules_t PowerPack::RemainingCapacity()
void PowerPack::Add( stg_joules_t j )
{
stored += MIN( RemainingCapacity(), j );
-
- charging = true;
}
void PowerPack::Subtract( stg_joules_t j )
@@ -97,6 +146,9 @@ void PowerPack::Subtract( stg_joules_t j )
void PowerPack::TransferTo( PowerPack* dest, stg_joules_t amount )
{
+ //printf( "amount %.2f stored %.2f dest capacity %.2f\n",
+ // amount, stored, dest->RemainingCapacity() );
+
// if stored is non-negative we can't transfer more than the stored
// amount. If it is negative, we have infinite energy stored
if( stored >= 0.0 )
@@ -105,10 +157,10 @@ void PowerPack::TransferTo( PowerPack* dest, stg_joules_t amount )
// we can't transfer more than he can take
amount = MIN( amount, dest->RemainingCapacity() );
- printf( "%s receives %.3f J from %s\n",
- mod->Token(), amount, dest->mod->Token() );
+
+ //printf( "%s gives %.3f J to %s\n",
+ // mod->Token(), amount, dest->mod->Token() );
-
Subtract( amount );
dest->Add( amount );
}
View
41 libstage/stage.hh
@@ -305,6 +305,8 @@ namespace Stg
printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
prefix, x,y,z,a );
}
+
+ bool IsZero(){ return( !(x || y || z || a )); };
};
/** Specify an object's basic geometry: position and rectangular
@@ -820,6 +822,7 @@ namespace Stg
static void UpdateCb( World* world);
static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids
+ GList* charge_list; ///< Models which receive charge are listed here
bool destroy;
bool dirty; ///< iff true, a gui redraw would be required
GHashTable* models_by_name; ///< the models that make up the world, indexed by name
@@ -933,13 +936,19 @@ namespace Stg
bool PastQuitTime();
void StartUpdatingModel( Model* mod )
- { update_list = g_list_append( update_list, mod ); }
+ {
+ if( ! g_list_find( update_list, mod ) )
+ update_list = g_list_append( update_list, mod );
+ }
void StopUpdatingModel( Model* mod )
{ update_list = g_list_remove( update_list, mod ); }
void StartUpdatingModelPose( Model* mod )
- { velocity_list = g_list_append( velocity_list, mod ); }
+ {
+ if( ! g_list_find( velocity_list, mod ) )
+ velocity_list = g_list_append( velocity_list, mod );
+ }
void StopUpdatingModelPose( Model* mod )
{ velocity_list = g_list_remove( velocity_list, mod ); }
@@ -1047,8 +1056,12 @@ namespace Stg
// /** Prepare to render the block in a new position in global coordinates */
// void SetPoseTentative( const Pose pose );
+
+ GList* AppendTouchingModels( GList* list );
+
+ /** Returns the first model that shares a bitmap cell with this model */
Model* TestCollision();
-
+
void SwitchToTestedCells();
void Load( Worldfile* wf, int entity );
@@ -1122,9 +1135,13 @@ namespace Stg
void CallDisplayList( Model* mod );
void Clear() ; /** deletes all blocks from the group */
+ GList* AppendTouchingModels( GList* list );
+ //void AddTouchingModelsToList( GList* list );
+
/** Returns a pointer to the first model detected to be colliding
with a block in this group, or NULL, if none are detected. */
Model* TestCollision();
+
void SwitchToTestedCells();
void Map();
@@ -1392,6 +1409,12 @@ namespace Stg
/** Transfer some stored energy to another power pack */
void TransferTo( PowerPack* dest, stg_joules_t amount );
+
+ double ProportionRemaining()
+ { return( stored / capacity ); }
+
+ void Print( const char* prefix )
+ { printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); }
};
class Visibility
@@ -1631,10 +1654,17 @@ namespace Stg
void registerOption( Option* opt )
{ drawOptions.push_back( opt ); }
+ GList* AppendTouchingModels( GList* list );
+ //void AddTouchingModelsToList( GList* list );
+
/** Check to see if the current pose will yield a collision with
obstacles. Returns a pointer to the first entity we are in
collision with, or NULL if no collision exists. */
Model* TestCollision();
+
+ /** Recursively call TestCollision() on this model and all its
+ descendents */
+ Model* TestCollisionTree();
void CommitTestedPose();
@@ -1690,6 +1720,7 @@ namespace Stg
virtual void Shutdown();
virtual void Update();
virtual void UpdatePose();
+ virtual void UpdateCharge();
void StartUpdating();
void StopUpdating();
@@ -1715,7 +1746,7 @@ namespace Stg
void PopCoords();
/** Draw the image stored in texture_id above the model */
- void DrawImage( uint32_t texture_id, Camera* cam, float alpha );
+ void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 );
/** static wrapper for DrawBlocks() */
@@ -2575,6 +2606,8 @@ namespace Stg
void SetZSpeed( double z );
void SetTurnSpeed( double a );
void SetSpeed( Velocity vel );
+ /** Set velocity along all axes to to zero. */
+ void Stop();
/** Sets the control mode to STG_POSITION_CONTROL_POSITION and sets
the goal pose */
View
5 libstage/world.cc
@@ -73,6 +73,7 @@ World::World( const char* token,
double ppm )
:
// private
+ charge_list( NULL ),
destroy( false ),
dirty( true ),
models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ),
@@ -426,6 +427,10 @@ bool World::Update()
// upate all positions first
LISTMETHOD( velocity_list, Model*, UpdatePose );
+
+ // test all models that supply charge to see if they are touching
+ // something that takes charge
+ LISTMETHOD( charge_list, Model*, UpdateCharge );
// then update all sensors
if( worker_threads == 0 ) // do all the work in this thread
View
111 worlds/fasr.world
@@ -7,24 +7,24 @@ include "map.inc"
include "sick.inc"
interval_sim 100 # simulation timestep in milliseconds
-interval_real 30 # real-time interval between simulation updates in milliseconds
+interval_real 0 # real-time interval between simulation updates in milliseconds
paused 1
resolution 0.02
# threads may speed things up here depending on available CPU cores & workload
-threadpool 0
-# threadpool 2
+ threadpool 0
+# threadpool 3
# configure the GUI window
window
(
- size [ 600.000 599.000 ]
+ size [ 902.000 856.000 ]
- center [ 4.605 -2.392 ]
+ center [ 6.726 -2.592 ]
rotate [ 0 0 ]
- scale 68.242
+ scale 116.966
pcam_loc [ 0 -4.000 2.000 ]
pcam_angle [ 70.000 0 ]
@@ -61,80 +61,77 @@ zone
ctrl "sink"
)
-define autorob pioneer2dx
-(
- sicklaser( samples 32 range_max 5 laser_return 2 watts 30 )
- ctrl "fasr"
- joules 1000000
- fiducial_return 1
-)
-
define charging_bump model
(
- pose [0.210 0 -0.100 0 ]
- size [0.050 0.100 0.100]
- take_watts 300.0
+ pose [0.240 0 -0.100 0 ]
+ size [0.120 0.050 0.100]
+ take_watts 1000.0
color "orange"
- obstacle_return 1
+ obstacle_return 0
+)
+
+define autorob pioneer2dx
+(
+ sicklaser( samples 32 range_max 5 laser_return 2 watts 30 )
+ ctrl "fasr"
+ joules 100000
+ joules_capacity 400000
+ fiducial_return 0
+ charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) )
)
define charge_station model
(
- color "yellow"
- size [ 0.100 0.500 0.400 ]
- joules -1 # infinite storage
- give_watts 500
-)
+ size [ 0.100 0.300 0.100 ]
+ color "purple"
-charge_station
-(
- pose [ 7.941 -3.001 0 0 ]
-)
+ model( color "purple" size [0.100 0.100 0.400] pose [ 0 0.100 0 0 ] )
-autorob
-(
- pose [2.933 0.917 0 -147.323]
- color "magenta"
- joules 10000
- joules_capacity 100000
- charging_bump()
-)
+ model(
+ pose [ 0.010 0 0 0 ]
+ color "yellow"
+ size [ 0.050 0.100 0.100 ]
+ joules -1 # infinite storage
+ give_watts 1000
+ fiducial_return 2
+ )
-autorob
-(
- pose [5.757 -3.528 0 14.941]
- color "green"
- charging_bump( take_watts 0 give_watts 300.0 )
+ model( color "purple" size [0.100 0.100 0.400] pose [ 0 -0.100 0 0 ] )
)
-autorob( pose [5.937 4.858 0 -147.503] )
+charge_station( pose [ 7.803 -1.332 0 34.377 ] )
+charge_station( pose [ 7.940 -2.349 0 0 ] )
+charge_station( pose [ 7.931 -3.367 0 0 ] )
+charge_station( pose [ 7.931 -4.444 0 0 ] )
+
+autorob( pose [4.144 6.834 0 -98.076] )
autorob( pose [7.574 6.269 0 -111.715] )
-autorob( pose [5.664 5.938 0 107.666] )
-autorob( pose [7.016 6.428 0 -128.279] )
+autorob( pose [5.615 6.185 0 107.666] )
+autorob( pose [7.028 6.502 0 -128.279] )
autorob( pose [5.750 4.137 0 -97.047] )
autorob( pose [4.909 6.097 0 -44.366] )
autorob( pose [6.898 4.775 0 -117.576] )
-autorob( pose [7.012 5.706 0 129.497] )
-
-autorob( pose [6.616 6.893 0 170.743] )
+autorob( pose [7.394 5.595 0 129.497] )
+autorob( pose [6.468 6.708 0 170.743] )
autorob( pose [6.451 4.189 0 -61.453] )
-autorob( pose [5.098 6.788 0 -61.295] )
-autorob( pose [4.374 5.163 0 -147.713] )
-autorob( pose [4.999 4.230 0 -125.236] )
-autorob( pose [3.533 4.220 0 78.789] )
+
+autorob( pose [5.246 6.813 0 -61.295] )
+autorob( pose [4.127 5.388 0 -147.713] )
+autorob( pose [5.020 4.213 0 -125.236] )
+autorob( pose [3.286 4.715 0 78.789] )
autorob( pose [5.440 5.317 0 -26.545] )
-autorob( pose [7.518 6.973 0 163.239] )
+autorob( pose [7.641 6.998 0 163.239] )
autorob( pose [7.559 4.764 0 -139.066] )
-autorob( pose [5.940 6.768 0 77.301] )
+autorob( pose [5.471 7.446 0 77.301] )
+autorob( pose [7.122 4.175 0 -31.440] )
+autorob( pose [5.944 6.951 0 2.937] )
-#autorob( pose [7.122 4.175 0 -31.440] )
-#autorob( pose [6.203 6.963 0 2.937] )
#autorob( pose [6.800 5.897 0 -103.060] )
-#autorob( pose [6.331 6.450 0 -103.060] )
+#autorob( pose [6.405 5.291 0 -103.060] )
#autorob( pose [5.974 5.725 0 -103.060] )
#autorob( pose [4.151 7.272 0 53.540] )
#autorob( pose [6.545 7.459 0 2.937] )
-#autorob( pose [7.225 7.459 0 34.450] )
+#autorob( pose [7.237 7.533 0 34.450] )
#autorob( pose [3.875 6.533 0 134.717] )
-#autorob( pose [3.944 5.045 0 -103.060] )
+#autorob( pose [3.944 4.674 0 -103.060] )
#autorob( pose [4.634 6.897 0 -103.060] )

0 comments on commit ff0dbdd

Please sign in to comment.