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

Timestamp:

04/08/16 15:40:57 (7 years ago)

Author:

Bayard Gildas

Message:

sources reformatted with flair-format-dir script

File:

• trunk/lib/FlairFilter/src/TrajectoryGenerator1D_impl.cpp (modified) (1 diff)

trunk/lib/FlairFilter/src/TrajectoryGenerator1D_impl.cpp

@@ -29 +29 @@
 using namespace flair::filter;
 
-TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(TrajectoryGenerator1D* self,const LayoutPosition* position,string name,string unit) {
-    first_update=true;
-    is_started=false;
+TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(
+    TrajectoryGenerator1D *self, const LayoutPosition *position, string name,
+    string unit) {
+  first_update = true;
+  is_started = false;
 
-    //init UI
-    GroupBox* reglages_groupbox=new GroupBox(position,name);
-    T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto:"," s",0,1,0.01);
-    if(unit=="") {
-        max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):",0.,200000,1);
-    } else {
-        max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):"," "+ unit+"/s",0.,200000,1);
-    }
-    if(unit=="") {
-        acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):",0.,10,1,3);
-    } else {
-        acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):"," "+unit +"/s²",0.,200000,1);
-    }
+  // init UI
+  GroupBox *reglages_groupbox = new GroupBox(position, name);
+  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:",
+                        " s", 0, 1, 0.01);
+  if (unit == "") {
+    max_veloctity =
+        new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
+                          "velocity max (absolute):", 0., 200000, 1);
+  } else {
+    max_veloctity = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
+                                      "velocity max (absolute):",
+                                      " " + unit + "/s", 0., 200000, 1);
+  }
+  if (unit == "") {
+    acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
+                                     "acceleration (absolute):", 0., 10, 1, 3);
+  } else {
+    acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
+                                     "acceleration (absolute):",
+                                     " " + unit + "/s²", 0., 200000, 1);
+  }
 
-    Reset();
+  Reset();
 
-    //init matrix
-    cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
-    desc->SetElementName(0,0,"pos");
-    desc->SetElementName(1,0,"vel");
-    output=new cvmatrix(self,desc,floatType,name);
+  // init matrix
+  cvmatrix_descriptor *desc = new cvmatrix_descriptor(2, 1);
+  desc->SetElementName(0, 0, "pos");
+  desc->SetElementName(1, 0, "vel");
+  output = new cvmatrix(self, desc, floatType, name);
 
-    output->SetValue(0,0,pos);
-    output->SetValue(1,0,v);
+  output->SetValue(0, 0, pos);
+  output->SetValue(1, 0, v);
 }
 
-TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {
+TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {}
+
+void TrajectoryGenerator1D_impl::Reset(void) {
+  pos = 0;
+  v = 0;
+  pos_off = 0;
+  vel_off = 0;
 }
 
-void TrajectoryGenerator1D_impl::Reset(void) {
-    pos=0;
-    v=0;
-    pos_off=0;
-    vel_off=0;
+void TrajectoryGenerator1D_impl::StartTraj(float start_pos, float end_pos) {
+  is_started = true;
+  is_finished = false;
+  first_update = true;
+
+  // configure trajectory
+  end_position = end_pos;
+  pos = start_pos;
+  acc = acceleration->Value();
+  v = 0;
+  if (end_position < start_pos) {
+    acc = -acc;
+    // max_veloctity=-max_veloctity;
+  }
 }
 
-void TrajectoryGenerator1D_impl::StartTraj(float start_pos,float end_pos) {
-    is_started=true;
-    is_finished=false;
-    first_update=true;
-
-    //configure trajectory
-    end_position=end_pos;
-    pos=start_pos;
-    acc=acceleration->Value();
-    v=0;
-    if(end_position<start_pos) {
-        acc=-acc;
-        //max_veloctity=-max_veloctity;
-    }
-}
-
-//revoir l'interet du stop?
+// revoir l'interet du stop?
 void TrajectoryGenerator1D_impl::StopTraj(void) {
-    is_started=false;
-    v=0;
-    //output->SetValue(1,0,v);
+  is_started = false;
+  v = 0;
+  // output->SetValue(1,0,v);
 }
 
 void TrajectoryGenerator1D_impl::Update(Time time) {
-    float delta_t;
+  float delta_t;
 
-    if(T->Value()==0) {
-        if(first_update==true) {
-            first_update=false;
-            previous_time=time;
-            output->GetMutex();
-            output->SetValueNoMutex(0,0,pos+pos_off);
-            output->SetValueNoMutex(1,0,v+vel_off);
-            output->ReleaseMutex();
+  if (T->Value() == 0) {
+    if (first_update == true) {
+      first_update = false;
+      previous_time = time;
+      output->GetMutex();
+      output->SetValueNoMutex(0, 0, pos + pos_off);
+      output->SetValueNoMutex(1, 0, v + vel_off);
+      output->ReleaseMutex();
 
-            output->SetDataTime(time);
-            return;
-        } else {
-            delta_t=(float)(time-previous_time)/1000000000.;
-        }
+      output->SetDataTime(time);
+      return;
     } else {
-        delta_t=T->Value();
+      delta_t = (float)(time - previous_time) / 1000000000.;
     }
-    previous_time=time;
+  } else {
+    delta_t = T->Value();
+  }
+  previous_time = time;
 
+  if (is_started == true) {
+    if (is_finished == false) {
+      v += acc * delta_t;
+      if (fabs(v) > fabs(max_veloctity->Value())) {
+        if (v > 0)
+          v = max_veloctity->Value();
+        else
+          v = -max_veloctity->Value();
+      }
+      pos += v * delta_t;
+      if (end_position - v * v / (2 * acc) <= pos && v >= 0)
+        acc = -acc;
+      if (end_position - v * v / (2 * acc) >= pos && v < 0)
+        acc = -acc;
+      if (pos >= end_position && v >= 0)
+        is_finished = true;
+      if (pos <= end_position && v < 0)
+        is_finished = true;
+    }
+    // else
+    if (is_finished == true) {
+      v = 0;
+      pos = end_position;
+    }
+  }
 
-    if(is_started==true) {
-        if(is_finished==false) {
-            v+=acc*delta_t;
-            if(fabs(v)>fabs(max_veloctity->Value())) {
-                if(v>0)
-                    v=max_veloctity->Value();
-                else
-                    v=-max_veloctity->Value();
-            }
-            pos+=v*delta_t;
-            if(end_position-v*v/(2*acc)<=pos && v>=0) acc=-acc;
-            if(end_position-v*v/(2*acc)>=pos && v<0) acc=-acc;
-            if(pos>=end_position && v>=0) is_finished=true;
-            if(pos<=end_position && v<0) is_finished=true;
-        }
-        //else
-        if(is_finished==true) {
-            v=0;
-            pos=end_position;
-        }
-    }
+  // on prend une fois pour toute les mutex et on fait des accès directs
+  output->GetMutex();
+  output->SetValueNoMutex(0, 0, pos + pos_off);
+  output->SetValueNoMutex(1, 0, v + vel_off);
+  output->ReleaseMutex();
 
-    //on prend une fois pour toute les mutex et on fait des accès directs
-    output->GetMutex();
-    output->SetValueNoMutex(0,0,pos+pos_off);
-    output->SetValueNoMutex(1,0,v+vel_off);
-    output->ReleaseMutex();
-
-    output->SetDataTime(time);
+  output->SetDataTime(time);
 }