Are there any pathfinding implementations (dijkstra, A*, etc) for ARGoS or shall I write my own?
Any code I'll produce I'll put on Github/BitBucket when I've completed my project.
Nick
Code: Select all
void CSingleRobot::SetWheelSpeeds(const CCI_PositioningSensor::SReading& s_me_inworld,
const CVector2& c_target_inworld) {
// Get my rotation in the world
CRadians cZAngle, cYAngle, cXAngle;
s_me_inworld.Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
// Get position of target wrt robot
CVector2 cMeToTarget(
c_target_inworld.GetX() - s_me_inworld.Position.GetX(),
c_target_inworld.GetY() - s_me_inworld.Position.GetY());
cMeToTarget.Rotate(-cZAngle);
// Get direction angle
CRadians cAngle = cMeToTarget.Angle();
// Set wheel speed depending on angle
if(m_cGoStraightAngleRange.WithinMinBoundIncludedMaxBoundIncluded(cAngle)) {
// straight
m_pcWheels->SetLinearVelocity(m_fWheelVelocity, m_fWheelVelocity);
} else {
if(cAngle.GetValue() >= 0.0) {
// go left
m_pcWheels->SetLinearVelocity(0.0, m_fWheelVelocity);
} else {
// go right
m_pcWheels->SetLinearVelocity(m_fWheelVelocity, 0.0);
}
}
}