Index: trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h
===================================================================
--- trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h	(revision 9)
+++ trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h	(revision 13)
@@ -47,4 +47,5 @@
 	double minPeakDetect(double e_minus);
 	double aSwitch(double e);
+	void reset();
 
 };
Index: trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h
===================================================================
--- trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h	(revision 9)
+++ trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h	(revision 13)
@@ -25,5 +25,5 @@
 
 protected:
-	double A_,M_,ddelta1_,ddelta2_,ddelta3_,A,delta_,B_, mpd_init_, w_switch_old_, a_switch1_old_, a_switch2_old_, a_switch3_old_,yr_,period_;
+	double A_,M_,ddelta1_,ddelta2_,ddelta3_,delta_,B_, mpd_init_, w_switch_old_, a_switch1_old_, a_switch2_old_, a_switch3_old_,yr_,period_;
 	double min_peak_,w_switch_;
 	std::vector<double> vel_ref_;
@@ -42,7 +42,8 @@
 	std::vector<double> monitor();
 	std::vector<std::string> monitorNames();
+	void reset();
 protected:
-	double wSwitch(double e_minus);
-	double minPeakDetect(double e_minus);
+	double wSwitch(double e);
+	double minPeakDetect(double e);
 	double aSwitch1(double e);
 	double aSwitch2(double e);
Index: trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp
===================================================================
--- trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp	(revision 9)
+++ 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 9)
+++ 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;
+}