[Line Tracer] 지능형 모형차 경진대회 2008 - PID 제어부

지난번에 올린 센서 인터럽트와 함께 사용되는 이벤트 이다.

대회당시에는 1kHz로 동작하도록 하였는데, 나름 최적의 제어주기라고 생각한다(ㅡㅡ;;)근거는... 마땅하지 않지만...

지난번과 마찬가지로 중요한 부분엔 모두 주석이 있으므로 크게 문제가 없을 것이라고 생각한다.

void Timer_OnInterrupt(void)

{

  //DC Motor

  long P, I, D;

  int PID;

//  Debug_Pin_PutVal(1); // 디버그용으로 사용

  Light_Sensor_PutVal(1); // 발광 센서 켜기

  //엔코더 값 읽기 현재는 A상만 읽음.

  encCnt = Enc_A_GetCounterValue();

//  Enc_A_ResetCounter();

 

//  LineView_PutVal(time_ms);

LineView_PutVal(0x16);

  motor.kp = P_VALUE;

  motor.ki = I_VALUE;

  motor.kd = D_VALUE;

  motor.error = USER_SPEED - encCnt;

  motor.errorIntegral += motor.error;

 

  if (motor.errorIntegral > 255)

      motor.errorIntegral = 255;

  else if (motor.errorIntegral < -255)

      motor.errorIntegral = -255;

  P = motor.error * motor.kp;

  I = motor.errorIntegral * motor.ki;

  D = (motor.error - motor.errorBefore) * motor.kd;

  PID = P;

  PID += I;

  PID += D;

  motor.errorBefore = motor.error;

  if (PID > 255)

    PID = 255;

  else if (PID < -255)

    PID = -255;

  if(PID > 0 )

  {

    (void) Motor_B_Disable();

    (void) Motor_A_Enable();  

  (void) Motor_A_SetRatio8(255 - PID);

  }

  else

  {

    (void) Motor_A_Disable();

    (void) Motor_B_Enable();

    (void) Motor_B_SetRatio8(255 + PID);

  }

  //  delay(0x0014);

  (void)Sensor_Start();  

}