Index: trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp
===================================================================
--- trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp	(revision 5)
+++ trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp	(revision 13)
@@ -134,2 +134,12 @@
 
 }
+
+void NNESC1D::reset(){
+	w_switch_old_ = 0;
+	a_switch_old_ = A;
+	yr_ = 0;
+	min_peak_ = 0;
+	vel_ref_ = 0;
+	w_switch_ = 0;
+	min_peak_detect_init_ = false;
+}
Index: trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp
===================================================================
--- trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp	(revision 5)
+++ trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp	(revision 13)
@@ -20,5 +20,5 @@
 	ddelta1_ = 0;
 	ddelta2_ = 0;
-	ddelta2_ = 0;
+	ddelta3_ = 0;
 	delta_ = 0;
 	B_ = 0;
@@ -51,4 +51,8 @@
 	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,4 +65,8 @@
 	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,10 +83,10 @@
 	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,22 +107,30 @@
 
 	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,6 +141,6 @@
 }
 
-double NNESC2D::minPeakDetect(double e_minus){
-	if(e_minus>0)
+double NNESC2D::minPeakDetect(double e){
+	if(e<=0)
 		return 0;
 	else
@@ -146,11 +162,13 @@
 
 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,2 +188,17 @@
 		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;
+}