1 |
|
|
/** |
2 |
|
|
* @file pid.cpp |
3 |
|
|
* @author Vincent Berenz |
4 |
|
|
* @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft, License BSD-3-Clause |
5 |
|
|
* @date 2019-12-09 |
6 |
|
|
*/ |
7 |
|
|
|
8 |
|
|
#include "ci_example_cpp/pid.hpp" |
9 |
|
|
|
10 |
|
|
namespace ci_example_cpp { |
11 |
|
|
|
12 |
|
|
PID::PID() |
13 |
|
|
: integral_(0) { |
14 |
|
|
configuration_ = new DefaultConfiguration(); |
15 |
|
|
private_configuration_ = true; |
16 |
|
|
} |
17 |
|
|
|
18 |
|
|
|
19 |
|
6 |
PID::PID(const Gains_configuration &configuration) |
20 |
|
6 |
:configuration_(&configuration),private_configuration_(false),integral_(0) {} |
21 |
|
|
|
22 |
|
|
|
23 |
|
12 |
PID::~PID(){ |
24 |
✗✓ |
6 |
if(private_configuration_){ |
25 |
|
|
delete configuration_; |
26 |
|
|
} |
27 |
|
6 |
} |
28 |
|
|
|
29 |
|
|
|
30 |
|
11 |
double PID::compute( const double position, |
31 |
|
|
const double velocity, |
32 |
|
|
const double position_target, |
33 |
|
|
const double delta_time ) { |
34 |
|
11 |
double position_error = position_target-position; |
35 |
|
11 |
integral_ += delta_time * position_error; |
36 |
|
11 |
double f = position_error*configuration_->get_kp() |
37 |
|
11 |
- velocity*configuration_->get_kd() |
38 |
|
11 |
+ integral_*configuration_->get_ki(); |
39 |
|
11 |
return f; |
40 |
|
|
} |
41 |
|
|
|
42 |
|
|
|
43 |
|
4 |
void PID::reset_integral(){ |
44 |
|
4 |
this->integral_=0; |
45 |
|
4 |
} |
46 |
|
|
|
47 |
|
|
|
48 |
|
|
/** @brief Use a PID factory for the unittests. */ |
49 |
|
|
class Default_pid_factory { |
50 |
|
|
|
51 |
|
|
public: |
52 |
|
|
/** The PID gains. */ |
53 |
|
|
static std::vector< std::shared_ptr<Gains_configuration> > configs_; |
54 |
|
|
/** List of PID controllers. */ |
55 |
|
|
static std::vector< std::shared_ptr<PID> > controllers_; |
56 |
|
|
/** |
57 |
|
|
* @brief PID controller factory. |
58 |
|
|
* |
59 |
|
|
* @return PID& Return a reference to a newly created PID controller. |
60 |
|
|
*/ |
61 |
|
4 |
static PID& get(){ |
62 |
✓✗✓✗
|
8 |
std::shared_ptr<Gains_configuration> configuration(new DefaultConfiguration()); |
63 |
✓✗✓✗
✓✗ |
8 |
std::shared_ptr<PID> controller(new PID(*configuration)); |
64 |
✓✗ |
4 |
configs_.push_back(configuration); |
65 |
✓✗ |
4 |
controllers_.push_back(controller); |
66 |
|
8 |
return *controller; |
67 |
|
|
} |
68 |
|
|
|
69 |
|
|
}; |
70 |
|
|
|
71 |
|
2 |
std::vector< std::shared_ptr<Gains_configuration> > Default_pid_factory::configs_; |
72 |
|
2 |
std::vector< std::shared_ptr<PID> > Default_pid_factory::controllers_; |
73 |
|
|
|
74 |
|
4 |
PID& get_default_pid(){ |
75 |
|
4 |
return Default_pid_factory::get(); |
76 |
|
|
} |
77 |
|
|
|
78 |
✓✗✓✗
|
6 |
} |