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

Last change on this file was 318, checked in by Sanahuja Guillaume, 2 years ago
File size: 4.6 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 <Matrix.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  MatrixDescriptor *desc = new MatrixDescriptor(2, 1);
63  desc->SetElementName(0, 0, "pos");
64  desc->SetElementName(1, 0, "vel");
65  output = new Matrix(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,float startVelocity) {
82  is_started = true;
83  is_finished = false;
84  first_update = true;
85
86  // configure trajectory
87  this->start_pos = start_pos;
88  this->end_pos = end_pos;
89  pos = start_pos;
90  acc = acceleration->Value();
91  v = startVelocity;
92  if (fabs(v) > fabs(max_veloctity->Value())) {
93    if (v > 0)
94      v = max_veloctity->Value();
95    else
96      v = -max_veloctity->Value();
97  }
98  if (end_pos < start_pos) {
99    acc = -acc;
100    // max_veloctity=-max_veloctity;
101  }
102}
103
104// revoir l'interet du stop?
105void TrajectoryGenerator1D_impl::StopTraj(void) {
106  is_started = false;
107  v = 0;
108  // output->SetValue(1,0,v);
109}
110
111float TrajectoryGenerator1D_impl::GetPercentageOfCompletion(void) const {
112  if(!is_started) {
113    return 0;
114  } else {
115    return (pos-start_pos)/(end_pos-start_pos)*100;
116  }
117}
118
119void TrajectoryGenerator1D_impl::Update(Time time) {
120  float delta_t;
121
122  if (T->Value() == 0) {
123    if (first_update == true) {
124      first_update = false;
125      previous_time = time;
126      output->GetMutex();
127      output->SetValueNoMutex(0, 0, pos + pos_off);
128      output->SetValueNoMutex(1, 0, v + vel_off);
129      output->ReleaseMutex();
130
131      output->SetDataTime(time);
132      return;
133    } else {
134      delta_t = (float)(time - previous_time) / 1000000000.;
135    }
136  } else {
137    delta_t = T->Value();
138  }
139  previous_time = time;
140
141  if (is_started == true) {
142    if (is_finished == false) {
143      v += acc * delta_t;
144      if (fabs(v) > fabs(max_veloctity->Value())) {
145        if (v > 0)
146          v = max_veloctity->Value();
147        else
148          v = -max_veloctity->Value();
149      }
150      pos += v * delta_t;
151      if (end_pos - v * v / (2 * acc) <= pos && v >= 0)
152        acc = -acc;
153      if (end_pos - v * v / (2 * acc) >= pos && v < 0)
154        acc = -acc;
155      if (pos >= end_pos && v >= 0)
156        is_finished = true;
157      if (pos <= end_pos && v < 0)
158        is_finished = true;
159    }
160    // else
161    if (is_finished == true) {
162      v = 0;
163      pos = end_pos;
164    }
165  }
166
167  // on prend une fois pour toute les mutex et on fait des accès directs
168  output->GetMutex();
169  output->SetValueNoMutex(0, 0, pos + pos_off);
170  output->SetValueNoMutex(1, 0, v + vel_off);
171  output->ReleaseMutex();
172
173  output->SetDataTime(time);
174}
Note: See TracBrowser for help on using the repository browser.