Permalink
Browse files

grippers partially implemented. they don't lift anything yet

  • Loading branch information...
rtv
rtv committed Feb 13, 2009
1 parent 626aacd commit 41a7d4a7966e538e0fdf8a2945674725cd93459a
Showing with 310 additions and 121 deletions.
  1. +33 −2 examples/ctrl/fasr.cc
  2. +1 −0 libstage/CMakeLists.txt
  3. +74 −0 libstage/block.cc
  4. +14 −10 libstage/model.cc
  5. +1 −1 libstage/model_fiducial.cc
  6. +2 −1 libstage/model_laser.cc
  7. +131 −68 libstage/stage.hh
  8. +4 −0 libstage/typetable.cc
  9. +50 −39 worlds/fasr.world
View
@@ -50,6 +50,7 @@ class Robot
ModelRanger* ranger;
ModelFiducial* fiducial;
ModelBlobfinder* blobfinder;
+ ModelGripper* gripper;
Model *source, *sink;
int avoidcount, randcount;
int work_get, work_put;
@@ -82,6 +83,7 @@ class Robot
ranger( (ModelRanger*)pos->GetUnusedModelOfType( MODEL_TYPE_RANGER )),
fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL )),
blobfinder( (ModelBlobfinder*)pos->GetUnusedModelOfType( MODEL_TYPE_BLOBFINDER )),
+ gripper( (ModelGripper*)pos->GetUnusedModelOfType( MODEL_TYPE_GRIPPER )),
source(source),
sink(sink),
avoidcount(0),
@@ -133,6 +135,14 @@ extern "C" int Init( Model* mod )
void Robot::Dock()
{
+ // close the grippers so they can be pushed into the charger
+ ModelGripper::data_t gripper_data = gripper->GetData();
+
+ if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED )
+ gripper->CommandClose();
+ else if( gripper_data.lift != ModelGripper::LIFT_UP )
+ gripper->CommandUp();
+
if( charger_ahoy )
{
double a_goal = normalize( charger_bearing );
@@ -183,9 +193,30 @@ void Robot::Dock()
void Robot::UnDock()
{
- if( charger_range < 0.3 )
- pos->SetXSpeed( -0.05 );
+ const stg_meters_t gripper_distance = 0.2;
+ const stg_meters_t back_off_distance = 0.3;
+ const stg_meters_t back_off_speed = -0.05;
+
+ // back up a bit
+ if( charger_range < back_off_distance )
+ pos->SetXSpeed( back_off_speed );
else
+ pos->SetXSpeed( 0.0 );
+
+ // once we have backed off a bit, open and lower the gripper
+ ModelGripper::data_t gripper_data = gripper->GetData();
+ if( charger_range > gripper_distance )
+ {
+ if( gripper_data.paddles != ModelGripper::PADDLE_OPEN )
+ gripper->CommandOpen();
+ else if( gripper_data.lift != ModelGripper::LIFT_DOWN )
+ gripper->CommandDown();
+ }
+
+ // if the gripper is down and open and we're away from the charger, undock is finished
+ if( gripper_data.paddles == ModelGripper::PADDLE_OPEN &&
+ gripper_data.lift == ModelGripper::LIFT_DOWN &&
+ charger_range > back_off_distance )
mode = MODE_WORK;
}
View
@@ -4,6 +4,7 @@ MESSAGE( STATUS "Configuring libstage" )
include_directories(${PROJECT_BINARY_DIR})
set( stageSrcs ancestor.cc
+ model_gripper.cc
block.cc
blockgroup.cc
camera.cc
View
@@ -61,6 +61,80 @@ Block::~Block()
g_ptr_array_free( candidate_cells, TRUE );
}
+void Block::Translate( double x, double y )
+{
+ for( unsigned int p=0; p<pt_count; p++)
+ {
+ pts[p].x += x;
+ pts[p].y += y;
+ }
+
+ // force redraw
+ mod->blockgroup.BuildDisplayList( mod );
+}
+
+
+double Block::CenterY()
+{
+ double min = billion;
+ double max = -billion;
+
+ for( unsigned int p=0; p<pt_count; p++)
+ {
+ if( pts[p].y > max ) max = pts[p].y;
+ if( pts[p].y < min ) min = pts[p].y;
+ }
+
+ // return the value half way between max and min
+ return( min + (max - min)/2.0 );
+}
+
+double Block::CenterX()
+{
+ double min = billion;
+ double max = -billion;
+
+ for( unsigned int p=0; p<pt_count; p++)
+ {
+ if( pts[p].x > max ) max = pts[p].x;
+ if( pts[p].x < min ) min = pts[p].x;
+ }
+
+ // return the value half way between maxx and min
+ return( min + (max - min)/2.0 );
+}
+
+void Block::SetCenter( double x, double y )
+{
+ // move the block by the distance required to bring its center to
+ // the requested position
+ Translate( x-CenterX(), y-CenterY() );
+}
+
+void Block::SetCenterY( double y )
+{
+ // move the block by the distance required to bring its center to
+ // the requested position
+ Translate( 0, y-CenterY() );
+}
+
+void Block::SetCenterX( double x )
+{
+ // move the block by the distance required to bring its center to
+ // the requested position
+ Translate( x-CenterX(), 0 );
+}
+
+void Block::SetZ( double min, double max )
+{
+ local_z.min = min;
+ local_z.max = max;
+
+ // force redraw
+ mod->blockgroup.BuildDisplayList( mod );
+}
+
+
stg_color_t Block::GetColor()
{
return( inherit_color ? mod->color : color );
View
@@ -374,11 +374,11 @@ void Model::LoadBlock( Worldfile* wf, int entity )
}
-void Model::AddBlockRect( stg_meters_t x,
- stg_meters_t y,
- stg_meters_t dx,
- stg_meters_t dy,
- stg_meters_t dz )
+Block* Model::AddBlockRect( stg_meters_t x,
+ stg_meters_t y,
+ stg_meters_t dx,
+ stg_meters_t dy,
+ stg_meters_t dz )
{
UnMap();
@@ -392,11 +392,15 @@ void Model::AddBlockRect( stg_meters_t x,
pts[3].x = x;
pts[3].y = y + dy;
- blockgroup.AppendBlock( new Block( this,
- pts, 4,
- 0, dz,
- color,
- true ) );
+ Block* newblock = new Block( this,
+ pts, 4,
+ 0, dz,
+ color,
+ true );
+
+ blockgroup.AppendBlock( newblock );
+
+ return newblock;
}
@@ -262,7 +262,7 @@ void ModelFiducial::DataVisualize( Camera* cam )
if ( !showFiducialData )
return;
-// // draw the FOV
+ // draw the FOV
// GLUquadric* quadric = gluNewQuadric();
// PushColor( 0,0,0,0.2 );
View
@@ -127,7 +127,6 @@ ModelLaser::~ModelLaser( void )
void ModelLaser::Load( void )
{
- Model::Load();
sample_count = wf->ReadInt( wf_entity, "samples", sample_count );
range_min = wf->ReadLength( wf_entity, "range_min", range_min);
range_max = wf->ReadLength( wf_entity, "range_max", range_max );
@@ -142,6 +141,8 @@ void ModelLaser::Load( void )
PRINT_WARN( "laser resolution set < 1. Forcing to 1" );
resolution = 1;
}
+
+ Model::Load();
}
stg_laser_cfg_t ModelLaser::GetConfig()
Oops, something went wrong.

0 comments on commit 41a7d4a

Please sign in to comment.