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