void CBTFootbotRecruiterRootBehavior::GoToFood(){
CVector2 tmp = m_pcObstacleAvoidance->GetVector();
tmp += m_pcOdometry->GetReversedLocationVector();
tmp.Normalize();
GoToVector(tmp);
m_pcOdometry->Step(*c_robot_state);
}
CVector2 CFootBotFlocking::FlockingVector() {
/* Get the camera readings */
const CCI_ColoredBlobOmnidirectionalCameraSensor::SReadings& sReadings = m_pcCamera->GetReadings();
/* Go through the camera readings to calculate the flocking interaction vector */
if(! sReadings.BlobList.empty()) {
CVector2 cAccum;
Real fLJ;
size_t unBlobsSeen = 0;
for(size_t i = 0; i < sReadings.BlobList.size(); ++i) {
/*
* The camera perceives the light as a yellow blob
* The robots have their red beacon on
* So, consider only red blobs
* In addition: consider only the closest neighbors, to avoid
* attraction to the farthest ones. Taking 180% of the target
* distance is a good rule of thumb.
*/
if(sReadings.BlobList[i]->Color == CColor::RED &&
sReadings.BlobList[i]->Distance < m_sFlockingParams.TargetDistance * 1.80f) {
/*
* Take the blob distance and angle
* With the distance, calculate the Lennard-Jones interaction force
* Form a 2D vector with the interaction force and the angle
* Sum such vector to the accumulator
*/
/* Calculate LJ */
fLJ = m_sFlockingParams.GeneralizedLennardJones(sReadings.BlobList[i]->Distance);
/* Sum to accumulator */
cAccum += CVector2(fLJ,
sReadings.BlobList[i]->Angle);
/* Increment the blobs seen counter */
++unBlobsSeen;
}
}
/* Divide the accumulator by the number of blobs seen */
cAccum /= unBlobsSeen;
/* Clamp the length of the vector to the max speed */
if(cAccum.Length() > m_sWheelTurningParams.MaxSpeed) {
cAccum.Normalize();
cAccum *= m_sWheelTurningParams.MaxSpeed;
}
return cAccum;
}
else {
return CVector2();
}
}
CVector2 CFootBotFlocking::VectorToLight() {
/* Get light readings */
const CCI_FootBotLightSensor::TReadings& tReadings = m_pcLight->GetReadings();
/* Calculate a normalized vector that points to the closest light */
CVector2 cAccum;
for(size_t i = 0; i < tReadings.size(); ++i) {
cAccum += CVector2(tReadings[i].Value, tReadings[i].Angle);
}
if(cAccum.Length() > 0.0f) {
/* Make the vector long as 1/4 of the max speed */
cAccum.Normalize();
cAccum *= 0.25f * m_sWheelTurningParams.MaxSpeed;
}
return cAccum;
}
CVector2 CFootBotForaging::DiffusionVector(bool& b_collision) {
/* Get readings from proximity sensor */
const CCI_FootBotProximitySensor::TReadings& tProxReads = m_pcProximity->GetReadings();
/* Sum them together */
CVector2 cDiffusionVector;
for(size_t i = 0; i < tProxReads.size(); ++i) {
cDiffusionVector += CVector2(tProxReads[i].Value, tProxReads[i].Angle);
}
/* If the angle of the vector is small enough and the closest obstacle
is far enough, ignore the vector and go straight, otherwise return
it */
if(m_sDiffusionParams.GoStraightAngleRange.WithinMinBoundIncludedMaxBoundIncluded(cDiffusionVector.Angle()) &&
cDiffusionVector.Length() < m_sDiffusionParams.Delta ) {
b_collision = false;
return CVector2::X;
}
else {
b_collision = true;
cDiffusionVector.Normalize();
return -cDiffusionVector;
}
}