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

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

lic

File size: 4.1 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(TrajectoryGenerator1D* self,const LayoutPosition* position,string name,string unit) {
32 first_update=true;
33 is_started=false;
34
35 //init UI
36 GroupBox* reglages_groupbox=new GroupBox(position,name);
37 T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto:"," s",0,1,0.01);
38 if(unit=="") {
39 max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):",0.,200000,1);
40 } else {
41 max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):"," "+ unit+"/s",0.,200000,1);
42 }
43 if(unit=="") {
44 acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):",0.,10,1,3);
45 } else {
46 acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):"," "+unit +"/s²",0.,200000,1);
47 }
48
49 Reset();
50
51 //init matrix
52 cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
53 desc->SetElementName(0,0,"pos");
54 desc->SetElementName(1,0,"vel");
55 output=new cvmatrix(self,desc,floatType,name);
56
57 output->SetValue(0,0,pos);
58 output->SetValue(1,0,v);
59}
60
61TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {
62}
63
64void TrajectoryGenerator1D_impl::Reset(void) {
65 pos=0;
66 v=0;
67 pos_off=0;
68 vel_off=0;
69}
70
71void TrajectoryGenerator1D_impl::StartTraj(float start_pos,float end_pos) {
72 is_started=true;
73 is_finished=false;
74 first_update=true;
75
76 //configure trajectory
77 end_position=end_pos;
78 pos=start_pos;
79 acc=acceleration->Value();
80 v=0;
81 if(end_position<start_pos) {
82 acc=-acc;
83 //max_veloctity=-max_veloctity;
84 }
85}
86
87//revoir l'interet du stop?
88void TrajectoryGenerator1D_impl::StopTraj(void) {
89 is_started=false;
90 v=0;
91 //output->SetValue(1,0,v);
92}
93
94void TrajectoryGenerator1D_impl::Update(Time time) {
95 float delta_t;
96
97 if(T->Value()==0) {
98 if(first_update==true) {
99 first_update=false;
100 previous_time=time;
101 output->GetMutex();
102 output->SetValueNoMutex(0,0,pos+pos_off);
103 output->SetValueNoMutex(1,0,v+vel_off);
104 output->ReleaseMutex();
105
106 output->SetDataTime(time);
107 return;
108 } else {
109 delta_t=(float)(time-previous_time)/1000000000.;
110 }
111 } else {
112 delta_t=T->Value();
113 }
114 previous_time=time;
115
116
117 if(is_started==true) {
118 if(is_finished==false) {
119 v+=acc*delta_t;
120 if(fabs(v)>fabs(max_veloctity->Value())) {
121 if(v>0)
122 v=max_veloctity->Value();
123 else
124 v=-max_veloctity->Value();
125 }
126 pos+=v*delta_t;
127 if(end_position-v*v/(2*acc)<=pos && v>=0) acc=-acc;
128 if(end_position-v*v/(2*acc)>=pos && v<0) acc=-acc;
129 if(pos>=end_position && v>=0) is_finished=true;
130 if(pos<=end_position && v<0) is_finished=true;
131 }
132 //else
133 if(is_finished==true) {
134 v=0;
135 pos=end_position;
136 }
137 }
138
139 //on prend une fois pour toute les mutex et on fait des accès directs
140 output->GetMutex();
141 output->SetValueNoMutex(0,0,pos+pos_off);
142 output->SetValueNoMutex(1,0,v+vel_off);
143 output->ReleaseMutex();
144
145 output->SetDataTime(time);
146}
Note: See TracBrowser for help on using the repository browser.