PID 제어

제어에 쓰이는 PID
개념은 너무나 쉽기 때문에 설명하지 않겠다.
게인을 잡는 것은 너무 어렵기 때문에 그것도 설명하지 않겠다.

그냥 경험상 P게인 잡고, D게인 잡고, I게인 잡고, 다시 반복하면 된다.
스텝 리스펀스를 이용해서 잡으면 된다.

헤더 파일

typedef struct {	float  Ref;   			// Input: Reference input 
			float  Fdb;   		// Input: Feedback input 
			float  Err;			// Variable: Error
			float	 ErrSum;		// Variable: Error Sum
			float  Kp;			// Parameter: Proportional gain
			float  Up;			// Variable: Proportional output 
			float  Ui;			// Variable: Integral output 
			float  Ud;			// Variable: Derivative output 	
			float  OutPreSat; 		// Variable: Pre-saturated output
			float  OutMax;		// Parameter: Maximum output 
			float  OutMin;	    	// Parameter: Minimum output
			float  Out;   		// Output: PID output 
			float  iMax;		// Parameter: Maximum Integration
			float  Ki;			// Parameter: Integral gain				  
			float  Kd; 		        // Parameter: Derivative gain
			float  ErrPrev;	   	// History: Previous error				
	 	 	void  (*calc)();	  	// Pointer to calculation function
			} PIDREG3;	 				            
typedef PIDREG3 *PIDREG3_handle;

#define PWM_MIN		1500 


// 20ms PID 제어주기 
// 속도 제어 PID 게인  
#define L_Kp 	33.0		//30.0		//30.0		
#define L_Ki 	6.0		//5.0		//8.0
#define L_Kd 	0
#define L_OutMax 	4900.0
#define L_OutMin 	-4900.0
#define L_iMax 	4900.0

void pid_reg3_calc_v(PIDREG3_handle);

#define PIDREG3_V_LEFT     { 0.0, 0.0, 0.0, 0.0, L_Kp, 0.0, 0.0, 0.0, 0.0, \
                           L_OutMax, L_OutMin, 0.0, L_iMax, L_Ki, L_Kd, 0.0, \
                           (void (*)(Uint32))pid_reg3_calc_v }

 

소스 파일

void pid_reg3_calc_v(PIDREG3 *v)
{
	float outpresave;
	
	// Compute the error
	v->Err = v->Ref - v->Fdb;

	// Compute the error sum
	v->ErrSum = v->ErrSum + v->Err;

	// Compute the proportional output
	v->Up = v->Kp * v->Err;

	// Compute the integral output
	v->Ui = v->Ki*v->ErrSum;
	if(v->Ui > v->iMax) 			v->Ui = v->iMax;
	else if(v->Ui < (-1.0 * v->iMax) ) 	v->Ui = -1.0 * v->iMax;

	// Compute the derivative output
	v->Ud = v->Kd * (v->Err - v->ErrPrev);

	// Compute the pre-saturated output
	v->OutPreSat = v->Up + v->Ui + v->Ud;
	outpresave = v->OutPreSat;

	// PWM 값이 20KHz에서 0 ~ 4000 까지의 값을 가질 수 있지만
	// 최소 어느 정도의 값을 줘야 실제 로봇(O2)의 바퀴가
	// 천천히 돌기 시작하기 때문에 다음과 같이 한다.
	if(v->OutPreSat < PWM_MIN && v->OutPreSat > 0.0 && v->Ref!=0) 		v->OutPreSat = PWM_MIN;
	else if(v->OutPreSat > -PWM_MIN && v->OutPreSat < 0.0 && v->Ref!=0)	v->OutPreSat = -PWM_MIN;

	// Saturate the output
	if(v->OutPreSat > v->OutMax)	v->Out =  v->OutMax;
	else if(v->OutPreSat < v->OutMin)	v->Out =  v->OutMin;
	else 						v->Out = v->OutPreSat;
	v->ErrPrev = v->Err;

//	myprintf("Vcalc: ");
//	myprintf("Kp: %d   Ki: %d   Kd: %d \r\n", (int)v->Kp, (int)v->Ki, (int)v->Kd );
//	myprintf("Ref: %d   Fdb: %d   Err: %d   ErrSum: %d   Outpre: %d(%d+%d+%d)   Out: %d  \r\n", (int)(v->Ref), (int)(v->Fdb), (int)(v->Err), (int)(v->ErrSum), (int)(outpresave), (int)(v->Up), (int)(v->Ui), (int)(v->Ud), (int)(v->Out) );
}

 

실제 사용

PIDREG3 pidVR = PIDREG3_V_RIGHT;
PIDREG3 pidVL = PIDREG3_V_LEFT;

		{
			........
			pidVR.Fdb = (float)(g_EncoderR - Encoder_R_prev);
			pidVL.Fdb = (float)(g_EncoderL - Encoder_L_prev);
			pidVR.calc(&pidVR);
			pidVL.calc(&pidVL);		
			rightS = (int32)pidVR.Out;
			leftS = (int32)pidVL.Out;
			SetDriveDirection(leftS, rightS);
			mR_PWM(rightS);
			mL_PWM(leftS);							
		}
		Encoder_R_prev = g_EncoderR;
		Encoder_L_prev = g_EncoderL;