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

Last change on this file since 10 was 10, checked in by Sanahuja Guillaume, 6 years ago

lic

File size: 2.0 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:    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
30NestedSat_impl::NestedSat_impl(NestedSat* self,const LayoutPosition* position,string name)
31{
32    this->self=self;
33        k=1;
34
35    //init matrix
36    self->input=new cvmatrix(self,3,1,floatType,name);
37
38    GroupBox* reglages_groupbox=new GroupBox(position,name);
39            sat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat ref:",0,9000000,1);
40            kp=new DoubleSpinBox(reglages_groupbox->NewRow(),"kp:",0,9000000,1);
41            dsat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat dref:",0,9000000,1);
42            kd=new DoubleSpinBox(reglages_groupbox->NewRow(),"kd:",0,9000000,0.1);
43            usat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat u:",0,1,.1);
44}
45
46NestedSat_impl::~NestedSat_impl(void)
47{
48}
49
50void NestedSat_impl::UpdateFrom(const io_data *data)
51{
52    float cons,dcons,law;
53    cvmatrix *input=(cvmatrix*)data;
54
55    input->GetMutex();
56
57    cons=Sat(input->ValueNoMutex(0,0),k*sat->Value());
58    dcons=(cons-input->ValueNoMutex(1,0))*kp->Value();
59    dcons=Sat(dcons,k*dsat->Value());
60    law=(input->ValueNoMutex(2,0)-dcons)*kd->Value();
61    law=Sat(law,usat->Value());
62
63    input->ReleaseMutex();
64
65    self->output->SetValue(0,0,law);
66    self->output->SetDataTime(data->DataTime());
67}
68
69float NestedSat_impl::Sat(float value,float borne)
70{
71    if(value<-borne) return -borne;
72    if(value>borne) return borne;
73    return value;
74}
Note: See TracBrowser for help on using the repository browser.