void Robot::lookAtBehind(const Math::Angle &angle) { float targetAngle; if (angle.rad() > 0.0f) targetAngle = angle.rad() - Math::PI; else targetAngle = angle.rad() + Math::PI; lookAt(Math::Rad(targetAngle)); }
void Robot::lookAt(const Math::Angle &angle) { setTargetOmega(Math::limit(angle.rad() * Config::lookAtP, Math::degToRad(Config::lookAtMaxSpeedAngle) * Config::lookAtP)); }
void Robot::setTargetDir(const Math::Angle &dir, float speed, float omega) { Math::Vector dirVector = Math::Vector::createForwardVec(dir.rad(), speed); setTargetDir(dirVector.x, dirVector.y, omega); }