source: flair-src/trunk/lib/FlairFilter/src/NestedSat_impl.cpp @ 157

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

sources reformatted with flair-format-dir script

File size: 2.1 KB
RevLine 
[10]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[10]4// %flair:license}
[7]5//  created:    2013/04/15
6//  filename:   NestedSat_impl.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    objet permettant le calcul d'un Pid avec saturations
14//
15//
16/*********************************************************************/
17
18#include "NestedSat_impl.h"
19#include "NestedSat.h"
20#include <cvmatrix.h>
21#include <Layout.h>
22#include <GroupBox.h>
23#include <DoubleSpinBox.h>
24
25using std::string;
26using namespace flair::core;
27using namespace flair::gui;
28using namespace flair::filter;
29
[15]30NestedSat_impl::NestedSat_impl(NestedSat *self, const LayoutPosition *position,
31                               string name) {
32  this->self = self;
33  k = 1;
[7]34
[15]35  // init matrix
36  self->input = new cvmatrix(self, 3, 1, floatType, name);
[7]37
[15]38  GroupBox *reglages_groupbox = new GroupBox(position, name);
39  sat =
40      new DoubleSpinBox(reglages_groupbox->NewRow(), "sat ref:", 0, 9000000, 1);
41  kp = new DoubleSpinBox(reglages_groupbox->NewRow(), "kp:", 0, 9000000, 1);
42  dsat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat dref:", 0, 9000000,
43                           1);
44  kd = new DoubleSpinBox(reglages_groupbox->NewRow(), "kd:", 0, 9000000, 0.1);
45  usat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat u:", 0, 1, .1);
[7]46}
47
[15]48NestedSat_impl::~NestedSat_impl(void) {}
[7]49
[15]50void NestedSat_impl::UpdateFrom(const io_data *data) {
51  float cons, dcons, law;
52  cvmatrix *input = (cvmatrix *)data;
[7]53
[15]54  input->GetMutex();
[7]55
[15]56  cons = Sat(input->ValueNoMutex(0, 0), k * sat->Value());
57  dcons = (cons - input->ValueNoMutex(1, 0)) * kp->Value();
58  dcons = Sat(dcons, k * dsat->Value());
59  law = (input->ValueNoMutex(2, 0) - dcons) * kd->Value();
60  law = Sat(law, usat->Value());
[7]61
[15]62  input->ReleaseMutex();
[7]63
[15]64  self->output->SetValue(0, 0, law);
65  self->output->SetDataTime(data->DataTime());
[7]66}
67
[15]68float NestedSat_impl::Sat(float value, float borne) {
69  if (value < -borne)
70    return -borne;
71  if (value > borne)
72    return borne;
73  return value;
[7]74}
Note: See TracBrowser for help on using the repository browser.