Điều khiển vận tốc động cơ DC servo

ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH
TRƯỜNG ĐẠI HỌC BÁCH KHOA
KHOA CƠ KHÍ

ĐIỀU KHIỂN VẬN TỐC
ĐỘNG CƠ DC SERVO
Nhóm 3


I.Giới thiệu

Điều khiển động cơ DC servo là một trong những ứng dụng cơ bản nhất của vi điều khiển

Các linh kiện được sử dụng trong project:
DC servo, PIC16F877A, L298, LCD, ma trận phím


I.Giới thiệu
DC servo: 12V-encoder 300xung vòng

PIC16F877A: dùng timer0,timer2,PWM (ở đây ta không dùng PWM bắn trực tiếp cho động cơ
mà thông qua diver L298)

Ma trận phím: sử dụng keypad có sẵng trong proteus


I.Giới thiệu
Mạch được chia làm 2 phần :
Mạch điều khiển:PIC để vận hành giải thuật PID, nhập xuất

Mạch động lực:

II.Giải thuật PID

Chọn các thông số PID ban đầu kp=, ki= ,kd= (try error)
Thời gian lấy mẫu 25ms
Ta có giải thuật PID:
Err = dSpeed - rpm;
P =(kp*Err);
I =I +(ki*Err*sampling_time);
D =kd*(Err - pre_Err)/sampling_time;
PID += P + I +D;


II.Giải thuật PID

Tính toán thời gian ngắt timer2
              Chọn tần số PWM bắn cho động cơ 5KHz
Ta có thời gian ngắt timer2 (precale=4)

4 × 4 × ( 1 + TMR 2 )
T=
= 1/ 5000
20 MHz
⇒ TMR 2 = 249


III.Mô phỏng

#include <16F877A.h>
#include <math.h>
#use delay (clock = 20M)

#define LCD_ENABLE_PIN PIN_D2
#define LCD_RS_PIN

PIN_D0

#define LCD_RW_PIN

PIN_D1

#define LCD_DATA4

PIN_D4

#define LCD_DATA5

PIN_D5

#define LCD_DATA6

PIN_D6

#define LCD_DATA7

PIN_D7

#include <lcd.c>

#include <control_lcd.c>

#define sampling_time 25 // thoi gian lay mau la 25ms
#define inv_sampling_time 40 // 1/thoi gian lay mau
float kp = 25, ki = 0.05,kd = 0.08;
unsigned int32 dSpeed = 0;
signed int32 P = 0,I = 0, PID=0, D = 0, Err = 0,pre_Err = 0,rSpeed=0;
float rpm = 0.0;
float pulse=0.0, pre_pulse=0.0;
int8 counter = 0;


void Motor_speed_pid(void)
{
rSpeed=pulse-pre_pulse; // xung/25ms
pre_pulse=pulse; //luu lai gia tri

Err = 0.125*dSpeed - abs(rSpeed); //(dSpeed*300*0.025/60) xung/25ms - pulse xung/25ms
P =kp*Err;
I +=ki*Err*sampling_time/1000;
D =kd*(Err - pre_Err)*inv_sampling_time;
PID += P + I +D;
if(PID > 249)
PID = 250;
if(PID <= 0)
PID = 1;
set_pwm1_duty(PID);
pre_Err = Err;
}

#INT_EXT
void encoder(void)
{
pulse = pulse + 1;
}
int main (void)
{
// xu li lcd, lay toc do nhap avo
lcd_init();
delay_ms(50);
lcd_gotoxy(1,1);
lcd_putc("Nhap_TD (vg/ph): ");
while (mode == 1)
{
quetphim();
}
dSpeed = getnum();
lcd_send_byte(0,0x01);
//L298 on
output_high(pin_B1);
output_low(pin_B2);
// ngat ngoai cho encoder
ext_int_edge(L_TO_H);
enable_interrupts(INT_EXT);


//dung timer0 dinh thoi lay xung/(sampling_time = 25ms)
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_256);
set_timer0(10);

enable_interrupts(INT_TIMER0);
enable_interrupts(GLOBAL);

//dung timer2 de dinh tan so PWM:mode =2 period 124 postscale = 10,
setup_timer_2(T2_DIV_BY_4,249,1);//pwm 5KHz
setup_ccp1(ccp_PWM);
while (1)
{
lcd_gotoxy(1,1);
lcd_putc("TD_dat:");
hienthi1(dSpeed);
lcd_gotoxy(1,2);
lcd_putc("Cur_speed:");
hienthi(rpm);
delay_ms(300);
lcd_send_byte(0,0x01);
}
return 0;
}


Cảm ơn thầy và các bạn đã lắng nghe