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

/*
 * nn_esc_2d.cpp
 *
 *  Created on: Jul 31, 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 two 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_2d.h"

NNESC2D::NNESC2D(){
        M_ = 0;
        A_ = 0;
        ddelta1_ = 0;
        ddelta2_ = 0;
        ddelta2_ = 0;
        delta_ = 0;
        B_ = 0;
        w_switch_old_ = 0;
        a_switch1_old_ = 0;
        a_switch2_old_ = 0;
        a_switch3_old_ = 0;
        yr_ = 0;
        period_ = 0;
        min_peak_ = 0;
        vel_ref_.resize(2);
        vel_ref_[0] = 0;
        vel_ref_[1] = 0;
        w_switch_ = 0;
        min_peak_detect_init_ = false;
        initialized_ = false;
}

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

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

std::vector<double> NNESC2D::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> NNESC2D::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;
}

NNESC2D::NNESC2D(double A,double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period){
        init(A, M, B, ddelta1, ddelta2, ddelta3, delta, period);
}

void NNESC2D::init(double A, double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period){
        A_ = A;
        M_ = M;
        B_ = B;
        ddelta1_ = ddelta1;
        ddelta2_ = ddelta2;
        ddelta2_ = ddelta3;
        delta_ = delta;
        period_ = period;
        w_switch_old_ = 0;
        a_switch1_old_ = A_;
        a_switch2_old_ = A_;
        a_switch3_old_ = 0;
        vel_ref_.resize(2);
        vel_ref_[0] = 0;
        vel_ref_[1] = 0;
        yr_ = 0;
        min_peak_ = 0;
        w_switch_ = 0;
        min_peak_detect_init_ = false;
        initialized_ = true;
}

std::vector<double> NNESC2D::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_[1] = aSwitch1(e)+aSwitch2(e);
        vel_ref_[0] = aSwitch2(e)+aSwitch3(e);
        min_peak_ = minPeakDetect(-e);
        w_switch_ = wSwitch(-e);
        yr_ = yr_ + (w_switch_+min_peak_)*period_;
        return vel_ref_;
}
double NNESC2D::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 NNESC2D::minPeakDetect(double e_minus){
        if(e_minus>0)
                return 0;
        else
                return -M_;
}

double NNESC2D::aSwitch1(double e){
        if( e < -ddelta1_ ){
                a_switch1_old_ = -A_;
                return -A_;
        }
        else if(e>ddelta1_){
                a_switch1_old_ = A_;
                return A_;
        }
        else
                return a_switch1_old_;
}

double NNESC2D::aSwitch2(double e){
        if( e < -ddelta2_ ){
                a_switch2_old_ = 0;
                return 0;
        }
        else if(e>ddelta2_){
                a_switch2_old_ = A_;
                return A_;
        }
        else
                return a_switch2_old_;
}

double NNESC2D::aSwitch3(double e){
        if( e < -ddelta3_ ){
                a_switch3_old_ = -2*A_;
                return -2*A_;
        }
        else if(e>ddelta3_){
                a_switch3_old_ = 0;
                return 0;
        }
        else
                return a_switch3_old_;
}