Anh Nguyễn

nvtienanh.info

Chương trình đo và hiển thị vận tốc động cơ DC servo

2014-12-22 Anh Nguyễnnotes

Giới thiệu

Để điều khiển động cơ DC thì thông thường chúng ta phải biết cách đọc encoder để lấy giá trị feedback cho bộ điều khiển. Để dễ dàng cho các bạn tìm hiểu thì mình sẽ giới thiệu bước đầu tiên để điều khiển động cơ đó là đo vận tốc của động cơ DC servo. Tất cả sẽ được mô phỏng trong phần mềm Proteus 8.1, chương trình viết cho vi điều khiển PIC16F877A bằng phần mềm PIC CCS-C 5.024, giá trị vận tốc được hiển thị trên LCD16x2.

16078875442 a82f39918f

Hình 1. Sơ đồ mô phỏng trong Proteus

Cách đo vận tốc như sau: - Dùng ngắt ngoài để đếm xung. - Dùng ngắt timer (thời gian lấy mẫu): số xung đọc được trong khoảng thời gian Δt\varDelta t từ đó tính ra được vận tốc của động cơ. - Dùng LCD16x2: hiển thị giá trị vận tốc, ngoài ra có thể dùng giao tiếp RS232 để truyền giá trị vận tốc lên máy tính.

Encoder và đếm xung encoder

Thông thường mình sẽ quan tâm đến chân tín hiệu encoder và chân bù của nó. Chúng ta sẽ dùng ngắt ngoài ( ngắt xung cạnh lên hoặc xung cạnh xuống), còn chân bù để xác định chiều quay.

#int_ext
void kenh_A()
{
   if (input_state(PIN_B4))
      //chieu='T';
      Pulse++;
   else
      //chieu='N';
      Pulse--;
}

Tính toán ngắt timer trong PIC

Một số khái niệm liên quan đến Timer:

  • Chu kỳ máy: chính là chu kỳ của xung Clock TOSC=1fOSCT_{OSC}=\frac{1}{f_{OSC}}
  • Chu kỳ lệnh: thời gian thực thi một lệnh (Hợp ngữ) bằng 4 lần chu kỳ máy.
  • Bộ chia Prescaler: là số chu kỳ lệnh để timer đếm 1 lần.
  • Ngắt timer: Là sự kiện xảy ra khi bộ đếm của timer bị tràn, giới hạn bộ đếm của timer: 8bit là 2^8, 16 bit là 2^16.

Ví dụ:

Khi sử dụng timer 16bit,Tần số 20MHz, Prescaler = 8 thì thời gian để Timer đếm 1 lần được tính như sau:

  • Chu kỳ máy: TOSC=120MHZT_{OSC}=\frac{1}{20MHZ}
  • Chu kỳ lệnh: 4×TOSC4{\times}T_{OSC} Do Prescaler = 8 nên phải qua 8 chu kỳ lệnh timer mới đếm một lần nên thời gian để timer đếm 1 lần là: T=4×PrescalefOSC=4×820MHZ=1.6musT=\frac{4{\times} Prescale}{f_{OSC}}=\frac{4{\times}8}{20MHZ}=1.6mu s Với Prescaler = 8 thì thời gian dài nhất để timer ngắt một lần là 2^16x1.6us = 0.10486s. Một vấn đề đặt ra là nếu bạn muốn đếm tới 1s chẳng hạn để thực hiện một công việc nào đó thì 0.10486s là không đủ: có 2 cách là bạn tăng Prescaler lên tới giá trị lớn nhất có thể (xem trong datasheet của từng vi điều khiển) hoặc dùng một biến đếm: counter cứ mỗi lần tràn timer biến đếm tăng lên 1 lần đến khi counter tăng lên 10 lần thì thời gian đạt được là 1.04s thì bạn có thể thực hiện công việc của mình. Lại phức tạp hơn một chút nữa là bạn muốn đếm chính xác 1s thì phải làm thế nào, nếu làm như cách ở trên thì thời gian là 1.04s như vậy là có sai số. Cách giải quyết là chúng ta phải lựa chọn số lần đếm timer để xảy ra tràn: mạc định timer sẽ đếm từ 0 đến 2^16 (timer 16 bit) tức là 2^16x1.6us = 0.104s thì sẽ xảy ra tràn và sự kiện ngắt timer xảy ra.
  • Nếu bạn muốn thời gian tràn là 0.1s thì số lần đếm timer là 0.1s/1.6us = 62500 như vậy timer tràn sau 62500 lần đếm và chúng ta phải cho timer bắt đầu đếm từ 2^16 - 62500 = 3036. Khi đó biến counter cần thiết là 10 thì sẽ đếm tròn 1s. Tóm lại sẽ có công thức tín toán thời gian chính xác cho timer1 16bit. Các timer khác tương tự. Tref=4×Prescalex(216TMR1)fOSC×counterT_{ref}=\frac{4{\times}Prescalex(2^{16}-TMR1)}{f_{OSC}}{\times} counter Chúng ta lựa chọn giá trị Prescale và TMR1 phù hợp thì sẽ được thời gian ngắt chẵn theo ý muốn. Trong ví dụ của mình thời gian ngắt là 100ms thì, prescale = 8, tần số 20MHZ: Tref=4×8(2163036)20MHZ×1=100msT_{ref}=\frac{4{\times}8(2^{16}-3036)}{20MHZ}{\times}1=100ms

Đoạn chương trình CCS-C

///////////////////////////////////////////////////////////////////////////////
//                    Do van toc dong co dung ngat ngoai                     //
//                    Xac dinh chieu quay va van toc                         //
//                    Dung PWM de chay dong co, dung LM298                   //
//                    Tien Anh 10/12/2013                                    //
///////////////////////////////////////////////////////////////////////////////
#include 
#device PIC16F877*=16 ADC=10    // Su dung con tro 16bit (cho MCU 14bit)  
                                // Su dung ADC 10bit
#fuses hs, nowdt, noprotect, nolvp, put, brownout
#use delay(clock=20000000)
#include 
#include 

// Dung ngat ngoai kenh A de do xung kenh B de xac dinh chieu quay
// Kenh A  B0
// Kenh B  B4
float32 w = 0.0;//kp=0.2 kd=0.02 ki=0.01
signed int32 Pulse=0,PrePulse=0,DeltaPulse=0;// Dem xung Encoder

float32 Get_W(void);
// Ngat kenh A
#int_ext
void kenh_A()
{
   if (input_state(PIN_B4))
      //chieu='T';
      Pulse++;
   else
      //chieu='N';
      Pulse--;
}

// Sau 100ms thi ngat timer1, do so xung dem duoc
#int_timer1
void ngat_timer1()
{
   // Tinh van toc w (vong/phut)
   w = get_W();   
   set_timer1(3036); // reset timer   
}
void main()
{
char so[8];
// Cai dat thong so ban dau
LCD_Init();
delay_ms(10);

// Cai dat timer1
// Prescale = 8; tran sau 100ms
//                  4*Prescale*[(2^16 - TMR1]
//   T_interrupt = -------------------------- x counter = 100 ms
//                         Fosc
//                  4*8*[(2^16-1)-3036]
//   T_interrupt = ----------------------- x 1 = 100 ms
//                           20MHz
setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);
ext_int_edge( L_TO_H );
// Khai bao ngat
enable_interrupts(global);
enable_interrupts(int_ext);
enable_interrupts(int_timer1);
set_timer1(3036);

while(1)
   { 
      sprintf(so,"%5.2f",w); 
      LCD_Gotoxy(1,0);   
      LCD_Puts(so);
      // Truyen du lieu len RS232       
   }
}
// Ham tinh van toc goc vong/phut
float32 Get_W(void)
{
   float32 rpm;
   DeltaPulse = Pulse - PrePulse;
   PrePulse = Pulse;
   /*
      PulsesPerRev: so xung/vong cua encoder
      DeltaPulse: so xung doc duoc giua hai lan ngat lien tiep
      Sampling_time: thoi gian(s) lay mau (thoi gian giua 2 lan ngat lien tiep)
      n: la so vong quay cua dong co trong mot phut
      Quy tac tam xuat:
            Xung                 giay
        n x PulsesPerRev          60           
          DeltaPulse          Sampling_time
          
     => n x PulsesPerRev x Sampling_time = DeltaPulse x 60
                  DeltaPulse x 60
     => n = ----------------------------
            PulsesPerRev x Sampling_time
   */
   // PulsesPerRev = 100 xung/vong
   // Sampling_time = 0.1 s
   //n = rpm = 6.0*(float32)DeltaPulse   
   rpm = 6.0*(float32)DeltaPulse;   
   return(rpm);
}

Cài đặt thông số cho động cơ trong Proteus

Cài đặt thông số motor Proteus

Hình 2. Cài đặt thông số cho Motor

Pulens per Renvolution: 100 là số xung encoder trong một vòng quay

Download project