source: flair-src/trunk/lib/FlairFilter/src/Pid_impl.cpp@ 137

Last change on this file since 137 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

File size: 3.2 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5// created: 2011/05/01
6// filename: Pid_impl.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: Class defining a PID
14//
15//
16/*********************************************************************/
17#include "Pid_impl.h"
18#include "Pid.h"
19#include <cvmatrix.h>
20#include <Layout.h>
21#include <GroupBox.h>
22#include <DoubleSpinBox.h>
23#include <DataPlot1D.h>
24
25using std::string;
26using namespace flair::core;
27using namespace flair::gui;
28using namespace flair::filter;
29
30Pid_impl::Pid_impl(Pid *self, const LayoutPosition *position, string name) {
31 i = 0;
32 first_update = true;
33 this->self = self;
34
35 // init matrix
36 self->input = new cvmatrix(self, 2, 1, floatType, name);
37
38 cvmatrix_descriptor *desc = new cvmatrix_descriptor(4, 1);
39 desc->SetElementName(0, 0, "p");
40 desc->SetElementName(1, 0, "i");
41 desc->SetElementName(2, 0, "d");
42 desc->SetElementName(3, 0, "p+i+d");
43 state = new cvmatrix(self, desc, floatType, name);
44
45 GroupBox *reglages_groupbox = new GroupBox(position, name);
46 T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
47 0, 1, 0.01);
48 kp = new DoubleSpinBox(reglages_groupbox->NewRow(), "kp:", 0, 90000000, 0.01,
49 3);
50 ki = new DoubleSpinBox(reglages_groupbox->NewRow(), "ki:", 0, 90000000, 0.01,
51 3);
52 sati = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "sat i:", 0, 1,
53 0.01);
54 kd = new DoubleSpinBox(reglages_groupbox->NewRow(), "kd:", 0, 90000000, 0.01,
55 3);
56 sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat:", 0, 1, 0.1);
57}
58
59Pid_impl::~Pid_impl(void) {}
60
61void Pid_impl::UseDefaultPlot(const LayoutPosition *position) {
62 DataPlot1D *plot = new DataPlot1D(position, self->ObjectName(), -1, 1);
63 plot->AddCurve(state->Element(0));
64 plot->AddCurve(state->Element(1), DataPlot::Green);
65 plot->AddCurve(state->Element(2), DataPlot::Blue);
66 plot->AddCurve(state->Element(3), DataPlot::Black);
67}
68
69void Pid_impl::UpdateFrom(const io_data *data) {
70 float p, d, total;
71 float delta_t;
72 cvmatrix *input = (cvmatrix *)data;
73
74 if (T->Value() == 0) {
75 delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
76 } else {
77 delta_t = T->Value();
78 }
79 if (first_update == true) {
80 delta_t = 0;
81 first_update = false;
82 }
83
84 input->GetMutex();
85 p = kp->Value() * input->ValueNoMutex(0, 0);
86 i += ki->Value() * input->ValueNoMutex(0, 0) * delta_t;
87 if (i > sati->Value())
88 i = sati->Value();
89 if (i < -sati->Value())
90 i = -sati->Value();
91 d = kd->Value() * input->ValueNoMutex(1, 0);
92 input->ReleaseMutex();
93
94 total = p + i + d;
95 if (total > sat->Value())
96 total = sat->Value();
97 if (total < -sat->Value())
98 total = -sat->Value();
99
100 state->GetMutex();
101 state->SetValueNoMutex(0, 0, p);
102 state->SetValueNoMutex(1, 0, i);
103 state->SetValueNoMutex(2, 0, d);
104 state->SetValueNoMutex(3, 0, total);
105 state->ReleaseMutex();
106
107 self->output->SetValue(0, 0, total);
108 self->output->SetDataTime(data->DataTime());
109
110 previous_time = data->DataTime();
111}
Note: See TracBrowser for help on using the repository browser.