1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18
19 #include "PID.hpp"
20
21 #include "FirstOrderLinearTIDS.hpp"
22 #include "SiconosVector.hpp"
23 #include "ActuatorFactory.hpp"
24 #include "TimeDiscretisation.hpp"
25 #include "ControlSensor.hpp"
26 #include "SimpleMatrix.hpp"
27 #include "Simulation.hpp"
28 #include "EventsManager.hpp"
29
30 #include <iostream>
31 // #define DEBUG_NOCOLOR
32 // #define DEBUG_STDOUT
33 // #define DEBUG_MESSAGES
34 //#define DEBUG_WHERE_MESSAGES
35 #include "siconos_debug.h"
36
37
PID(SP::ControlSensor sensor,SP::SimpleMatrix B)38 PID::PID(SP::ControlSensor sensor, SP::SimpleMatrix B): Actuator(PID_, sensor, B), _ref(0), _curDeltaT(0)
39 {
40 _u.reset(new SiconosVector(1, 0));
41 }
42
~PID()43 PID::~PID()
44 {
45 }
46
initialize(const NonSmoothDynamicalSystem & nsds,const Simulation & s)47 void PID::initialize(const NonSmoothDynamicalSystem& nsds, const Simulation& s)
48 {
49 Actuator::initialize(nsds,s);
50
51 _curDeltaT = s.currentTimeStep();
52
53 // initialize _err
54 _err.reset(new boost::circular_buffer<double> (3));
55 for(unsigned int i = 0; i < 3; ++i)
56 (*_err).push_front(0.0);
57 }
58
actuate()59 void PID::actuate()
60 {
61 DEBUG_BEGIN("void PID::actuate()\n");
62 /** \todo We have to distinguish two cases : linear or nonlinear
63 * support the nonlinear case
64 */
65
66 // Compute the new error
67
68 (*_err).push_front(_ref - _sensor->y()(0));
69 DEBUG_PRINTF("_curDeltaT = %g\n", _curDeltaT);
70 DEBUG_PRINTF("_ref = %g\n", _ref);
71 DEBUG_EXPR(_sensor->y().display(););
72 DEBUG_EXPR(_u->display(););
73 DEBUG_PRINTF("added term = %g\n", ((*_K)(0) + (*_K)(2) / _curDeltaT + (*_K)(1) * _curDeltaT) * (*_err)[0] +
74 (-(*_K)(0) - 2 * (*_K)(2) / _curDeltaT) * (*_err)[1] + (*_K)(2) / _curDeltaT * (*_err)[2]);
75 // compute the new control and update it
76 (*_u)(0) += ((*_K)(0) + (*_K)(2) / _curDeltaT + (*_K)(1) * _curDeltaT) * (*_err)[0] +
77 (-(*_K)(0) - 2 * (*_K)(2) / _curDeltaT) * (*_err)[1] + (*_K)(2) / _curDeltaT * (*_err)[2];
78 DEBUG_EXPR(_u->display(););
79 DEBUG_END("void PID::actuate()\n");
80 }
81
setK(SP::SiconosVector K)82 void PID::setK(SP::SiconosVector K)
83 {
84 // check dimensions ...
85 if(K->size() != 3)
86 {
87 THROW_EXCEPTION("PID::setK - the size of K should be 3");
88 }
89 else
90 {
91 _K = K;
92 }
93 }
94
setTimeDiscretisation(const TimeDiscretisation & td)95 void PID::setTimeDiscretisation(const TimeDiscretisation& td)
96 {
97 }
98
display() const99 void PID::display() const
100 {
101 Actuator::display();
102 std::cout << "current error vector: ";
103 std::cout << (*_err)[0] << " " << (*_err)[1] << " " << (*_err)[2] << std::endl;
104 }
105 AUTO_REGISTER_ACTUATOR(PID_, PID)
106