Điều khiển vị trí động cơ DC servo

LOGO

Điều Khiển Vị Trí Động Cơ DC Servo
GVHD: TS. Võ Tường Quân


Nội
NộiDung
Dung

1

Giới thiệu

2

Mạch nguyên lý

3

Thuật toán PID

4

Thuật toán điều khiển


1. Giới thiệu

 Động cơ servo được thiết kế cho những hệ thống hồi tiếp vòng kín.
 Đặc tính vận hành của một động cơ servo phụ thuộc rất nhiều vào đặc tính từ và phương pháp điều khiển

động cơ servo.

 Tín hiệu ra của động cơ được nối với một mạch điều khiển.
 Khi động cơ quay, vận tốc và vị trí sẽ được hồi tiếp về mạch điều khiển này.
 Nếu có bất kỳ lý do nào ngăn cản chuyển động quay của động cơ, cơ cấu hồi tiếp sẽ nhận thấy tín hiệu ra
chưa đạt được vị trí mong muốn.

 Mạch điều khiển tiếp tục chỉnh sai lệch cho động cơ đạt được điểm chính xác.


2. Mạch nguyên lý


3. Thuật toán PID

 Bộ điều khiển PID là bộ điều khiển sử dụng kỹ thuât điều khiển theo vòng lặp có hồi tiếp được sử dụng
rộng rãi trong các hệ thống điều khiển tự động.

 Một bộ điều khiển PID hiệu chỉnh sai lệch giữa tín hiệu ngõ ra và ngõ vào sau đó đưa ra mt một tín hiệu
điều khiển để điều chỉnh quá trình cho phù hợp


3. Thuật toán PID

Chọn các thông số động của động cơ để điều khiển:

 Trở kháng R = 2.75Ω
 Cảm kháng L = 0.035H
 Hệ số sức điện động cảm ứng Kb=1,2 (Vol/rad/s)
 Hệ sô momen Kt=0,04689 (Nm/Amp)

 Monen quán tính của roto J = 0,022 (Kgm2/rad)
 Hệ số ma sát b = 0,0005(Nm/rad/s)


3. Thuật toán PID
Hàm truyền:
 

Ta tìm được các hệ số:
KP = 0.3

KI = 0.06

KD = 0.3


4. Thuật toán điều khiển

Dùng ngắt ngoài đếm xung encoder:
Nối kênh A của encoder với 1 ngắt ngoài và kênh B với một chân bất kỳ. Cứ mỗi lần ngắt ngoài xảy ra, tức có 1
xung xuất hiện trên ở kênh A thì trình phục vụ ngắt ngoài tự động được gọi. Trong trình phục vụ ngắt này, sẽ kiểm
tra mức của kênh B, tùy theo mức của kênh B chúng ta sẽ tăng biến đếm xung lên 1 hoặc giảm đi 1.

#int_EXT
void ngat_ngoai()
{ if(!input(PIN_B1)) //Khi co tin hieu thi xung tang len 1
Pulse++;
else Pulse--;
}

4. Thuật toán điều khiển

Code giải thuật PID:
void Motor_Position_PID(int16 des_Pulse)
{ Err= des_Pulse - Pulse; //Tinh sai so
pPart=Kp*Err;
dPart=Kd*(Err-pre_Err)*inv_Sampling_time;
iPart+=Ki*Sampling_time*(Err+pre_Err)/1000;
Output=pPart+dPart+iPart;
if (Output>=500) Output = 500; //(0<= Output <=500)
if (Output<=0) Output = 0;
set_pwm1_duty(Output);
pre_Err=Err; //Luu lai gia tri Err
}


#include <16F877a.h>
#device ADC=10
#fuses NOWDT, HS, NOPUT, NOPROTECT, NODEBUG, NOBROWNOUT, NOLVP, NOCPD, NOWRT 
#use delay(clock=20000000)
#use RS232(BAUD=9600, XMIT=PIN_C6, RCV=PIN_C7)

#byte PR2=0x92
#bit RC2=0x07.2

#define LCD_ENABLE_PIN  PIN_D3
#define LCD_RS_PIN PIN_D1  
#define LCD_RW_PIN PIN_D2 
#define LCD_DATA4 PIN_D4 

#define LCD_DATA5 PIN_D5 
#define LCD_DATA6 PIN_D6 
#define LCD_DATA7 PIN_D7 

#define Sampling_time 10 //ms
#define inv_Sampling_time 100

#include <lcd.c>
unsigned int16 Pulse=0, des_Pulse=50;
signed long Err=0, pre_Err=0;
float Kp=0.3, Ki=0.06, Kd=0.3, pPart=0, dPart=0, iPart=0;
signed int16 Output;

void Motor_Position_PID(int16 des_Pulse);

www.themegallery.com


#include <16F877a.h>
#device ADC=10
#fuses NOWDT, HS, NOPUT, NOPROTECT, NODEBUG, NOBROWNOUT, NOLVP, NOCPD, NOWRT 
#use delay(clock=20000000)
#use RS232(BAUD=9600, XMIT=PIN_C6, RCV=PIN_C7)

#byte PR2=0x92
#bit RC2=0x07.2

#define LCD_ENABLE_PIN  PIN_D3
#define LCD_RS_PIN PIN_D1  
#define LCD_RW_PIN PIN_D2 

#define LCD_DATA4 PIN_D4 
#define LCD_DATA5 PIN_D5 
#define LCD_DATA6 PIN_D6 
#define LCD_DATA7 PIN_D7 

#define Sampling_time 10 //ms
#define inv_Sampling_time 100

#include <lcd.c>
unsigned int16 Pulse=0, des_Pulse=50;
signed long Err=0, pre_Err=0;
float Kp=0.3, Ki=0.06, Kd=0.3, pPart=0, dPart=0, iPart=0;
signed int16 Output;

void Motor_Position_PID(int16 des_Pulse);
www.themegallery.com


LOGO

Thank you