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

Last change on this file since 137 was 15, checked in by Bayard Gildas, 6 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.