Điều khiển động cơ DC servo bằng thuật toán PID với ATMEGA8 (PID vận tốc, PID vị trí)

-Điều khiển động cơ DC là một điều gần như luôn được sử dụng trong Robot cũng như trong công nghiệp hiện nay, việc điều khiển động cơ đòi hỏi có độ chính xác cao, vì với một sai lệch nhỏ sẽ ảnh hưởng đến đường đi của Robot. Vậy để làm thế nào mà có thể điều khiển động cơ một cách chính xác nhất, để sai số của nó là nhỏ nhất???  

-Các bạn đã từng nghe đến giải thuật PID(tỉ lệ, tích phân, đạo hàm) chưa nhỉ, nếu bạn nào từng học qua môn lý thuyết điều khiển chắc hẳn sẽ biết giải thuật này. Lấy cơ sở từ bộ phản hồi trong luật điều khiển cùng với các cách khác nhau để tìm ra các hệ số Kp, Kd, Ki...(với mình thường sử dụng phương pháp đối xứng vì tính đơn giản và hiệu quả khá cao)  

-Chúng ta sẽ đi sâu hơn về phương pháp này: 

+ Lấy một ví dụ đơn giản: Chúng ta muốn điều khiển một chiếc xe đi từ A đến B cần 1 lực F để điều khiển, câu hỏi đặt ra là cần lực F thay đổi như thế nào để xe đến vị trí B một cách chính xác và nhanh nhất ???   

+Gọi khoảng cách giữa xe và điểm B là Err(Error), Err càng lớn, đòi hỏi lực F càng lớn. Chúng ta có 1 tỷ lệ thuật F=Kp*Err, với Kp là một hằng số.

+Khi xe đến B, Err=0 nên lực F=0, tuy nhiên do quán tính xe sẽ lệch khỏi B, sai số Err<0, vì thế cần 1 lực F kéo ngược lại, tương tự thế khi xe lại lùi về trước điểm B, quá trình này cứ lặp đi lặp lại và có thể xe sẽ xa dần điểm B      Vì vậy lúc này cần 1 cái "phanh" để xe đi gần vị trí B sẽ giảm dần tốc độ so vs ở xa. Vì ở gần B thì vận tốc là max, tức là dErr/dt =max, Err sẽ thay đổi mạnh nhất. Như vậy đạo hàm của sai số Err tăng giá trị nhưng ngược chiều của lực F, nếu sử dụng đạo hàm làm thành phần phanh thì có thể giảm dc đáng kể sai số lệch tĩnh khỏi vị trí B   (tìm hiểu kĩ sai số lệch tĩnh ở lý thuyết điều khiển) và ta có F=Kp*Err + Kd*(dErr/dt)      

+Sự có mặt của thành phần D làm giảm được sai lệch tĩnh của xe, khi xe tiến gần về B, lực F gồm 2 thành phần Kp*Err > =0 (P) và Kd*(dErr/dt) <=0. Trong một số trường hợp thành phần D có giá trị lớn hơn thành phần P và lực F đổi chiều, “phanh” xe lại, vận tốc của xe vì thế giảm mạnh ở gần điểm B. Một vấn đề ở đây là nếu thành phần D quá lớn so với thành phần P hoặc bản thân thành phần P nhỏ thì khi xe tiến gần điểm B (chưa thật sự đến B), xe có thể dừng hẳn, thành phần D bằng 0 (vì sai số Err không thay đổi nữa), lực F = Kp*Err. Lúc này cả thành phần D vs P đều nhỏ và có thể ko thắng dc lực ma sát tĩnh, chúng ta cần 1 thành phần cộng dồn để đẩy xe lại gần B hơn, đó ko gì khác ngoài thành phần I (tích phân ~ cộng dồn), lúc này ta có  F=Kp*Err + Kd*(dErr/dt)+Ki*∫Errdt .

=>>>>>>>>Đối với điều khiển số, đòi hỏi quá trình lấy mẫu nhanh (dt rất nhỏ) chúng ta tuyến tính hóa với thời gian lấy mẫu h sẽ dc pPart=Kp*Err, dPart=Kd*(Err-old_Err)/h và iPart=iPart(ở thời điểm lấy mẫu trước :h(k-1))+Err*h.

 Trên đây là hình ảnh về cấu tạo của bộ Encoder gồm 2 kênh A và B

Kênh A, B nhận dc tín hiệu (A, B ở mức thấp), và ngược lại (ở mức cao)

-Từ hình vẽ, chọn chiều dương ngược chiều kim đồng hồ, khi quay theo chiều dương Kênh A có 1 cạnh xuống thì kênh B đang ở mức cao, bằng cách kiểm tra kênh B để biết dc chiều động cơ, và ngược lại khi quay theo chiều âm nếu kênh A cạnh xuống thì kênh B đang ở mức thấp.

-Ý tưởng để điều khiển kênh A: 

+Dùng input capture: Sau mỗi lần chân ICP trên AVR cạnh lên hoặc cạnh xuống thì giá trị dc gán cho thanh ghi ICR, so sánh 2 lần liên tiếp ta dc chu kì của xung kênh A, từ đó tính vận tốc động cơ DC, nhưng thường thì input capture chỉ có ở timer1 (đối vs mấy co avr ko có timer 3, timer 3 thường từ atmega 64 trở lên-hơi đắt), mà timer 1 dùng để điều khiển PWM ở động cơ nên ko thể dùng input capture cho đo xung encoder dc

+Dùng counter (đơn giản) nhưng lại thường ko xác định dc chiều quay của động cơ >>>>ko dùng

+Dùng ngắt ngoài (đơn giản), dễ hiểu là khi có 1 cạnh xuống ở kênh A (ối vs INT0 hoặc INT1) thì kiểm tra chân nối kênh B để xác định chiều và mỗi lần như thế thì xung tăng thêm 1 >>>quyết định dùng ngắt ngoài  

//Dưới đây mình viết mẫu đoạn code cho điều khiển PID vận tốc và PID vị trí (đã test trên mạch thật)

//PID vận tốc

void Pid_control(float v1)  //v1 la van toc dieu khien, v2 la van toc hien tai

{

//chon chieu duong la chieu nguoc chieu kim dong ho, khi v2>0 tuong ung dir=1 va nguoc lai

     Speed=Pulse-old_Pulse;

     old_Pulse=Pulse;

     if(Speed<0)

     {

         Speed=-Speed; 

//         DIR=0;

     }

//     else DIR=1;

     Err=(float)(v1-Speed);   //tinh sai so

      //cac thanh phan PID

     pPart=Kp*Err;

     dPart=Kd*(Err-old_Err)*inv_Sampling_time;

     iPart+=Ki*Sampling_time*Err/1000.0; //tinh Sampling_time theo s

     Output+=(pPart+dPart+iPart);

//     Output=(unsigned int)Output;

     if(Output>=period) Output=period-1;

     else if(Output<=0.0) Output=1;

     PWM=(unsigned int)Output;

      

     old_Err=Err;

     

}

// External Interrupt 0 service routine

interrupt [EXT_INT0] void ext_int0_isr(void)

{

// Place your code here

   //kenh A noi vs INT0 (ngat canh xuong)

   if(CH_B==1)  //quay theo chieu thuan

   {

       Pulse ++;

//        cnt=1;

   }

   else 

   {

        Pulse--;  //quay nguoc chieu kim dong ho 

//         cnt=0;

   }

}

// Timer2 overflow interrupt service routine

interrupt [TIM2_OVF] void timer2_ovf_isr(void)

{

// Place your code here

  TCNT2=60;        //ngat tran 25ms= thoi gian lay mau

  sample_count++;

  Pid_control(Ctrl_Speed,1);   //van toc ctr_speed, quay theo chieu thuan

}

//PID vị trí

void Motor_Vtri_PID(long int des_Alpha) //des_Vtri mong muon (dc tinh bang so goc quay dc)

{

       rAlpha=Pulse; 

//       if(rAlpha<0) rAlpha=-rAlpha;

       Err=des_Alpha-rAlpha; //tinh error

   

   //cac thanh phan PID

       if(Err<0) { Err=-Err; DIR=0;}

       else DIR=1;

       pPart=Kp*Err;

       dPart=Kd*(Err-old_Err)*inv_Sampling_time;

       iPart+=Ki*Sampling_time*Err/1000; //tinh Sampling_time theo s

       Output+=(long int)(pPart+dPart+iPart);

//       DIR=1;

       if(Err==0) {//cbi(Motor_PORT,Motor_EN); 

       DIR=0;

       Output=0;}

       if(Output>=period)

       {

           Output=period-1;

       }

       if(Output<=0)

       {

           DIR=0;

           Output=1;

       }

       

       PWM=Output; //gan gia tri dutycycle

       old_Err=Err; //luu lai gia tri Error

}

// External Interrupt 0 service routine

interrupt [EXT_INT0] void ext_int0_isr(void)

{

// Place your code here

   //kenh A noi vs INT0 (ngat canh xuong)

   if(CH_B==1)  //quay theo chieu thuan

   {

       Pulse ++;

//        cnt=1;

   }

   else 

   {

        Pulse--;  //quay nguoc chieu kim dong ho 

//         cnt=0;

   }

}

// Timer2 overflow interrupt service routine

interrupt [TIM2_OVF] void timer2_ovf_isr(void)

{

// Place your code here

  TCNT2=60;        //ngat tran 25ms= thoi gian lay mau

  sample_count++;

  Motor_Vtri_PID(Ctrl_Speed,1);   //van toc ctr_speed, quay theo chieu thuan

}

Link video test hãm động cơ: Tam BK STAR
Project full, liên hệ: DoAnDienTu