.. _program_listing_file_src_tap_algorithms_fuzzy_pd.cpp:

Program Listing for File fuzzy_pd.cpp
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_tap_algorithms_fuzzy_pd.cpp>` (``src/tap/algorithms/fuzzy_pd.cpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington <robomstr@uw.edu>
    *
    * This file is part of Taproot.
    *
    * Taproot is free software: you can redistribute it and/or modify
    * it under the terms of the GNU General Public License as published by
    * the Free Software Foundation, either version 3 of the License, or
    * (at your option) any later version.
    *
    * Taproot is distributed in the hope that it will be useful,
    * but WITHOUT ANY WARRANTY; without even the implied warranty of
    * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    * GNU General Public License for more details.
    *
    * You should have received a copy of the GNU General Public License
    * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
    */
   
   #include "fuzzy_pd.hpp"
   
   #include "tap/algorithms/math_user_utils.hpp"
   
   using namespace tap::algorithms;
   
   namespace tap::algorithms
   {
   FuzzyPD::FuzzyPD(const FuzzyPDConfig &pidConfig, const SmoothPidConfig &smoothPidConfig)
       : SmoothPid(smoothPidConfig),
         config(pidConfig)
   {
   }
   
   float FuzzyPD::runController(float error, float errorDerivative, float dt)
   {
       udpatePidGains(
           limitVal(error / config.maxError, -1.0f, 1.0f),
           limitVal(errorDerivative / config.maxErrorDerivative, -1.0f, 1.0f));
       return SmoothPid::runController(error, errorDerivative, dt);
   }
   
   void FuzzyPD::udpatePidGains(float error, float errorDerivative)
   {
       config.fuzzyTable.performFuzzyUpdate(error, errorDerivative);
   
       setP(config.fuzzyTable.getFuzzyGains()[0][0]);
       setD(config.fuzzyTable.getFuzzyGains()[1][0]);
   }
   
   }  // namespace tap::algorithms