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

// %flair:license{

// This file is part of the Flair framework distributed under the

// CECILL-C License, Version 1.0.

// %flair:license}

//  created:    2011/05/01

//  filename:   TrajectoryGenerator1D_impl.cpp

//

//  author:     Guillaume Sanahuja

//              Copyright Heudiasyc UMR UTC/CNRS 7253

//

//  version:    $Id: $

//

//  purpose:    Class generating a trajectory in 1D

//

//

/*********************************************************************/



#include "TrajectoryGenerator1D.h"

#include "TrajectoryGenerator1D_impl.h"

#include <cvmatrix.h>

#include <Layout.h>

#include <GroupBox.h>

#include <DoubleSpinBox.h>

#include <cmath>



using std::string;

using namespace flair::core;

using namespace flair::gui;

using namespace flair::filter;



TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(TrajectoryGenerator1D* self,const LayoutPosition* position,string name,string unit) {

    first_update=true;

    is_started=false;



    //init UI

    GroupBox* reglages_groupbox=new GroupBox(position,name);

    T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto:"," s",0,1,0.01);

    if(unit=="") {

        max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):",0.,200000,1);

    } else {

        max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):"," "+ unit+"/s",0.,200000,1);

    }

    if(unit=="") {

        acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):",0.,10,1,3);

    } else {

        acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):"," "+unit +"/s²",0.,200000,1);

    }



    Reset();



    //init matrix

    cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);

    desc->SetElementName(0,0,"pos");

    desc->SetElementName(1,0,"vel");

    output=new cvmatrix(self,desc,floatType,name);



    output->SetValue(0,0,pos);

    output->SetValue(1,0,v);

}



TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {

}



void TrajectoryGenerator1D_impl::Reset(void) {

    pos=0;

    v=0;

    pos_off=0;

    vel_off=0;

}



void TrajectoryGenerator1D_impl::StartTraj(float start_pos,float end_pos) {

    is_started=true;

    is_finished=false;

    first_update=true;



    //configure trajectory

    end_position=end_pos;

    pos=start_pos;

    acc=acceleration->Value();

    v=0;

    if(end_position<start_pos) {

        acc=-acc;

        //max_veloctity=-max_veloctity;

    }

}



//revoir l'interet du stop?

void TrajectoryGenerator1D_impl::StopTraj(void) {

    is_started=false;

    v=0;

    //output->SetValue(1,0,v);

}



void TrajectoryGenerator1D_impl::Update(Time time) {

    float delta_t;



    if(T->Value()==0) {

        if(first_update==true) {

            first_update=false;

            previous_time=time;

            output->GetMutex();

            output->SetValueNoMutex(0,0,pos+pos_off);

            output->SetValueNoMutex(1,0,v+vel_off);

            output->ReleaseMutex();



            output->SetDataTime(time);

            return;

        } else {

            delta_t=(float)(time-previous_time)/1000000000.;

        }

    } else {

        delta_t=T->Value();

    }

    previous_time=time;





    if(is_started==true) {

        if(is_finished==false) {

            v+=acc*delta_t;

            if(fabs(v)>fabs(max_veloctity->Value())) {

                if(v>0)

                    v=max_veloctity->Value();

                else

                    v=-max_veloctity->Value();

            }

            pos+=v*delta_t;

            if(end_position-v*v/(2*acc)<=pos && v>=0) acc=-acc;

            if(end_position-v*v/(2*acc)>=pos && v<0) acc=-acc;

            if(pos>=end_position && v>=0) is_finished=true;

            if(pos<=end_position && v<0) is_finished=true;

        }

        //else

        if(is_finished==true) {

            v=0;

            pos=end_position;

        }

    }



    //on prend une fois pour toute les mutex et on fait des accès directs

    output->GetMutex();

    output->SetValueNoMutex(0,0,pos+pos_off);

    output->SetValueNoMutex(1,0,v+vel_off);

    output->ReleaseMutex();



    output->SetDataTime(time);

}