Ignore:
Timestamp:
04/08/16 15:40:57 (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/TrajectoryGenerator1D_impl.cpp

    r10 r15  
    2929using namespace flair::filter;
    3030
    31 TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(TrajectoryGenerator1D* self,const LayoutPosition* position,string name,string unit) {
    32     first_update=true;
    33     is_started=false;
     31TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(
     32    TrajectoryGenerator1D *self, const LayoutPosition *position, string name,
     33    string unit) {
     34  first_update = true;
     35  is_started = false;
    3436
    35     //init UI
    36     GroupBox* reglages_groupbox=new GroupBox(position,name);
    37     T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto:"," s",0,1,0.01);
    38     if(unit=="") {
    39         max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):",0.,200000,1);
    40     } else {
    41         max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):"," "+ unit+"/s",0.,200000,1);
    42     }
    43     if(unit=="") {
    44         acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):",0.,10,1,3);
    45     } else {
    46         acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):"," "+unit +"/s²",0.,200000,1);
    47     }
     37  // init UI
     38  GroupBox *reglages_groupbox = new GroupBox(position, name);
     39  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:",
     40                        " s", 0, 1, 0.01);
     41  if (unit == "") {
     42    max_veloctity =
     43        new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     44                          "velocity max (absolute):", 0., 200000, 1);
     45  } else {
     46    max_veloctity = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     47                                      "velocity max (absolute):",
     48                                      " " + unit + "/s", 0., 200000, 1);
     49  }
     50  if (unit == "") {
     51    acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     52                                     "acceleration (absolute):", 0., 10, 1, 3);
     53  } else {
     54    acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     55                                     "acceleration (absolute):",
     56                                     " " + unit + "/s²", 0., 200000, 1);
     57  }
    4858
    49     Reset();
     59  Reset();
    5060
    51     //init matrix
    52     cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
    53     desc->SetElementName(0,0,"pos");
    54     desc->SetElementName(1,0,"vel");
    55     output=new cvmatrix(self,desc,floatType,name);
     61  // init matrix
     62  cvmatrix_descriptor *desc = new cvmatrix_descriptor(2, 1);
     63  desc->SetElementName(0, 0, "pos");
     64  desc->SetElementName(1, 0, "vel");
     65  output = new cvmatrix(self, desc, floatType, name);
    5666
    57     output->SetValue(0,0,pos);
    58     output->SetValue(1,0,v);
     67  output->SetValue(0, 0, pos);
     68  output->SetValue(1, 0, v);
    5969}
    6070
    61 TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {
     71TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {}
     72
     73void TrajectoryGenerator1D_impl::Reset(void) {
     74  pos = 0;
     75  v = 0;
     76  pos_off = 0;
     77  vel_off = 0;
    6278}
    6379
    64 void TrajectoryGenerator1D_impl::Reset(void) {
    65     pos=0;
    66     v=0;
    67     pos_off=0;
    68     vel_off=0;
     80void TrajectoryGenerator1D_impl::StartTraj(float start_pos, float end_pos) {
     81  is_started = true;
     82  is_finished = false;
     83  first_update = true;
     84
     85  // configure trajectory
     86  end_position = end_pos;
     87  pos = start_pos;
     88  acc = acceleration->Value();
     89  v = 0;
     90  if (end_position < start_pos) {
     91    acc = -acc;
     92    // max_veloctity=-max_veloctity;
     93  }
    6994}
    7095
    71 void TrajectoryGenerator1D_impl::StartTraj(float start_pos,float end_pos) {
    72     is_started=true;
    73     is_finished=false;
    74     first_update=true;
    75 
    76     //configure trajectory
    77     end_position=end_pos;
    78     pos=start_pos;
    79     acc=acceleration->Value();
    80     v=0;
    81     if(end_position<start_pos) {
    82         acc=-acc;
    83         //max_veloctity=-max_veloctity;
    84     }
    85 }
    86 
    87 //revoir l'interet du stop?
     96// revoir l'interet du stop?
    8897void TrajectoryGenerator1D_impl::StopTraj(void) {
    89     is_started=false;
    90     v=0;
    91     //output->SetValue(1,0,v);
     98  is_started = false;
     99  v = 0;
     100  // output->SetValue(1,0,v);
    92101}
    93102
    94103void TrajectoryGenerator1D_impl::Update(Time time) {
    95     float delta_t;
     104  float delta_t;
    96105
    97     if(T->Value()==0) {
    98         if(first_update==true) {
    99             first_update=false;
    100             previous_time=time;
    101             output->GetMutex();
    102             output->SetValueNoMutex(0,0,pos+pos_off);
    103             output->SetValueNoMutex(1,0,v+vel_off);
    104             output->ReleaseMutex();
     106  if (T->Value() == 0) {
     107    if (first_update == true) {
     108      first_update = false;
     109      previous_time = time;
     110      output->GetMutex();
     111      output->SetValueNoMutex(0, 0, pos + pos_off);
     112      output->SetValueNoMutex(1, 0, v + vel_off);
     113      output->ReleaseMutex();
    105114
    106             output->SetDataTime(time);
    107             return;
    108         } else {
    109             delta_t=(float)(time-previous_time)/1000000000.;
    110         }
     115      output->SetDataTime(time);
     116      return;
    111117    } else {
    112         delta_t=T->Value();
     118      delta_t = (float)(time - previous_time) / 1000000000.;
    113119    }
    114     previous_time=time;
     120  } else {
     121    delta_t = T->Value();
     122  }
     123  previous_time = time;
    115124
     125  if (is_started == true) {
     126    if (is_finished == false) {
     127      v += acc * delta_t;
     128      if (fabs(v) > fabs(max_veloctity->Value())) {
     129        if (v > 0)
     130          v = max_veloctity->Value();
     131        else
     132          v = -max_veloctity->Value();
     133      }
     134      pos += v * delta_t;
     135      if (end_position - v * v / (2 * acc) <= pos && v >= 0)
     136        acc = -acc;
     137      if (end_position - v * v / (2 * acc) >= pos && v < 0)
     138        acc = -acc;
     139      if (pos >= end_position && v >= 0)
     140        is_finished = true;
     141      if (pos <= end_position && v < 0)
     142        is_finished = true;
     143    }
     144    // else
     145    if (is_finished == true) {
     146      v = 0;
     147      pos = end_position;
     148    }
     149  }
    116150
    117     if(is_started==true) {
    118         if(is_finished==false) {
    119             v+=acc*delta_t;
    120             if(fabs(v)>fabs(max_veloctity->Value())) {
    121                 if(v>0)
    122                     v=max_veloctity->Value();
    123                 else
    124                     v=-max_veloctity->Value();
    125             }
    126             pos+=v*delta_t;
    127             if(end_position-v*v/(2*acc)<=pos && v>=0) acc=-acc;
    128             if(end_position-v*v/(2*acc)>=pos && v<0) acc=-acc;
    129             if(pos>=end_position && v>=0) is_finished=true;
    130             if(pos<=end_position && v<0) is_finished=true;
    131         }
    132         //else
    133         if(is_finished==true) {
    134             v=0;
    135             pos=end_position;
    136         }
    137     }
     151  // on prend une fois pour toute les mutex et on fait des accès directs
     152  output->GetMutex();
     153  output->SetValueNoMutex(0, 0, pos + pos_off);
     154  output->SetValueNoMutex(1, 0, v + vel_off);
     155  output->ReleaseMutex();
    138156
    139     //on prend une fois pour toute les mutex et on fait des accès directs
    140     output->GetMutex();
    141     output->SetValueNoMutex(0,0,pos+pos_off);
    142     output->SetValueNoMutex(1,0,v+vel_off);
    143     output->ReleaseMutex();
    144 
    145     output->SetDataTime(time);
     157  output->SetDataTime(time);
    146158}
Note: See TracChangeset for help on using the changeset viewer.