//----------------------------------------------------------------------------------------------------------------------
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();
}
///Initialize motor params
foreach( Servo *s, motors )
{
Servo::TMotorData &data = s->data;
RoboCompJointMotor::MotorParams ¶ms = 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);
}