source: flair-src/trunk/lib/FlairFilter/src/Pid_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: 2.9 KB
Line 
1//  created:    2011/05/01
2//  filename:   Pid_impl.cpp
3//
4//  author:     Guillaume Sanahuja
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    Class defining a PID
10//
11//
12/*********************************************************************/
13#include "Pid_impl.h"
14#include "Pid.h"
15#include <cvmatrix.h>
16#include <Layout.h>
17#include <GroupBox.h>
18#include <DoubleSpinBox.h>
19#include <DataPlot1D.h>
20
21using std::string;
22using namespace flair::core;
23using namespace flair::gui;
24using namespace flair::filter;
25
26Pid_impl::Pid_impl(Pid* self,const LayoutPosition* position,string name) {
27    i=0;
28    first_update=true;
29    this->self=self;
30
31    //init matrix
32    self->input=new cvmatrix(self,2,1,floatType,name);
33
34    cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,1);
35    desc->SetElementName(0,0,"p");
36    desc->SetElementName(1,0,"i");
37    desc->SetElementName(2,0,"d");
38    desc->SetElementName(3,0,"p+i+d");
39    state=new cvmatrix(self,desc,floatType,name);
40
41    GroupBox* reglages_groupbox=new GroupBox(position,name);
42    T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto"," s",0,1,0.01);
43    kp=new DoubleSpinBox(reglages_groupbox->NewRow(),"kp:",0,90000000,0.01,3);
44    ki=new DoubleSpinBox(reglages_groupbox->NewRow(),"ki:",0,90000000,0.01,3);
45    sati=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"sat i:",0,1,0.01);
46    kd=new DoubleSpinBox(reglages_groupbox->NewRow(),"kd:",0,90000000,0.01,3);
47    sat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat:",0,1,0.1);
48}
49
50Pid_impl::~Pid_impl(void) {
51}
52
53void Pid_impl::UseDefaultPlot(const LayoutPosition* position) {
54    DataPlot1D *plot=new DataPlot1D(position,self->ObjectName(),-1,1);
55    plot->AddCurve(state->Element(0));
56    plot->AddCurve(state->Element(1),DataPlot::Green);
57    plot->AddCurve(state->Element(2),DataPlot::Blue);
58    plot->AddCurve(state->Element(3),DataPlot::Black);
59}
60
61void Pid_impl::UpdateFrom(const io_data *data) {
62    float p,d,total;
63    float delta_t;
64    cvmatrix *input=(cvmatrix*)data;
65
66    if(T->Value()==0) {
67        delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
68    } else {
69        delta_t=T->Value();
70    }
71    if(first_update==true) {
72        delta_t=0;
73        first_update=false;
74    }
75
76    input->GetMutex();
77    p=kp->Value()*input->ValueNoMutex(0,0);
78    i+=ki->Value()*input->ValueNoMutex(0,0)*delta_t;
79    if(i>sati->Value()) i=sati->Value();
80    if(i<-sati->Value()) i=-sati->Value();
81    d=kd->Value()*input->ValueNoMutex(1,0);
82    input->ReleaseMutex();
83
84    total=p+i+d;
85    if(total>sat->Value()) total=sat->Value();
86    if(total<-sat->Value()) total=-sat->Value();
87
88    state->GetMutex();
89    state->SetValueNoMutex(0,0,p);
90    state->SetValueNoMutex(1,0,i);
91    state->SetValueNoMutex(2,0,d);
92    state->SetValueNoMutex(3,0,total);
93    state->ReleaseMutex();
94
95    self->output->SetValue(0,0,total);
96    self->output->SetDataTime(data->DataTime());
97
98    previous_time=data->DataTime();
99}
Note: See TracBrowser for help on using the repository browser.