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

Timestamp:

01/03/13 15:27:56 (11 years ago)

Author:

wcaarls

Message:

Updated extremum_seeking to revision 1177

File:

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

trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp

@@ -31 +31 @@
         min_peak_ = 0;
         vel_ref_.resize(2);
+        vel_ref_old_.resize(2);
         vel_ref_[0] = 0;
         vel_ref_[1] = 0;
@@ -73 +74 @@
 }
 
-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){
+NNESC2D::NNESC2D(double A,double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period, int stopping_cycle_number, double stoping_min_val){
+        init(A, M, B, ddelta1, ddelta2, ddelta3, delta, period, stopping_cycle_number, stoping_min_val);
+}
+
+void NNESC2D::init(double A, double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period, int stopping_cycle_number, double stoping_min_val){
         A_ = A;
         M_ = M;
@@ -86 +87 @@
         delta_ = delta;
         period_ = period;
-        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;
+        reset();
         initialized_ = true;
+        stopping_cycle_number_ = stopping_cycle_number;
+        stoping_min_val_ = stoping_min_val;
 }
 
@@ -110 +103 @@
                 yr_ = min_peak_;
                 min_peak_detect_init_ = true;
+                obj_val_cycle_init_ = obj_val;
+                vel_ref_old_[0] = vel_ref_[0];
+                vel_ref_old_[1] = vel_ref_[1];
         }
 
@@ -124 +120 @@
         w_switch_ = wSwitch(e);
         yr_ = yr_ + (w_switch_+min_peak_)*period_;
+
+        if((vel_ref_old_[0] != vel_ref_[0] || vel_ref_old_[1] != vel_ref_[1]) && vel_ref_[1] == A_ && vel_ref_[0] == 0){
+                if(obj_val_cycle_init_ - obj_val < stoping_min_val_){
+                        nn_cycle_count_++;
+                }
+                else
+                        nn_cycle_count_ = 0;
+                obj_val_cycle_init_ = obj_val;
+        }
+        //printf("[nn_esc_2d]: nn_cycle_count = %d \n",nn_cycle_count_);
+        vel_ref_old_[0] = vel_ref_[0];
+        vel_ref_old_[1] = vel_ref_[1];
 
         return vel_ref_;
@@ -197 +205 @@
         vel_ref_[0] = 0;
         vel_ref_[1] = 0;
+        vel_ref_old_.resize(2);
+        vel_ref_old_[0] = 0;
+        vel_ref_old_[1] = 0;
         yr_ = 0;
         min_peak_ = 0;
         w_switch_ = 0;
+        nn_cycle_count_ = 0;
         min_peak_detect_init_ = false;
-        initialized_ = true;
-}
+}
+
+bool NNESC2D::isStoppingConditionsMet(){
+        if(stopping_cycle_number_ <= 0)
+                return false;
+        else if(nn_cycle_count_ > stopping_cycle_number_){
+                return true;
+        }
+        else
+                return false;
+}