source: trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp @ 5

/*
 * nn_esc_1d.cpp
 *
 *  Created on: Jul 26, 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 neural network extremum seeking control
 *
 * * References:
 * - M. Teixeira and S. Zak, “Analog neural nonderivative optimizers,” IEEE Transactions on Neural Networks, vol. 9, pp. 629–638, 1998.
 * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
 */
#include "esc_nn/nn_esc_1d.h"

NNESC1D::NNESC1D(){
        M_ = 0;
        A_ = 0;
        ddelta_ = 0;
        delta_ = 0;
        B_ = 0;
        w_switch_old_ = 0;
        a_switch_old_ = 0;
        yr_ = 0;
        period_ = 0;
        min_peak_ = 0;
        vel_ref_ = 0;
        w_switch_ = 0;
        min_peak_detect_init_ = false;
        initialized_ = false;
}

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

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

std::vector<double> NNESC1D::monitor(){
        std::vector<double> monitor_vals;
        monitor_vals.push_back(yr_);
        monitor_vals.push_back(min_peak_);
        monitor_vals.push_back(w_switch_);

        return monitor_vals;

}

std::vector<std::string> NNESC1D::monitorNames(){
        std::vector<std::string> monitor_names;
        monitor_names.push_back("driving input value");
        monitor_names.push_back("minimum peak detector output");
        monitor_names.push_back("w switch value");

        return monitor_names;
}

NNESC1D::NNESC1D(double A,double M, double B, double ddelta, double delta, double period){
        init(A, M, B, ddelta, delta, period);
}

void NNESC1D::init(double A, double M, double B, double ddelta, double delta, double period){
        A_ = A;
        M_ = M;
        B_ = B;
        ddelta_ = ddelta;
        delta_ = delta;
        period_ = period;
        w_switch_old_ = 0;
        a_switch_old_ = A;
        yr_ = 0;
        min_peak_ = 0;
        vel_ref_ = 0;
        w_switch_ = 0;
        min_peak_detect_init_ = false;
        initialized_ = true;
}

std::vector<double> NNESC1D::step(double obj_val){
        if (!initialized_){
                fprintf(stderr,"The neural network ESC (1D) is not initialized... It will not be executed. \n");
                return std::vector<double>();
        }

        if(!min_peak_detect_init_){
                yr_ = obj_val;
                min_peak_detect_init_ = true;
        }

        double e = yr_ - obj_val;
        vel_ref_ = aSwitch(e);
        min_peak_ = minPeakDetect(-e);
        w_switch_ = wSwitch(-e);
        yr_ = yr_ + (w_switch_+min_peak_)*period_;
        std::vector<double> output;
        output.push_back(vel_ref_);
        return output;
}
double NNESC1D::wSwitch(double e_minus){
        if(e_minus<-delta_){
                w_switch_old_ = 0;
                return 0;
        }
        else if(e_minus>delta_){
                w_switch_old_ = B_;
                return B_;
        }
        else
                return w_switch_old_;

}

double NNESC1D::minPeakDetect(double e_minus){
        if(e_minus>0)
                return 0;
        else
                return -M_;
}

double NNESC1D::aSwitch(double e){
        if( e < -ddelta_ ){
                a_switch_old_ = -A_;
                return -A_;
        }
        else if(e>=ddelta_){
                a_switch_old_ = A_;
                return A_;
        }
        else
                return a_switch_old_;

}