Changeset 13 for trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp

Timestamp:

08/29/12 16:52:52 (12 years ago)

Author:

wcaarls

Message:

Updated extremum_seeking to revision 1017

File:

• trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp (modified) (8 diffs)

trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp

@@ -20 +20 @@
         ddelta1_ = 0;
         ddelta2_ = 0;
-        ddelta2_ = 0;
+        ddelta3_ = 0;
         delta_ = 0;
         B_ = 0;
@@ -51 +51 @@
         monitor_vals.push_back(min_peak_);
         monitor_vals.push_back(w_switch_);
+        monitor_vals.push_back(yr_+ddelta1_);
+        monitor_vals.push_back(yr_+ddelta2_);
+        monitor_vals.push_back(yr_+ddelta3_);
+        monitor_vals.push_back(yr_+delta_);
 
         return monitor_vals;
@@ -61 +65 @@
         monitor_names.push_back("minimum peak detector output");
         monitor_names.push_back("w switch value");
+        monitor_names.push_back("threshold value 1");
+        monitor_names.push_back("threshold value 2");
+        monitor_names.push_back("threshold value 3");
+        monitor_names.push_back("threshold value 4");
 
         return monitor_names;
@@ -75 +83 @@
         ddelta1_ = ddelta1;
         ddelta2_ = ddelta2;
-        ddelta2_ = ddelta3;
+        ddelta3_ = ddelta3;
         delta_ = delta;
         period_ = period;
         w_switch_old_ = 0;
         a_switch1_old_ = A_;
-        a_switch2_old_ = A_;
+        a_switch2_old_ = 0;
         a_switch3_old_ = 0;
         vel_ref_.resize(2);
@@ -99 +107 @@
 
         if(!min_peak_detect_init_){
-                yr_ = obj_val;
+                min_peak_ = obj_val;
+                yr_ = min_peak_;
                 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);
+        double s[3];
+        s[0] = aSwitch1(e);
+        s[1] = aSwitch2(e);
+        s[2] = aSwitch3(e);
+
+        vel_ref_[1] = s[0]+s[1];
+        vel_ref_[0] = s[1]+s[2];
+
+        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_){
+double NNESC2D::wSwitch(double e){
+        if(e>delta_){
                 w_switch_old_ = 0;
                 return 0;
         }
-        else if(e_minus>delta_){
+        else if(e<-delta_){
                 w_switch_old_ = B_;
                 return B_;
@@ -125 +141 @@
 }
 
-double NNESC2D::minPeakDetect(double e_minus){
-        if(e_minus>0)
+double NNESC2D::minPeakDetect(double e){
+        if(e<=0)
                 return 0;
         else
@@ -146 +162 @@
 
 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 if(e>ddelta2_){
+                a_switch2_old_ = 0;
+                return 0;
         }
         else
@@ -170 +188 @@
                 return a_switch3_old_;
 }
+
+void NNESC2D::reset(){
+        w_switch_old_ = 0;
+        a_switch1_old_ = A_;
+        a_switch2_old_ = 0;
+        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;
+}