ARGoS robot turning function

gistfile1.cpp

void CSingleRobot::SetWheelSpeeds(const CCI_PositioningSensor::SReading& sPosReading) {

  //LOG << "\n";
  /* Get positioning reading */

  CRadians cZAngle, cYAngle, cXAngle;
    sPosReading.Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);

  // Yaw
  //LOG << "Yaw: " << cZAngle << "\n";

  m_cRobotDirection.Set(
    Cos(cZAngle),
    Sin(cZAngle)
  );

  m_cDesiredDirection.Set(
    (m_sCurrentGoal.x - sPosReading.Position.GetX()),
    (m_sCurrentGoal.y - sPosReading.Position.GetY())
  );
  m_cDesiredDirection.Normalize();

  Real dot = (m_cRobotDirection.GetX() * m_cDesiredDirection.GetX()) +
              (m_cRobotDirection.GetY() * m_cDesiredDirection.GetY());

  CRadians cAngle = (ATan2(m_cRobotDirection.GetY(), m_cRobotDirection.GetX())) - 
                (ATan2(m_cDesiredDirection.GetY(), m_cDesiredDirection.GetX()));

  LOG << "Angle: " << cAngle << std::endl;

  if(m_cGoStraightAngleRange.WithinMinBoundIncludedMaxBoundIncluded(cAngle)) {
  //if(check_range(cAngle.GetValue(), , 0.2)) {
      // straight
      m_pcWheels->SetLinearVelocity(m_fWheelVelocity, m_fWheelVelocity);
  } else {
    if(cAngle.GetValue() >= 0.0f) {
    //if(dot < 0.0f) {
      // right
      m_pcWheels->SetLinearVelocity(m_fWheelVelocity, 0.0f);
    } else {
      // left
      m_pcWheels->SetLinearVelocity(0.0f, m_fWheelVelocity);
    }
  }

}