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

Last change on this file since 148 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.