diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp index bc5b45f8ed2fba457d47079d8f7f2667619edd65..9f48adbc37bfc1359848496f060902ec10d1f6f2 100644 --- a/src/shared/algorithms/Follower/Follower.cpp +++ b/src/shared/algorithms/Follower/Follower.cpp @@ -115,6 +115,18 @@ AntennaAngles Follower::getTargetAngles() return targetAngles; } +bool Follower::setMaxGain(float yawGainNew, float pitchGainNew) +{ + // In case of negative or over the limit values, do not set the gains + if (yawGainNew < 0 || yawGainNew > YAW_GAIN_LIMIT || pitchGainNew < 0 || + pitchGainNew > PITCH_GAIN_LIMIT) + return false; + + yawGain = yawGainNew; + pitchGain = pitchGainNew; + return true; +} + bool Follower::init() { if (isInit) @@ -162,8 +174,10 @@ void Follower::step() } // Rotate in the shortest direction - diffAngles.yaw = YAW_GAIN * minimizeRotation(diffAngles.yaw); - diffAngles.pitch = PITCH_GAIN * minimizeRotation(diffAngles.pitch); + diffAngles.yaw = + std::min(yawGain, YAW_GAIN_LIMIT) * minimizeRotation(diffAngles.yaw); + diffAngles.pitch = std::min(pitchGain, PITCH_GAIN_LIMIT) * + minimizeRotation(diffAngles.pitch); // Calculate angular velocity for moving the antennas toward position float horizontalSpeed = diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h index 850a63f90e8e1793d8c889d93aa07cf1c0813616..b161cf02ea979fc339310679cd79e311c710c8d9 100644 --- a/src/shared/algorithms/Follower/Follower.h +++ b/src/shared/algorithms/Follower/Follower.h @@ -38,8 +38,10 @@ namespace Boardcore { -static constexpr float YAW_GAIN = 0.1; -static constexpr float PITCH_GAIN = 1.0; +static constexpr float YAW_GAIN_LIMIT = + 1.0; ///< Max limit for the Yaw gain, cannot be set more +static constexpr float PITCH_GAIN_LIMIT = + 1.0; ///< Max limit for the pirch gain cannot be set more /** * @brief Follower class to output the yaw ad pitch necessary to track from the @@ -89,6 +91,16 @@ public: */ void setLastAntennaAttitude(const VN300Data& attitudeData); + /** + * @brief Set the maximum gain for the yaw and pitch + * + * @param yawGainNew the gain for the yaw + * @param pitchGainNew the gain for the pitch + * @return true if set correctly + * @return false if negative or over the maximum limits + */ + bool setMaxGain(float yawGainNew, float pitchGainNew); + /** * @brief Synchronized getter for the State of the follower algorithm. * @returns The state of the follower algorithm. @@ -181,6 +193,9 @@ private: // General mutex for the follower miosix::FastMutex followerMutex; + + float yawGain = YAW_GAIN_LIMIT; ///< Gain on the yaw + float pitchGain = PITCH_GAIN_LIMIT; ///< Gain on the pitch }; } // namespace Boardcore