Changeset 15 in flair-src for trunk/lib/FlairFilter/src/Pid_impl.cpp


Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (6 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairFilter/src/Pid_impl.cpp

    r10 r15  
    2828using namespace flair::filter;
    2929
    30 Pid_impl::Pid_impl(Pid* self,const LayoutPosition* position,string name) {
    31     i=0;
    32     first_update=true;
    33     this->self=self;
     30Pid_impl::Pid_impl(Pid *self, const LayoutPosition *position, string name) {
     31  i = 0;
     32  first_update = true;
     33  this->self = self;
    3434
    35     //init matrix
    36     self->input=new cvmatrix(self,2,1,floatType,name);
     35  // init matrix
     36  self->input = new cvmatrix(self, 2, 1, floatType, name);
    3737
    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);
     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);
    4444
    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);
     45  GroupBox *reglages_groupbox = new GroupBox(position, name);
     46  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
     47                        0, 1, 0.01);
     48  kp = new DoubleSpinBox(reglages_groupbox->NewRow(), "kp:", 0, 90000000, 0.01,
     49                         3);
     50  ki = new DoubleSpinBox(reglages_groupbox->NewRow(), "ki:", 0, 90000000, 0.01,
     51                         3);
     52  sati = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "sat i:", 0, 1,
     53                           0.01);
     54  kd = new DoubleSpinBox(reglages_groupbox->NewRow(), "kd:", 0, 90000000, 0.01,
     55                         3);
     56  sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat:", 0, 1, 0.1);
    5257}
    5358
    54 Pid_impl::~Pid_impl(void) {
    55 }
     59Pid_impl::~Pid_impl(void) {}
    5660
    57 void 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);
     61void Pid_impl::UseDefaultPlot(const LayoutPosition *position) {
     62  DataPlot1D *plot = new DataPlot1D(position, self->ObjectName(), -1, 1);
     63  plot->AddCurve(state->Element(0));
     64  plot->AddCurve(state->Element(1), DataPlot::Green);
     65  plot->AddCurve(state->Element(2), DataPlot::Blue);
     66  plot->AddCurve(state->Element(3), DataPlot::Black);
    6367}
    6468
    6569void Pid_impl::UpdateFrom(const io_data *data) {
    66     float p,d,total;
    67     float delta_t;
    68     cvmatrix *input=(cvmatrix*)data;
     70  float p, d, total;
     71  float delta_t;
     72  cvmatrix *input = (cvmatrix *)data;
    6973
    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     }
     74  if (T->Value() == 0) {
     75    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     76  } else {
     77    delta_t = T->Value();
     78  }
     79  if (first_update == true) {
     80    delta_t = 0;
     81    first_update = false;
     82  }
    7983
    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();
     84  input->GetMutex();
     85  p = kp->Value() * input->ValueNoMutex(0, 0);
     86  i += ki->Value() * input->ValueNoMutex(0, 0) * delta_t;
     87  if (i > sati->Value())
     88    i = sati->Value();
     89  if (i < -sati->Value())
     90    i = -sati->Value();
     91  d = kd->Value() * input->ValueNoMutex(1, 0);
     92  input->ReleaseMutex();
    8793
    88     total=p+i+d;
    89     if(total>sat->Value()) total=sat->Value();
    90     if(total<-sat->Value()) total=-sat->Value();
     94  total = p + i + d;
     95  if (total > sat->Value())
     96    total = sat->Value();
     97  if (total < -sat->Value())
     98    total = -sat->Value();
    9199
    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();
     100  state->GetMutex();
     101  state->SetValueNoMutex(0, 0, p);
     102  state->SetValueNoMutex(1, 0, i);
     103  state->SetValueNoMutex(2, 0, d);
     104  state->SetValueNoMutex(3, 0, total);
     105  state->ReleaseMutex();
    98106
    99     self->output->SetValue(0,0,total);
    100     self->output->SetDataTime(data->DataTime());
     107  self->output->SetValue(0, 0, total);
     108  self->output->SetDataTime(data->DataTime());
    101109
    102     previous_time=data->DataTime();
     110  previous_time = data->DataTime();
    103111}
Note: See TracChangeset for help on using the changeset viewer.