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

Last change on this file since 7 was 7, checked in by Sanahuja Guillaume, 7 years ago

filter and meta

File size: 3.9 KB
Line 
1// created: 2011/05/01
2// filename: TrajectoryGenerator1D_impl.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: Class generating a trajectory in 1D
10//
11//
12/*********************************************************************/
13
14#include "TrajectoryGenerator1D.h"
15#include "TrajectoryGenerator1D_impl.h"
16#include <cvmatrix.h>
17#include <Layout.h>
18#include <GroupBox.h>
19#include <DoubleSpinBox.h>
20#include <cmath>
21
22using std::string;
23using namespace flair::core;
24using namespace flair::gui;
25using namespace flair::filter;
26
27TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(TrajectoryGenerator1D* self,const LayoutPosition* position,string name,string unit) {
28 first_update=true;
29 is_started=false;
30
31 //init UI
32 GroupBox* reglages_groupbox=new GroupBox(position,name);
33 T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto:"," s",0,1,0.01);
34 if(unit=="") {
35 max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):",0.,200000,1);
36 } else {
37 max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):"," "+ unit+"/s",0.,200000,1);
38 }
39 if(unit=="") {
40 acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):",0.,10,1,3);
41 } else {
42 acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):"," "+unit +"/s²",0.,200000,1);
43 }
44
45 Reset();
46
47 //init matrix
48 cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
49 desc->SetElementName(0,0,"pos");
50 desc->SetElementName(1,0,"vel");
51 output=new cvmatrix(self,desc,floatType,name);
52
53 output->SetValue(0,0,pos);
54 output->SetValue(1,0,v);
55}
56
57TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {
58}
59
60void TrajectoryGenerator1D_impl::Reset(void) {
61 pos=0;
62 v=0;
63 pos_off=0;
64 vel_off=0;
65}
66
67void TrajectoryGenerator1D_impl::StartTraj(float start_pos,float end_pos) {
68 is_started=true;
69 is_finished=false;
70 first_update=true;
71
72 //configure trajectory
73 end_position=end_pos;
74 pos=start_pos;
75 acc=acceleration->Value();
76 v=0;
77 if(end_position<start_pos) {
78 acc=-acc;
79 //max_veloctity=-max_veloctity;
80 }
81}
82
83//revoir l'interet du stop?
84void TrajectoryGenerator1D_impl::StopTraj(void) {
85 is_started=false;
86 v=0;
87 //output->SetValue(1,0,v);
88}
89
90void TrajectoryGenerator1D_impl::Update(Time time) {
91 float delta_t;
92
93 if(T->Value()==0) {
94 if(first_update==true) {
95 first_update=false;
96 previous_time=time;
97 output->GetMutex();
98 output->SetValueNoMutex(0,0,pos+pos_off);
99 output->SetValueNoMutex(1,0,v+vel_off);
100 output->ReleaseMutex();
101
102 output->SetDataTime(time);
103 return;
104 } else {
105 delta_t=(float)(time-previous_time)/1000000000.;
106 }
107 } else {
108 delta_t=T->Value();
109 }
110 previous_time=time;
111
112
113 if(is_started==true) {
114 if(is_finished==false) {
115 v+=acc*delta_t;
116 if(fabs(v)>fabs(max_veloctity->Value())) {
117 if(v>0)
118 v=max_veloctity->Value();
119 else
120 v=-max_veloctity->Value();
121 }
122 pos+=v*delta_t;
123 if(end_position-v*v/(2*acc)<=pos && v>=0) acc=-acc;
124 if(end_position-v*v/(2*acc)>=pos && v<0) acc=-acc;
125 if(pos>=end_position && v>=0) is_finished=true;
126 if(pos<=end_position && v<0) is_finished=true;
127 }
128 //else
129 if(is_finished==true) {
130 v=0;
131 pos=end_position;
132 }
133 }
134
135 //on prend une fois pour toute les mutex et on fait des accès directs
136 output->GetMutex();
137 output->SetValueNoMutex(0,0,pos+pos_off);
138 output->SetValueNoMutex(1,0,v+vel_off);
139 output->ReleaseMutex();
140
141 output->SetDataTime(time);
142}
Note: See TracBrowser for help on using the repository browser.