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

Last change on this file since 5 was 5, checked in by wcaarls, 12 years ago

Imported extremum_seeking at revision 987

File size: 2.9 KB
Line 
1/*
2 * nn_esc_1d.cpp
3 *
4 *  Created on: Jul 26, 2012
5 *      Author: Berk Calli
6 *      Organization: Delft Biorobotics Lab., Delft University of Technology
7 *              Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
8 *
9 * Class for one dimensional neural network extremum seeking control
10 *
11 * * References:
12 * - M. Teixeira and S. Zak, “Analog neural nonderivative optimizers,” IEEE Transactions on Neural Networks, vol. 9, pp. 629–638, 1998.
13 * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
14 */
15#include "esc_nn/nn_esc_1d.h"
16
17NNESC1D::NNESC1D(){
18        M_ = 0;
19        A_ = 0;
20        ddelta_ = 0;
21        delta_ = 0;
22        B_ = 0;
23        w_switch_old_ = 0;
24        a_switch_old_ = 0;
25        yr_ = 0;
26        period_ = 0;
27        min_peak_ = 0;
28        vel_ref_ = 0;
29        w_switch_ = 0;
30        min_peak_detect_init_ = false;
31        initialized_ = false;
32}
33
34ESC::inputType NNESC1D::getInputType(){
35        return inputValue;
36}
37
38ESC::outputType NNESC1D::getOutputType(){
39        return outputVelocity;
40}
41
42std::vector<double> NNESC1D::monitor(){
43        std::vector<double> monitor_vals;
44        monitor_vals.push_back(yr_);
45        monitor_vals.push_back(min_peak_);
46        monitor_vals.push_back(w_switch_);
47
48        return monitor_vals;
49
50}
51
52std::vector<std::string> NNESC1D::monitorNames(){
53        std::vector<std::string> monitor_names;
54        monitor_names.push_back("driving input value");
55        monitor_names.push_back("minimum peak detector output");
56        monitor_names.push_back("w switch value");
57
58        return monitor_names;
59}
60
61NNESC1D::NNESC1D(double A,double M, double B, double ddelta, double delta, double period){
62        init(A, M, B, ddelta, delta, period);
63}
64
65void NNESC1D::init(double A, double M, double B, double ddelta, double delta, double period){
66        A_ = A;
67        M_ = M;
68        B_ = B;
69        ddelta_ = ddelta;
70        delta_ = delta;
71        period_ = period;
72        w_switch_old_ = 0;
73        a_switch_old_ = A;
74        yr_ = 0;
75        min_peak_ = 0;
76        vel_ref_ = 0;
77        w_switch_ = 0;
78        min_peak_detect_init_ = false;
79        initialized_ = true;
80}
81
82std::vector<double> NNESC1D::step(double obj_val){
83        if (!initialized_){
84                fprintf(stderr,"The neural network ESC (1D) is not initialized... It will not be executed. \n");
85                return std::vector<double>();
86        }
87
88        if(!min_peak_detect_init_){
89                yr_ = obj_val;
90                min_peak_detect_init_ = true;
91        }
92
93        double e = yr_ - obj_val;
94        vel_ref_ = aSwitch(e);
95        min_peak_ = minPeakDetect(-e);
96        w_switch_ = wSwitch(-e);
97        yr_ = yr_ + (w_switch_+min_peak_)*period_;
98        std::vector<double> output;
99        output.push_back(vel_ref_);
100        return output;
101}
102double NNESC1D::wSwitch(double e_minus){
103        if(e_minus<-delta_){
104                w_switch_old_ = 0;
105                return 0;
106        }
107        else if(e_minus>delta_){
108                w_switch_old_ = B_;
109                return B_;
110        }
111        else
112                return w_switch_old_;
113
114}
115
116double NNESC1D::minPeakDetect(double e_minus){
117        if(e_minus>0)
118                return 0;
119        else
120                return -M_;
121}
122
123double NNESC1D::aSwitch(double e){
124        if( e < -ddelta_ ){
125                a_switch_old_ = -A_;
126                return -A_;
127        }
128        else if(e>=ddelta_){
129                a_switch_old_ = A_;
130                return A_;
131        }
132        else
133                return a_switch_old_;
134
135}
Note: See TracBrowser for help on using the repository browser.