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