PID 제어

Posted 2010. 2. 18. 16:13

제어에 쓰이는 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;

'Enginius > Firmware' 카테고리의 다른 글

PIC32 - PWM  (0) 2010.02.19
PIC32 - Input Capture  (0) 2010.02.19
PID 제어  (1) 2010.02.18
PIC32 - I2C  (0) 2010.02.17
PIC32 - GPIO  (0) 2010.02.17
PIC32 - Interrupt  (0) 2010.02.17
  1. researcher

    | 2017.02.10 15:27 | PERMALINK | EDIT | REPLY |

    PID제어 관련하여 자료를 조사하던 중에 들어오게 되었는데 많은 도움되었습니다!! 추가로 궁금한 것이 있어 질문 남깁니다... pid제어의 결과값(OutPreSat)이 PWM에 대입되는 것이 의문이 들어서요..ㅎㅎ
    pid제어할때 feedback받는 값들은 속도나 각도, 온도 등으로 다양하더라고요. 이러한 입력값들로부터 계산된 PID 결과값을 바로 PWM에 대입을 하면 되는것인가요? 속도와 PWM의 관계식 같은 것들은 고려되지않아도 되는건가요?.?

Write your message and submit
« PREV : 1 : ··· : 6 : 7 : 8 : 9 : 10 : 11 : 12 : 13 : 14 : ··· : 17 : NEXT »