source: trunk/extremum_seeking/esc_sm/src/sm_esc_1d.cpp @ 5

/*
 * sm_esc_1d.cpp
 *
 *  Created on: Jul 30, 2012
 *      Author: Berk Calli
 *      Organization: Delft Biorobotics Lab., Delft University of Technology
 *              Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
 *
 *  Class for one dimensional sliding mode extremum seeking control with periodic driving signal
 *
 * * References:
 * - H. Yu and U. Ozguner, “Extremum-seeking Control Strategy for ABS System with Time Delay,” ACC 2002.
 * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
 */

#include "esc_sm/sm_esc_1d.h"

SMESC1D::SMESC1D(){
        rho_ = 0;
        k_ = 0;
        alpha_ = 0;
        driving_input_ = 0;
        driving_input_init_ = false;
        initialized_ = false;
}

SMESC1D::SMESC1D(double rho, double k, double alpha){
        init(rho,k,alpha);
}

void SMESC1D::init(double rho, double k, double alpha){
        rho_ = rho;
        k_ = k;
        alpha_ = alpha;
        driving_input_ = 0;
        driving_input_init_ = false;
        initialized_ = true;
}

ESC::inputType SMESC1D::getInputType(){
        return ESC::inputValue;
}

ESC::outputType SMESC1D::getOutputType(){
        return ESC::outputVelocity;
}

std::vector<double> SMESC1D::step(double obj_val){
        if (!initialized_){
                fprintf(stderr,"The sliding mode ESC (1D) is not initialized... step function will not be executed. \n");
                return std::vector<double>();
        }
        if(!driving_input_init_){
                driving_input_ = obj_val;
                driving_input_init_ = true;
        }

        driving_input_ = driving_input_- rho_;
        double s = obj_val - driving_input_;
        vel_ref_ = k_*sign(sin(PI/alpha_*s));
        std::vector<double> output;
        output.push_back(vel_ref_);
        return output;
}

int SMESC1D::sign(double value){
        if (value>0){
                return 1;
        }
        else if (value<0){
                return -1;
        }
        else{
                return 0;
        }
}

std::vector<double> SMESC1D::monitor(){
        std::vector<double> monitor_values;
        monitor_values.push_back(driving_input_);
        return monitor_values;
}
std::vector<std::string> SMESC1D::monitorNames(){
        std::vector<std::string> monitor_names;
        monitor_names.push_back("driving input");
        return monitor_names;
}