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

Last change on this file was 214, checked in by Sanahuja Guillaume, 4 years ago

matrix

File size: 2.2 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"
[214]20#include <Matrix.h>
[7]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
[214]36  self->input = new Matrix(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;
[214]52  const Matrix* input = dynamic_cast<const Matrix*>(data);
53 
54  if (!input) {
55      self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
56      return;
57  }
[7]58
[15]59  input->GetMutex();
[7]60
[15]61  cons = Sat(input->ValueNoMutex(0, 0), k * sat->Value());
62  dcons = (cons - input->ValueNoMutex(1, 0)) * kp->Value();
63  dcons = Sat(dcons, k * dsat->Value());
64  law = (input->ValueNoMutex(2, 0) - dcons) * kd->Value();
65  law = Sat(law, usat->Value());
[7]66
[15]67  input->ReleaseMutex();
[7]68
[15]69  self->output->SetValue(0, 0, law);
70  self->output->SetDataTime(data->DataTime());
[7]71}
72
[15]73float NestedSat_impl::Sat(float value, float borne) {
74  if (value < -borne)
75    return -borne;
76  if (value > borne)
77    return borne;
78  return value;
[7]79}
Note: See TracBrowser for help on using the repository browser.