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

Last change on this file since 7 was 7, checked in by Sanahuja Guillaume, 5 years ago

filter and meta

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