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

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

lic

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