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

Last change on this file was 318, checked in by Sanahuja Guillaume, 2 years ago
File size: 3.3 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 <Matrix.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 Matrix(self, 2, 1, floatType, name);
37
38  MatrixDescriptor *desc = new MatrixDescriptor(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 Matrix(self, desc, floatType, name);
44  delete desc;
45
46  GroupBox *reglages_groupbox = new GroupBox(position, name);
47  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
48                        0, 1, 0.01);
49  kp = new DoubleSpinBox(reglages_groupbox->NewRow(), "kp:", 0, 90000000, 0.01,
50                         3);
51  ki = new DoubleSpinBox(reglages_groupbox->NewRow(), "ki:", 0, 90000000, 0.01,
52                         3);
53  sati = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "sat i:", 0, 1,
54                           0.01);
55  kd = new DoubleSpinBox(reglages_groupbox->NewRow(), "kd:", 0, 90000000, 0.01,
56                         3);
57  sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat:", 0, 1, 0.1);
58}
59
60Pid_impl::~Pid_impl(void) {}
61
62void Pid_impl::UseDefaultPlot(const LayoutPosition *position) {
63  DataPlot1D *plot = new DataPlot1D(position, self->ObjectName(), -1, 1);
64  plot->AddCurve(state->Element(0));
65  plot->AddCurve(state->Element(1), DataPlot::Green);
66  plot->AddCurve(state->Element(2), DataPlot::Blue);
67  plot->AddCurve(state->Element(3), DataPlot::Black);
68}
69
70void Pid_impl::UpdateFrom(const io_data *data) {
71  float p, d, total;
72  float delta_t;
73  const Matrix* input = dynamic_cast<const Matrix*>(data);
74 
75  if (!input) {
76      self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
77      return;
78  }
79
80  if (T->Value() == 0) {
81    delta_t = (float)(data->DataDeltaTime() ) / 1000000000.;
82  } else {
83    delta_t = T->Value();
84  }
85  if (first_update == true) {
86    delta_t = 0;
87    first_update = false;
88  }
89
90  input->GetMutex();
91  p = kp->Value() * input->ValueNoMutex(0, 0);
92  i += ki->Value() * input->ValueNoMutex(0, 0) * delta_t;
93  if (i > sati->Value())
94    i = sati->Value();
95  if (i < -sati->Value())
96    i = -sati->Value();
97  d = kd->Value() * input->ValueNoMutex(1, 0);
98  input->ReleaseMutex();
99
100  total = p + i + d;
101  if (total > sat->Value())
102    total = sat->Value();
103  if (total < -sat->Value())
104    total = -sat->Value();
105
106  state->GetMutex();
107  state->SetValueNoMutex(0, 0, p);
108  state->SetValueNoMutex(1, 0, i);
109  state->SetValueNoMutex(2, 0, d);
110  state->SetValueNoMutex(3, 0, total);
111  state->ReleaseMutex();
112
113  self->output->SetValue(0, 0, total);
114  self->output->SetDataTime(data->DataTime());
115}
Note: See TracBrowser for help on using the repository browser.