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

Last change on this file since 157 was 148, checked in by Sanahuja Guillaume, 5 years ago

m

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  delete desc;
67
68  output->SetValue(0, 0, pos);
69  output->SetValue(1, 0, v);
70}
71
72TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {}
73
74void TrajectoryGenerator1D_impl::Reset(void) {
75  pos = 0;
76  v = 0;
77  pos_off = 0;
78  vel_off = 0;
79}
80
81void TrajectoryGenerator1D_impl::StartTraj(float start_pos, float end_pos) {
82  is_started = true;
83  is_finished = false;
84  first_update = true;
85
86  // configure trajectory
87  end_position = end_pos;
88  pos = start_pos;
89  acc = acceleration->Value();
90  v = 0;
91  if (end_position < start_pos) {
92    acc = -acc;
93    // max_veloctity=-max_veloctity;
94  }
95}
96
97// revoir l'interet du stop?
98void TrajectoryGenerator1D_impl::StopTraj(void) {
99  is_started = false;
100  v = 0;
101  // output->SetValue(1,0,v);
102}
103
104void TrajectoryGenerator1D_impl::Update(Time time) {
105  float delta_t;
106
107  if (T->Value() == 0) {
108    if (first_update == true) {
109      first_update = false;
110      previous_time = time;
111      output->GetMutex();
112      output->SetValueNoMutex(0, 0, pos + pos_off);
113      output->SetValueNoMutex(1, 0, v + vel_off);
114      output->ReleaseMutex();
115
116      output->SetDataTime(time);
117      return;
118    } else {
119      delta_t = (float)(time - previous_time) / 1000000000.;
120    }
121  } else {
122    delta_t = T->Value();
123  }
124  previous_time = time;
125
126  if (is_started == true) {
127    if (is_finished == false) {
128      v += acc * delta_t;
129      if (fabs(v) > fabs(max_veloctity->Value())) {
130        if (v > 0)
131          v = max_veloctity->Value();
132        else
133          v = -max_veloctity->Value();
134      }
135      pos += v * delta_t;
136      if (end_position - v * v / (2 * acc) <= pos && v >= 0)
137        acc = -acc;
138      if (end_position - v * v / (2 * acc) >= pos && v < 0)
139        acc = -acc;
140      if (pos >= end_position && v >= 0)
141        is_finished = true;
142      if (pos <= end_position && v < 0)
143        is_finished = true;
144    }
145    // else
146    if (is_finished == true) {
147      v = 0;
148      pos = end_position;
149    }
150  }
151
152  // on prend une fois pour toute les mutex et on fait des accès directs
153  output->GetMutex();
154  output->SetValueNoMutex(0, 0, pos + pos_off);
155  output->SetValueNoMutex(1, 0, v + vel_off);
156  output->ReleaseMutex();
157
158  output->SetDataTime(time);
159}
Note: See TracBrowser for help on using the repository browser.