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, 5 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.