Index: /applications/pmsm/pmsm_ctrl.h
===================================================================
--- /applications/pmsm/pmsm_ctrl.h (revision 1298)
+++ /applications/pmsm/pmsm_ctrl.h (revision 1302)
@@ -174,4 +174,8 @@
 UIREGISTER(PMSM_PICtrl);
 
+/*************************************/
+/*			LQ alpa-beta		  */
+/*************************************/
+
 class PMSM_LQCtrl: public PMSMCtrl{
 public:
@@ -214,5 +218,6 @@
 	
 //control maximum
-	const double MAXu;
+	double MAXu;
+	int MAXuflag;
 	
 //lqg controler class
@@ -225,5 +230,5 @@
 				r(0.005), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
 				A(5, 5), B(5, 2), Q(5, 5), R(2, 2),
-				uab(2), icond(6), MAXu(100.0)	{			
+				uab(2), icond(6), MAXu(100.0), MAXuflag(0)	{			
 		//set fix matrices elements & dimensions
 		A.zeros();
@@ -294,17 +299,30 @@
 		vec tmp = L*icond;		    
 		
-		uab = tmp(0,1);
-		
-		if(uab(0) > MAXu) uab(0) = MAXu;
-		else if(uab(0) < -MAXu) uab(0) = -MAXu;
-		if(uab(1) > MAXu) uab(1) = MAXu;
-		else if(uab(1) < -MAXu) uab(1) = -MAXu;
+		uab = tmp(0,1);		
+		
+		if(MAXuflag == 1){ //square cut off
+			if(uab(0) > MAXu) uab(0) = MAXu;
+			else if(uab(0) < -MAXu) uab(0) = -MAXu;
+			if(uab(1) > MAXu) uab(1) = MAXu;
+			else if(uab(1) < -MAXu) uab(1) = -MAXu;
+		}
+		else if(MAXuflag == 2){ //circular cut off
+			double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
+			double uangl = atan2(uab(1), uab(0));
+			if (uampl > MAXu) uampl = MAXu;
+			uab(0) = uampl*cos(uangl);
+			uab(1) = uampl*sin(uangl);
+		}
+		//else{ //(MAXuflag == 0) //no cut off }
 		
 		return uab;
-	};
+	}
+	
 	void from_setting(const Setting &set){
 		PMSMCtrl::from_setting(set);
 		UI::get(r,set, "r", UI::optional);
 		UI::get(rec_hor,set, "h", UI::optional);
+		UI::get(MAXu,set, "MAXu", UI::optional);
+		UI::get(MAXuflag,set, "MAXuflag", UI::optional);
 	}
 
@@ -351,4 +369,8 @@
 UIREGISTER(PMSM_LQCtrl);
 
+/*************************************/
+/*			LQ d-q 1			  */
+/*************************************/
+
 class PMSM_LQCtrl_dq: public PMSMCtrl{
 public:
@@ -371,4 +393,5 @@
 //input penalty
 	double r;
+	double rpd; //penalize differences u - u_old
 	
 //time step
@@ -383,13 +406,16 @@
 	//mat C; //2x5
 	mat Q; //5x5
-	mat R; //2x2	
+	mat R; //2x2
+	mat Rpd; //2x4
 	
 //control matrix
 	mat L;
-	vec uab;
-	vec icond;
+	vec uab;	
+	vec icondpd;
+	vec u_old;
 	
 //control maximum
-	const double MAXu;
+	double MAXu;
+	int MAXuflag;
 	
 //lqg controler class
@@ -400,7 +426,7 @@
 		
 	PMSM_LQCtrl_dq():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
-				r(0.005), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
-				A(5, 5), B(5, 2), Q(5, 5), R(2, 2),
-				uab(2), icond(6), MAXu(100.0)	{			
+				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
+				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
+				uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)	{			
 		//set fix matrices elements & dimensions
 		A.zeros();
@@ -413,11 +439,10 @@
 			A(3, 3) = A(4, 4) = 1.0;
 		B.zeros();
-			B(0, 0) = B(1, 1) = c;
-		//C.zeros();
-		//	C(0, 0) = C(1, 1) = 1.0;
+			B(0, 0) = B(1, 1) = c;		
 		Q.zeros();
 			Q(2, 2) = 1.0;
 		R.zeros();
-								
+		Rpd.zeros();
+		u_old.zeros();						
 	}
 		
@@ -425,34 +450,23 @@
 	virtual vec ctrlaction(const itpp::vec& cond) {
 		PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
-		
-		int i;
+				
 		vec udq;
+		vec tmp;
+		
 		lq.resetTime();
-		
-		//create prediction
-		/*p_isa.zeros();
-		p_isb.zeros();
-		p_ome.zeros();
-		p_the.zeros();
-		p_isa(0) = isa;
-		p_isb(0) = isb;
-		p_ome(0) = ome;
-		p_the(0) = the;
-				
-		//create control matrix
-		/*for(i = rec_hor; i > 0; i--){								
-			lq.redesign();
-		}
-		lq.redesign();
-		*/
+				
 		L = lq.getL();
-		icond(0) = isa*cos(the) + isb*sin(the);
-		icond(1) = isb*cos(the) - isa*sin(the);
-		icond(2) = ome - Ww;
-		icond(3) = the;
-		icond(4) = Ww;
-		icond(5) = 1;
-		vec tmp = L*icond;		    
-		
+		
+		icondpd(0) = isa*cos(the) + isb*sin(the);
+		icondpd(1) = isb*cos(the) - isa*sin(the);
+		icondpd(2) = ome - Ww;
+		icondpd(3) = the;
+		icondpd(4) = Ww;
+		icondpd(5) = u_old(0);
+		icondpd(6) = u_old(1);
+		icondpd(7) = 1;	
+			
+		tmp = L*icondpd;		
+						
 		udq = tmp(0,1);
 		
@@ -461,20 +475,38 @@
 		uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
 		
-		if(uab(0) > MAXu) uab(0) = MAXu;
-		else if(uab(0) < -MAXu) uab(0) = -MAXu;
-		if(uab(1) > MAXu) uab(1) = MAXu;
-		else if(uab(1) < -MAXu) uab(1) = -MAXu;
+		if(MAXuflag == 1){ //square cut off
+			if(uab(0) > MAXu) uab(0) = MAXu;
+			else if(uab(0) < -MAXu) uab(0) = -MAXu;
+			if(uab(1) > MAXu) uab(1) = MAXu;
+			else if(uab(1) < -MAXu) uab(1) = -MAXu;
+		}
+		else if(MAXuflag == 2){ //circular cut off
+			double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
+			double uangl = atan2(uab(1), uab(0));
+			if (uampl > MAXu) uampl = MAXu;
+			uab(0) = uampl*cos(uangl);
+			uab(1) = uampl*sin(uangl);
+		}
+		//else{ //(MAXuflag == 0) //no cut off }
+		
+		u_old = uab;
 		
 		return uab;
-	};
+	}
+	
 	void from_setting(const Setting &set){
 		PMSMCtrl::from_setting(set);
 		UI::get(r,set, "r", UI::optional);
 		UI::get(rec_hor,set, "h", UI::optional);
+		UI::get(MAXu,set, "MAXu", UI::optional);
+		UI::get(MAXuflag,set, "MAXuflag", UI::optional);
+		UI::get(rpd,set, "rpd", UI::optional);		
 	}
 
 	void validate(){
-		R(0,0)=R(1,1)=r;
-
+		R(0, 0) = R(1, 1) = r;
+		Rpd(0, 0) = Rpd(1, 1) = rpd;
+		Rpd(0, 2) = Rpd(1, 3) = -rpd;
+				
 		p_isa.set_length(rec_hor+1);
 		p_isb.set_length(rec_hor+1);
@@ -482,14 +514,19 @@
 		p_the.set_length(rec_hor+1);
 
-		Array<quadraticfn> qloss(2);
+		Array<quadraticfn> qloss(3);
 		qloss(0).Q.setCh(Q);
-		qloss(0).rv = RV("x", 5, 1);
+		qloss(0).rv = RV("x", 5, 1);		
 		qloss(1).Q.setCh(R);
-		qloss(1).rv = RV("u", 2, 0);		
+		qloss(1).rv = RV("u", 2, 0);
+		qloss(2).Q.setCh(Rpd);		
+		qloss(2).rv = RV("u", 2, 0);
+		qloss(2).rv.add(RV("u", 2, -1));		
 		lq.Losses = qloss;		
 
-				//set lq
-		lq.rv = RV("u", 2, 0);			
-		lq.set_rvc(RV("x", 5, 0));
+		//set lq
+		lq.rv = RV("u", 2, 0);	
+		RV tmp = RV("x", 5, 0);
+		tmp.add(RV("u", 2, -1));
+		lq.set_rvc(tmp);		
 		lq.horizon = rec_hor;	
 		
@@ -507,9 +544,9 @@
 		lq.finalLoss.Q.setCh(Q);
 		lq.finalLoss.rv = RV("x", 5, 1);
-		
+				
 		lq.validate();
 						
 		uab.zeros();
-		
+				
 		//create control matrix
 		for(int i = rec_hor; i > 0; i--){								
@@ -520,4 +557,8 @@
 };
 UIREGISTER(PMSM_LQCtrl_dq);
+
+/*************************************/
+/*			LQ d-q 2			  */
+/*************************************/
 
 class PMSM_LQCtrl_dq2: public PMSMCtrl{
@@ -541,4 +582,5 @@
 //input penalty
 	double r;
+	double rpd; //penalize differences u - u_old
 	
 //time step
@@ -553,13 +595,16 @@
 	//mat C; //2x5
 	mat Q; //5x5
-	mat R; //2x2	
+	mat R; //2x2
+	mat Rpd; //2x4	
 	
 //control matrix
 	mat L;
 	vec uab,udq;
-	vec icond;
+	vec icondpd;
+	vec u_old;
 	
 //control maximum
-	const double MAXu;
+	double MAXu;
+	int MAXuflag;
 	
 //lqg controler class
@@ -570,7 +615,7 @@
 		
 	PMSM_LQCtrl_dq2():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
-				r(0.005), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
-				A(5, 5), B(5, 2), Q(5, 5), R(2, 2),
-				uab(2), udq(2), icond(6), MAXu(100.0)	{			
+				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
+				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
+				uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)	{			
 		//set fix matrices elements & dimensions
 		A.zeros();
@@ -582,11 +627,10 @@
 			A(3, 3) = A(4, 4) = 1.0;
 		B.zeros();
-			B(0, 0) = B(1, 1) = c;
-		//C.zeros();
-		//	C(0, 0) = C(1, 1) = 1.0;
+			B(0, 0) = B(1, 1) = c;		
 		Q.zeros();
 			Q(2, 2) = 1.0;
 		R.zeros();
-								
+		Rpd.zeros();
+		u_old.zeros();						
 	}
 		
@@ -596,7 +640,8 @@
 		
 		int i;
+		vec tmp;
 		
 		lq.resetTime();
-	//cout << "OK" << endl;
+	
 		//create prediction
 		p_isd.zeros();
@@ -608,30 +653,15 @@
 		p_ome(0) = ome;
 		p_the(0) = the;
-	//cout << "OKkkk" << endl;	
-		for(i = 0; i < rec_hor-1; i++){
-			/*p_isa(i+1) = a*p_isa(i) + b*p_ome(i)*sin(p_the(i)) + c*0;//uab(0); //use last used control
-			p_isb(i+1) = a*p_isb(i) - b*p_ome(i)*cos(p_the(i)) + c*0;//uab(1);
-			p_ome(i+1) = d*p_ome(i) + e*(p_isb(i)*cos(p_the(i)) - p_isa(i)*sin(p_the(i)));
-			p_the(i+1) = p_the(i) + Dt*p_ome(i);*/
-			p_isd(i+1) = a*p_isd(i) + p_isq(i)*p_ome(i)*Dt + c*udq(0);
-			//cout << "OKaa" << endl;
+		
+		for(i = 0; i < rec_hor-1; i++){			
+			p_isd(i+1) = a*p_isd(i) + p_isq(i)*p_ome(i)*Dt + c*udq(0);			
 			p_isq(i+1) = -p_isd(i)*p_ome(i)*Dt + a*p_isq(i) - b*p_ome(i) + c*udq(1);
 			p_ome(i+1) = d*p_ome(i) + e*p_isq(i);
-			p_the(i+1) = p_the(i) + Dt*p_ome(i);
-			//cout << "1";
+			p_the(i+1) = p_the(i) + Dt*p_ome(i);			
 		}
 		
 		//create control matrix
 		for(i = rec_hor; i > 0; i--){		
-			//set variable matrices elements (A matrix only)
-			/*A(0, 2) = b*sin(p_the(i));
-			A(0, 3) = b*(p_ome(i))*cos(p_the(i));
-			A(0, 4) = b*sin(p_the(i));
-			A(1, 2) = -b*cos(p_the(i));
-			A(1, 3) = b*(p_ome(i))*sin(p_the(i));
-			A(1, 4) = -b*cos(p_the(i));
-			A(2, 0) = -e*sin(p_the(i));
-			A(2, 1) = e*cos(p_the(i));
-			A(2, 3) = -e*(p_isa(i)*cos(p_the(i)) + p_isb(i)*sin(p_the(i)));*/
+			//set variable matrices elements (A matrix only)			
 			A(0, 1) = Dt*p_ome(i);
 			A(0, 2) = Dt*p_isq(i);
@@ -647,15 +677,16 @@
 		
 		L = lq.getL();
-		//icond(0) = isa;
-		//icond(1) = isb;
-		icond(0) = isa*cos(the) + isb*sin(the);
-		icond(1) = isb*cos(the) - isa*sin(the);
-		icond(2) = ome - Ww;
-		icond(3) = the;
-		icond(4) = Ww;
-		icond(5) = 1;
-		vec tmp = L*icond;		    
-		
-		//uab = tmp(0,1);
+		
+		icondpd(0) = isa*cos(the) + isb*sin(the);
+		icondpd(1) = isb*cos(the) - isa*sin(the);
+		icondpd(2) = ome - Ww;
+		icondpd(3) = the;
+		icondpd(4) = Ww;
+		icondpd(5) = u_old(0);
+		icondpd(6) = u_old(1);
+		icondpd(7) = 1;
+			
+		tmp = L*icondpd;			
+				
 		udq = tmp(0,1);
 		
@@ -664,20 +695,38 @@
 		uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
 		
-		if(uab(0) > MAXu) uab(0) = MAXu;
-		else if(uab(0) < -MAXu) uab(0) = -MAXu;
-		if(uab(1) > MAXu) uab(1) = MAXu;
-		else if(uab(1) < -MAXu) uab(1) = -MAXu;
+		if(MAXuflag == 1){ //square cut off
+			if(uab(0) > MAXu) uab(0) = MAXu;
+			else if(uab(0) < -MAXu) uab(0) = -MAXu;
+			if(uab(1) > MAXu) uab(1) = MAXu;
+			else if(uab(1) < -MAXu) uab(1) = -MAXu;
+		}
+		else if(MAXuflag == 2){ //circular cut off
+			double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
+			double uangl = atan2(uab(1), uab(0));
+			if (uampl > MAXu) uampl = MAXu;
+			uab(0) = uampl*cos(uangl);
+			uab(1) = uampl*sin(uangl);
+		}
+		//else{ //(MAXuflag == 0) //no cut off }
+		
+		u_old = uab;
 		
 		return uab;
-	};
+	}
+	
 	void from_setting(const Setting &set){
 		PMSMCtrl::from_setting(set);
 		UI::get(r,set, "r", UI::optional);
 		UI::get(rec_hor,set, "h", UI::optional);
+		UI::get(MAXu,set, "MAXu", UI::optional);
+		UI::get(MAXuflag,set, "MAXuflag", UI::optional);
+		UI::get(rpd,set, "rpd", UI::optional);
 	}
 
 	void validate(){
-		R(0,0)=R(1,1)=r;
-
+		R(0, 0) = R(1, 1) = r;
+		Rpd(0, 0) = Rpd(1, 1) = rpd;		
+		Rpd(0, 2) = Rpd(1, 3) = -rpd;		
+		
 		p_isd.set_length(rec_hor+1);
 		p_isq.set_length(rec_hor+1);
@@ -685,14 +734,20 @@
 		p_the.set_length(rec_hor+1);
 
-		Array<quadraticfn> qloss(2);
+		Array<quadraticfn> qloss(3);
 		qloss(0).Q.setCh(Q);
-		qloss(0).rv = RV("x", 5, 1);
+		qloss(0).rv = RV("x", 5, 1);		
 		qloss(1).Q.setCh(R);
-		qloss(1).rv = RV("u", 2, 0);		
+		qloss(1).rv = RV("u", 2, 0);
+		qloss(2).Q.setCh(Rpd);
+		qloss(2).rv = RV("u", 2, 0);
+		qloss(2).rv.add(RV("u", 2, -1));		
 		lq.Losses = qloss;		
 
-				//set lq
-		lq.rv = RV("u", 2, 0);			
-		lq.set_rvc(RV("x", 5, 0));
+		//set lq
+		lq.rv = RV("u", 2, 0);		
+		RV tmp = RV("x", 5, 0);
+		tmp.add(RV("u", 2, -1));	
+		lq.set_rvc(tmp);
+		
 		lq.horizon = rec_hor;	
 		
@@ -710,10 +765,9 @@
 		lq.finalLoss.Q.setCh(Q);
 		lq.finalLoss.rv = RV("x", 5, 1);
-		
+				
 		lq.validate();
 						
 		uab.zeros();
-		udq.zeros();
-
+		udq.zeros();		
 	}
 };
