DC Mouse 3. 아두이노 DC 모터 PID 제어

아두이노 나노로 모터 2개의 엔코더 입력에 문제없음을 확인함 (DC Mouse 2 참조)

PID 제어를 구현해본다

마이크로 마우스에서 직진과 회전 구간을 구분해서 동작하는데

직진 구간에서는 벽에 부딪히지 않도록 양쪽 바퀴의 속도 제어를 하면서 움직이고,

회전 구간에서는 정확한 각도만큼 회전할 수 있도록 모터의 각도 제어를 해야 한다.

1. 속도 제어

   PID는 에러를 측정하고, 그 에러의 양 자체(P), 에러의 누적량(I), 에러의 변화량(D)을 이용해서 제어하는 것

   따라서 무엇을 에러로 정할 것인지가 중요하다.

   속도 제어를 할 때는 당연히 에러는 속도가 된다.

   속도 측정 방법은 예전 글 참조 -> https://maxpulse.tistory.com/148 

   모터의 속도와 타이머 설정에 따라 M, T, M/T method 등을 사용해야 하는데...

   내 모터의 경우 바퀴가 한 바퀴 회전할 때 4200 펄스이고, 초당 3~4바퀴 이하로 회전하므로 초당 10000 펄스 정도

   그러면 10ms 한 번씩 측정하면 100 펄스 안팎일 것이므로 쉽게 M Method를 사용하기로 한다.

   속도 측정 후 목표 속도와 비교하여 P, I, D 게인을 적용하면 된다.

void motorPID(){    //10ms마다 호출      speedActualLeft = (leftMotorEncoder - leftMotorEncoderPrevPID)*60*(1000/LOOPTIME)/4200;  //속도를 RPM으로 측정하는 코드 : Encoder의 현재값에서 이전값을 뺀후, 측정 시간으로 나누고 Pulse 수 등을 사용해서 RPM 으로 변환한다.   LOOPTIME이 PID 제어가 동작하는 주기이다 (나의 경우는 10ms)       leftMotorEncoderPrevPID = leftMotorEncoder;     //현재 엔코더 값을 다음에 사용하도록 저장     error = (speedRequestLeft) - (speedActualLeft);    //에러량을 계산     iTermLeft += (Ki * error);                                 //I 항의 경우 누적해야 하므로 별도로 저장     pidTerm = (Kp * error) + (Kd * (error - lastErrorLeft)) + iTermLeft; //전체 PID 값을 계산     lastErrorLeft = error;                                      //현재 에러 값을 다음에 사용하도록 저장     leftControl = constrain(abs(pidTerm), 0, 255) * ((pidTerm <0)?-1:1);   //PID 텀을 모터 제어에 사용되는 값의 범위인 0~255의 값으로 변환하는 함수     LeftMotor_PID( (leftControl>=0)? 1:0, abs(leftControl));   //실제 모터의 PWM 값으로 출력하는 함수 }

 

2. 각도 제어

   에러를 각도로 사용하는 것 말고는 속도 제어 소스와 동일함

void motorPID(){    //10ms마다 호출      double ErrorAngle;     ErrorAngle = (leftMotorEncoderTarget - leftMotorEncoder)*ratio; //엔코더의 차이를 계산한 뒤 ratio를 이용하여 각도 값으로 변경     iTermLeft += (KiAngle * ErrorAngle);     pidTerm = KpAngle * ErrorAngle + iTermLeft + KdAngle*(ErrorAngle - lastErrorDegreeLeft);     lastErrorDegreeLeft = ErrorAngle;     leftControl = constrain(abs(pidTerm), 0, NormalSpeed) * ((pidTerm <0)?-1:1);     LeftMotor_PID( (leftControl>=0)? 1:0, abs(leftControl)); }

 

* 아래 소스들은 P나 PD제어만 적용한 소스이므로 I 나 D는 위의 내용 참고해서 추가하면 됨

 (어차피 PID 게인은 각자 맞춰야 함)  

속도 제어 소스 

DC_Motor_Test_P_Control_Speed.ino

0.00MB

 

각도 제어 소스 

DC_Motor_Test_P_Control_Angle.ino

0.00MB