source: flair-src/trunk/lib/FlairFilter/src/TrajectoryGenerator1D_impl.cpp @ 15

Last change on this file since 15 was 15, checked in by Bayard Gildas, 5 years ago

sources reformatted with flair-format-dir script

File size: 4.3 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:   TrajectoryGenerator1D_impl.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class generating a trajectory in 1D
14//
15//
16/*********************************************************************/
17
18#include "TrajectoryGenerator1D.h"
19#include "TrajectoryGenerator1D_impl.h"
20#include <cvmatrix.h>
21#include <Layout.h>
22#include <GroupBox.h>
23#include <DoubleSpinBox.h>
24#include <cmath>
25
26using std::string;
27using namespace flair::core;
28using namespace flair::gui;
29using namespace flair::filter;
30
31TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(
32    TrajectoryGenerator1D *self, const LayoutPosition *position, string name,
33    string unit) {
34  first_update = true;
35  is_started = false;
36
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  }
58
59  Reset();
60
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);
66
67  output->SetValue(0, 0, pos);
68  output->SetValue(1, 0, v);
69}
70
71TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {}
72
73void TrajectoryGenerator1D_impl::Reset(void) {
74  pos = 0;
75  v = 0;
76  pos_off = 0;
77  vel_off = 0;
78}
79
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  }
94}
95
96// revoir l'interet du stop?
97void TrajectoryGenerator1D_impl::StopTraj(void) {
98  is_started = false;
99  v = 0;
100  // output->SetValue(1,0,v);
101}
102
103void TrajectoryGenerator1D_impl::Update(Time time) {
104  float delta_t;
105
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();
114
115      output->SetDataTime(time);
116      return;
117    } else {
118      delta_t = (float)(time - previous_time) / 1000000000.;
119    }
120  } else {
121    delta_t = T->Value();
122  }
123  previous_time = time;
124
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  }
150
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();
156
157  output->SetDataTime(time);
158}
Note: See TracBrowser for help on using the repository browser.