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

Last change on this file was 318, checked in by Sanahuja Guillaume, 3 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.