示例#1
0
//----------------------------------------------------------------------------------------------------------------------
void SMCAgent::init()
{
  m_radius = 0.02;
  
  if (SETTINGS->hasChild("Config/GA/Evolvable"))
  {
    const ci::XmlTree& xml = SETTINGS->getChild("Config/GA/Evolvable");
    
    // Todo: move to respective sensor class!!!
    // Distance sensor
    if (xml.getChild("DistanceSensor").getValue<bool>(0))
    {
      m_distanceSensor = new DistanceSensor();
      m_distanceSensor->fromXml(xml.getChild("DistanceSensor"));
      setSensorMode(xml.getChild("DistanceSensor").getAttributeValue<std::string>("Mode", "Absolute"));
    }
    else
    {
      m_distanceSensor = NULL;
    }
    
    // Gradient sensor
    if (xml.getChild("GradientSensor").getValue<bool>(0))
    {
      m_gradientSensor = new GradientSensor();
      m_gradientSensor->fromXml(xml.getChild("GradientSensor"));
    }
    else
    {
      m_gradientSensor = NULL;
    }
    
    // Torus sensor
    if (xml.getChild("TorusSensor").getValue<bool>(0))
    {
      m_torusSensor = new TorusSensor();
      m_torusSensor->fromXml(xml.getChild("TorusSensor"));
    }
    else
    {
      m_torusSensor = NULL;
    }

    
    setMaxSpeed(xml.getChild("MaxSpeed").getValue<double>(1.0));
    setMaxAngularSpeed(degreesToRadians(xml.getChild("MaxAngularSpeed").getValue<double>(180)));
    setMaxAngle(degreesToRadians(xml.getChild("MaxAngle").getValue<double>(90)));
    setMaxPosition(xml.getChild("MaxPosition").getValue<double>(0.5));
    setAngleWraps(xml.getChild("AngleWraps").getValue<bool>(1));
    setPositionWraps(xml.getChild("PositionWraps").getValue<bool>(1));
    m_energyInitial = xml.getChild("Energy").getAttributeValue<float>("initial", 30.0);
    m_energySpeedTresh = xml.getChild("Energy").getAttributeValue<float>("threshForSpeed", -1);
    
    m_evMin = xml.getChild("Energy").getAttributeValue<float>("evMin", 0);
    m_evMax = xml.getChild("Energy").getAttributeValue<float>("evMax", 10);
    
    m_engReplFoodSens = xml.getChild("Energy").getAttributeValue<bool>("engReplFoodSens", false);
    
    getEnvironment().fromXml(xml.getChild("Environment"));
  }
  
  reset();
}
示例#2
0
  ///Initialize motor params
  foreach( Servo *s, motors )
  {
	Servo::TMotorData &data = s->data;
	RoboCompJointMotor::MotorParams &params = s->params;
	
	std::cout << "JointMotor::Dynamixel::Dynamixel - Configuration data of motor " << params.name << std::endl;
	///Set Status return level to 1. Level 0: no response ; Level 1: only for reading commands. Default ; Level 2: always
	int level = 1;
	setStatusReturnLevel(params.busId, level);
	getStatusReturnLevel(params.busId, level);
	qDebug() << "	Status return level: " << level;
	
	///Return delay time
	int rt = 50;
	if (setReturnDelayTime( params.busId, rt) == true and getReturnDelayTime( params.busId, rt) == true)
	{
	  qDebug() << "	Return delay time: " << rt;
	}
	else
	  qDebug() << "Error setting delay time";
	
	///Control params
	int m;
	if (getPunch( params.busId, m ) == true)
	{
	  qDebug() << "	Punch: " << m;
	}
	else
	  qDebug() << "Error reading Punch";
	
	if (getCCWComplianceMargin( params.busId, m ) == true)
	{
	  qDebug() << "	CCWComplianceMargin: " << m;
	}
	else
	  qDebug() << "Error reading CCWComplianceMargin";  
	
	if (getCWComplianceMargin( params.busId, m ) == true)
	{
	  qDebug() << "	CWComplianceMargin: " << m;
	}
	else
	  qDebug() << "Error reading CWComplianceMargin";  
	if (getCCWComplianceSlope( params.busId, m ) == true)
	{
	  qDebug() << "	CCWComplianceSlope: " << m;
	}
	else
	  qDebug() << "Error reading CCWComplianceSlope";  
	
	if (getCWComplianceSlope( params.busId, m ) == true)
	{
	  qDebug() << "	CWComplianceSlope: " << m;
	}
	else
	  qDebug() << "Error reading CWComplianceSlope";  
	
	
	///Read current position
	int p;
	getPosition(params.busId, p );
	data.currentPos  = p;
	data.antPos = p;
	data.currentPosRads = s->steps2Rads(p);
	qDebug() << "	Current position (steps): " << data.currentPos;
	qDebug() << "	Current position (rads): " << data.currentPosRads;
	
	///Set limits
	setMaxPosition(params.busId, MAXSTEPSPOSITION);
	setMinPosition(params.busId, MINSTEPSPOSITION);	
	
	getMaxPosition( params.busId, p);
	qDebug() << "	Max position (steps): " << params.maxPos <<  p;
	getMinPosition( params.busId, p);
	qDebug() << "	Min position (steps): " << params.minPos << p;
	
	///set servos to maximum speed
	setVelocity( params.busId, params.maxVelocity);
	
	qDebug() << "	Max velocity " << params.maxVelocity;
	
 	setBothComplianceMargins(params.busId, 2);	
 	setBothComplianceSlopes(params.busId, 120);
//         setPunch(params.busId, 50);

  }