bool Mbfo::isRoadClear(const CVector2& obstacleProximity) {
CDegrees obstacleAngle(ToDegrees(obstacleProximity.Angle()));
CDegrees safeAngle(150.0f);
return (minAngleFromObstacle.WithinMinBoundIncludedMaxBoundIncluded(obstacleAngle)
&& obstacleProximity.Length() < minDistanceFromObstacle) ||
(obstacleAngle < -safeAngle || obstacleAngle > safeAngle);
}
void CFootBotUN::updateDesideredVelocity() {
CVector2 agentToTarget = targetPosition - position;
CRadians a0 = agentToTarget.Angle() - angle;
DEBUG_CONTROLLER("updateDesideredVelocity to %.2f, state = %d \r\n",a0.GetValue(),state);
agent->targetPosition = targetPosition;
agent->updateDesideredVelocity();
printf("=> desidered speed %.2f, desidered angle %.2f \n", agent->desideredSpeed, agent->desideredAngle.GetValue());
}
CVector2 CFootBotForaging::CalculateVectorToLight() {
/* Get readings from light sensor */
const CCI_FootBotLightSensor::TReadings& tLightReads = m_pcLight->GetReadings();
/* Sum them together */
CVector2 cAccumulator;
for(size_t i = 0; i < tLightReads.size(); ++i) {
cAccumulator += CVector2(tLightReads[i].Value, tLightReads[i].Angle);
}
/* If the light was perceived, return the vector */
if(cAccumulator.Length() > 0.0f) {
return CVector2(1.0f, cAccumulator.Angle());
}
/* Otherwise, return zero */
else {
return CVector2();
}
}
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;
}
}
void CBuzzControllerEFootBot::SetWheelSpeedsFromVector(const CVector2& c_heading) {
/* Get the heading angle */
CRadians cHeadingAngle = c_heading.Angle().SignedNormalize();
/* Get the length of the heading vector */
Real fHeadingLength = c_heading.Length();
/* Clamp the speed so that it's not greater than MaxSpeed */
Real fBaseAngularWheelSpeed = Min<Real>(fHeadingLength, m_sWheelTurningParams.MaxSpeed);
/* Turning state switching conditions */
if(Abs(cHeadingAngle) <= m_sWheelTurningParams.NoTurnAngleThreshold) {
/* No Turn, heading angle very small */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::NO_TURN;
}
else if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
/* Hard Turn, heading angle very large */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
}
else if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::NO_TURN &&
Abs(cHeadingAngle) > m_sWheelTurningParams.SoftTurnOnAngleThreshold) {
/* Soft Turn, heading angle in between the two cases */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
}
/* Wheel speeds based on current turning state */
Real fSpeed1, fSpeed2;
switch(m_sWheelTurningParams.TurningMechanism) {
case SWheelTurningParams::NO_TURN: {
/* Just go straight */
fSpeed1 = fBaseAngularWheelSpeed;
fSpeed2 = fBaseAngularWheelSpeed;
break;
}
case SWheelTurningParams::SOFT_TURN: {
/* Both wheels go straight, but one is faster than the other */
Real fSpeedFactor = (m_sWheelTurningParams.HardTurnOnAngleThreshold - Abs(cHeadingAngle)) / m_sWheelTurningParams.HardTurnOnAngleThreshold;
fSpeed1 = fBaseAngularWheelSpeed - fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
fSpeed2 = fBaseAngularWheelSpeed + fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
break;
}
case SWheelTurningParams::HARD_TURN: {
/* Opposite wheel speeds */
fSpeed1 = -m_sWheelTurningParams.MaxSpeed;
fSpeed2 = m_sWheelTurningParams.MaxSpeed;
break;
}
}
/* Apply the calculated speeds to the appropriate wheels */
Real fLeftWheelSpeed, fRightWheelSpeed;
if(cHeadingAngle > CRadians::ZERO) {
/* Turn Left */
fLeftWheelSpeed = fSpeed1;
fRightWheelSpeed = fSpeed2;
}
else {
/* Turn Right */
fLeftWheelSpeed = fSpeed2;
fRightWheelSpeed = fSpeed1;
}
/* Finally, set the wheel speeds */
m_pcWheels->SetLinearVelocity(fLeftWheelSpeed, fRightWheelSpeed);
}
void Mbfo::avoidObstacle(const CVector2& obstacleProximity) {
auto obstacleAngle = ToDegrees(obstacleProximity.Angle());
auto rotationDirection = getRotationDirection(obstacleAngle);
rotate(rotationDirection);
}