Revision: 7530
http://playerstage.svn.sourceforge.net/playerstage/?rev=7530&view=rev
Author: natepak
Date: 2009-03-18 17:29:30 +0000 (Wed, 18 Mar 2009)
Log Message:
-----------
Updates
Modified Paths:
--------------
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
Modified:
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
===================================================================
---
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
2009-03-18 03:00:08 UTC (rev 7529)
+++
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
2009-03-18 17:29:30 UTC (rev 7530)
@@ -70,11 +70,23 @@
void Pioneer2_Gripper::LoadChild(XMLConfigNode *node)
{
XMLConfigNode *jNode;
- this->myIface = dynamic_cast<GripperIface*>(this->ifaces[0]);
+ this->gripIface = dynamic_cast<GripperIface*>(this->ifaces[0]);
- if (!this->myIface)
- gzthrow("Pioneer2_Gripper controller requires a GripperIface");
+ if (!this->gripIface)
+ {
+ this->gripIface = dynamic_cast<GripperIface*>(this->ifaces[1]);
+ if (!this->gripIface)
+ gzthrow("Pioneer2_Gripper controller requires a GripperIface");
+ }
+ this->actIface = dynamic_cast<ActarrayIface*>(this->ifaces[1]);
+ if (!this->actIface)
+ {
+ this->actIface = dynamic_cast<ActarrayIface*>(this->ifaces[0]);
+ if (!this->actIface)
+ gzthrow("Pioneer2_Gripper controller requires an ActarrayIface");
+ }
+
Param::Begin(&this->parameters);
jNode = node->GetChild("leftJoint");
if (jNode)
@@ -166,9 +178,9 @@
double rightPaddlePos = this->joints[RIGHT]->GetPosition();
*/
- this->myIface->Lock(1);
+ this->gripIface->Lock(1);
- switch( this->myIface->data->cmd)
+ switch( this->gripIface->data->cmd)
{
case GAZEBO_GRIPPER_CMD_OPEN:
this->joints[RIGHT]->SetParam(dParamVel,0.1);
@@ -180,18 +192,19 @@
this->joints[LEFT]->SetParam(dParamVel,0.1);
break;
- case GAZEBO_GRIPPER_CMD_STORE:
+/* case GAZEBO_GRIPPER_CMD_STORE:
this->joints[LIFT]->SetParam(dParamVel, 0.2);
break;
case GAZEBO_GRIPPER_CMD_RETRIEVE:
this->joints[LIFT]->SetParam(dParamVel, -0.2);
break;
+ */
case GAZEBO_GRIPPER_CMD_STOP:
this->joints[RIGHT]->SetParam(dParamVel,0);
this->joints[LEFT]->SetParam(dParamVel,0);
- this->joints[LIFT]->SetParam(dParamVel,0);
+ // this->joints[LIFT]->SetParam(dParamVel,0);
break;
@@ -203,7 +216,13 @@
*/
}
+ // Move the lift
+ if (this->actIface->data->cmd_pos[0] > 0.5)
+ this->joints[LIFT]->SetParam(dParamVel, 0.2);
+ else if (this->actIface->data->cmd_pos[0] < 0.5)
+ this->joints[LIFT]->SetParam(dParamVel, -0.2);
+
this->joints[LEFT]->SetParam(dParamFMax, **(this->forcesP[LEFT]));
this->joints[RIGHT]->SetParam(dParamFMax, **(this->forcesP[RIGHT]));
this->joints[LIFT]->SetParam(dParamFMax, **(this->forcesP[LIFT]));
@@ -212,6 +231,9 @@
/*printf("Left Pos[%f] High[%f]
Low[%f]\n",this->joints[LEFT]->GetPosition(),
this->joints[LEFT]->GetHighStop(),this->joints[LEFT]->GetLowStop() );
printf("Right Pos[%f] High[%f]
Low[%f]\n",this->joints[RIGHT]->GetPosition(),
this->joints[RIGHT]->GetHighStop(),this->joints[RIGHT]->GetLowStop() );
*/
+ printf("Lift Pos[%f] High[%f] Low[%f]\n", this->joints[LIFT]->GetPosition(),
+ this->joints[LIFT]->GetHighStop(), this->joints[LIFT]->GetLowStop());
+
// Set the state of the paddles
if (fabs(this->joints[LEFT]->GetPosition() -
@@ -219,27 +241,36 @@
fabs(this->joints[RIGHT]->GetPosition() -
this->joints[RIGHT]->GetLowStop()) < 0.01)
{
- this->myIface->state = GAZEBO_GRIPPER_CLOSED;
- this->myIface->left_paddle_open = 0;
- this->myIface->right_paddle_open = 0;
+ this->gripIface->data->state = GAZEBO_GRIPPER_STATE_CLOSED;
+ this->gripIface->data->left_paddle_open = 0;
+ this->gripIface->data->right_paddle_open = 0;
}
else if (fabs(this->joints[LEFT]->GetPosition() -
this->joints[LEFT]->GetLowStop()) < 0.01 &&
fabs(this->joints[RIGHT]->GetPosition() -
this->joints[RIGHT]->GetHighStop()) < 0.01)
{
- this->myIface->state = GAZEBO_GRIPPER_OPEN;
- this->myIface->left_paddle_open = 1;
- this->myIface->right_paddle_open = 1;
+ this->gripIface->data->state = GAZEBO_GRIPPER_STATE_OPEN;
+ this->gripIface->data->left_paddle_open = 1;
+ this->gripIface->data->right_paddle_open = 1;
}
else
{
- this->myIface->state = GAZEBO_GRIPPER_MOVING;
- this->myIface->left_paddle_open = 0;
- this->myIface->right_paddle_open = 0;
+ this->gripIface->data->state = GAZEBO_GRIPPER_STATE_MOVING;
+ this->gripIface->data->left_paddle_open = 0;
+ this->gripIface->data->right_paddle_open = 0;
}
- this->myIface->Unlock();
+ // Set the state of the lift
+ if (fabs(this->joints[LIFT]->GetPosition() -
+ this->joints[LIFT]->GetHighStop()) < 0.01)
+ this->actIface->data->actuators[0].position = 1;
+ else if (fabs(this->joints[LIFT]->GetPosition() -
+ this->joints[LIFT]->GetLowStop()) < 0.01)
+ this->actIface->data->actuators[0].position = 0;
+
+ this->actIface->data->actuators_count = 1;
+ this->gripIface->Unlock();
}
////////////////////////////////////////////////////////////////////////////////
Modified:
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
===================================================================
---
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
2009-03-18 03:00:08 UTC (rev 7529)
+++
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
2009-03-18 17:29:30 UTC (rev 7530)
@@ -36,6 +36,7 @@
{
class SliderJoint;
class GripperIface;
+ class ActarrayIface;
/// \addtogroup gazebo_controller
/// \{
@@ -87,9 +88,12 @@
/// \return 0 on success
protected: virtual void FiniChild();
- /// The Position interface
- private: GripperIface *myIface;
+ /// The gripper interface
+ private: GripperIface *gripIface;
+ // The interface for the lift
+ private: ActarrayIface *actIface;
+
/// The parent Model
private: Model *myParent;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Apps built with the Adobe(R) Flex(R) framework and Flex Builder(TM) are
powering Web 2.0 with engaging, cross-platform capabilities. Quickly and
easily build your RIAs with Flex Builder, the Eclipse(TM)based development
software that enables intelligent coding and step-through debugging.
Download the free 60 day trial. http://p.sf.net/sfu/www-adobe-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
