控制工具PID

前一篇：

pressbooks.online.ucf.edu/phy2048tjb/chapter/15-5-damped-oscillations/

byjus.com/jee/damped-oscillation/

阻尼振荡器原理上面两个链接，程序下面一个链接。

​​​​​​控制工具基础案例

如何控制呢？PID？ 

看的如何很懵，正常，后续会把这些点串起来。

  

   

    

     

    
    

     

      /**********************************************************************************************************************
     
    
   

    

     

    
    

     

      This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
     
    
   

    

     

    
    

     

      Licensed under the BSD-2 license (see LICENSE file in main directory)
     
    
   

    

     

    
    

     

       **********************************************************************************************************************/
     
    
   

    

     

    
    

     

      #pragma once
     
    
   

    

     

    
    

     

      class CustomController : public ct::core::Controller<2, 1>
     
    
   

    

     

    
    

     

      {
     
    
   

    

     

    
    

     

      public:
     
    
   

    

     

    
    

     
    static const size_t state_dim = 2;    // two states
     
    
   

    

     

    
    

     
    static const size_t control_dim = 1;  // one control action
     
    
   

    

     

    
    

     
    CustomController(const ct::core::ControlVector<control_dim>& uff,  // feedforward control
     
    
   

    

     

    
    

     
        const double& kp,                                              // P gain
     
    
   

    

     

    
    

     
        const double& kd                                               // D gain
     
    
   

    

     

    
    

     

              )
     
    
   

    

     

    
    

     

              : uff_(uff), kp_(kp), kd_(kd)
     
    
   

    

     

    
    

     

          {
     
    
   

    

     

    
    

     

          }
     
    
   

    

     

    
    

     

          ~CustomController() {}
     
    
   

    

     

    
    

     
    CustomController(const CustomController& other) : uff_(other.uff_), kp_(other.kp_), kd_(other.kd_) {}
     
    
   

    

     

    
    

     
    CustomController* clone() const override
     
    
   

    

     

    
    

     

          {
     
    
   

    

     

    
    

     
        return new CustomController(*this);  // calls copy constructor
     
    
   

    

     

    
    

     

          }
     
    
   

    

     

    
    

     
    void computeControl(const ct::core::StateVector<state_dim>& state,
     
    
   

    

     

    
    

     

       const double& t,
     
    
   

    

     

    
    

     

       ct::core::ControlVector<control_dim>& controlAction) override
     
    
   

    

     

    
    

     

          {
     
    
   

    

     

    
    

     

              controlAction = uff_;                                 // apply feedforward control
     
    
   

    

     

    
    

     
        controlAction(0) -= kp_ * state(0) + kd_ * state(1);  // add feedback control
     
    
   

    

     

    
    

     

          }
     
    
   

    

     

    
    

     

      private:
     
    
   

    

     

    
    

     

          ct::core::ControlVector<control_dim> uff_;  
     
    
   

    

     

    
    

     
    double kp_;                                 
     
    
   

    

     

    
    

     
    double kd_;                                 
     
    
   

    

     

    
    

     

      };
     
    
  
 

  

   

    

     

    
    

     

      /**********************************************************************************************************************
     
    
   

    

     

    
    

     

      This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
     
    
   

    

     

    
    

     

      Licensed under the BSD-2 license (see LICENSE file in main directory)
     
    
   

    

     

    
    

     

      **********************************************************************************************************************/
     
    
   

    

     

    
    

     

      #include <ct/core/core.h>
     
    
   

    

     

    
    

     

      #include <ct/core/examples/CustomController.h>
     
    
   

    

     

    
    

     

      int main(int argc, char** argv)
     
    
   

    

     

    
    

     

      {
     
    
   

    

     

    
    

     
    // a damped oscillator has two states, position and velocity
     
    
   

    

     

    
    

     
    const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;      // = 2
     
    
   

    

     

    
    

     
    const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;  // = 1
     
    
   

    

     

    
    

     
    // create a state
     
    
   

    

     

    
    

     

          ct::core::StateVector<state_dim> x;
     
    
   

    

     

    
    

     
    // we initialize it at a point with unit deflection and zero velocity
     
    
   

    

     

    
    

     
    x(0) = 1.0;
     
    
   

    

     

    
    

     
    x(1) = 0.0;
     
    
   

    

     

    
    

     
    // create our oscillator
     
    
   

    

     

    
    

     
    double w_n = 50;
     
    
   

    

     

    
    

     
    std::shared_ptr<ct::core::SecondOrderSystem> oscillator(new ct::core::SecondOrderSystem(w_n));
     
    
   

    

     

    
    

     
    // create our controller
     
    
   

    

     

    
    

     
    double kp = 10;
     
    
   

    

     

    
    

     
    double kd = 1;
     
    
   

    

     

    
    

     

          ct::core::ControlVector<control_dim> uff;
     
    
   

    

     

    
    

     

          uff << 2.0;
     
    
   

    

     

    
    

     
    std::shared_ptr<CustomController> controller(new CustomController(uff, kp, kd));
     
    
   

    

     

    
    

     
    // assign our controller
     
    
   

    

     

    
    

     

          oscillator->setController(controller);
     
    
   

    

     

    
    

     
    // create an integrator
     
    
   

    

     

    
    

     

          ct::core::Integrator<state_dim> integrator(oscillator, ct::core::IntegrationType::RK4);
     
    
   

    

     

    
    

     
    // simulate 1000 steps
     
    
   

    

     

    
    

     
    double dt = 0.001;
     
    
   

    

     

    
    

     

          ct::core::Time t0 = 0.0;
     
    
   

    

     

    
    

     
    size_t nSteps = 1000;
     
    
   

    

     

    
    

     

          integrator.integrate_n_steps(x, t0, nSteps, dt);
     
    
   

    

     

    
    

     
    // print the new state
     
    
   

    

     

    
    

     

          std::cout << "state after integration: " << x.transpose() << std::endl;
     
    
   

    

     

    
    

     
    return 0;
     
    
   

    

     

    
    

     

      }
     
    
  
 

文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。

原文链接：zhangrelay.blog.csdn.net/article/details/123487993