#include <delay.h> #include <mega16.h> #include <math.h> #include <stdio.h> #include <alcd.h> #include <stdlib.h>
//Dinh nghia cac duong dieu khien motor #define MOTOR_DDR DDRD #define MOTOR_PORT PORTD
#define Sampling_time 25 //thoi gian lay mau (ms) #define inv_Sampling_time 40 // 1/Sampling_time #define PWM_Period 8000 //8000 cycles = 1ms, f=8MHz
unsigned char toc_do_phai[8] = {15,17,19,21,23,25,27,29};
unsigned char toc_do_trai[8] = {15,17,19,21,23,25,27,29};
long int Pulse1,Pulse2, pre_Pulse1,pre_Pulse2; long int rSpeed1,rSpeed2,Err1 ,Err2, pre_Err1,pre_Err2; long int Kp1=40, Kd1=1, Ki1=1; //for speed control long int Kp2=40, Kd2=1, Ki2=1; //for speed control long int pPart1=0, iPart1=0, dPart1=0,pPart2=0, iPart2=0, dPart2=0; //PID gains long int Ctrl_Speed1=5,Ctrl_Speed2=5; //van toc can dieu khien (desired speed) long int Output1, absrSpeed1,Output2, absrSpeed2; unsigned char sample_count=0;
//dieu khien van toc bang PID
void Motor1_Speed_PID(long int des_Speed1){ rSpeed1=Pulse1-pre_Pulse1; //tinh van toc (trong sampling time) pre_Pulse1=Pulse1; //luu lai gia tri Pulse: so xung Err1=des_Speed1-abs(rSpeed1); //tinh error (loi) //cac thanh phan cua PID pPart1=Kp1*Err1; dPart1=Kd1*(Err1-pre_Err1)*inv_Sampling_time; iPart1+=Ki1*Sampling_time*(Err1+pre_Err1)/1000; Output1 +=pPart1+dPart1+iPart1; //cong thuc duoc bien doi vi la dieu khien van toc //saturation if (Output1 >=PWM_Period) Output1=PWM_Period-1; if (Output1 <=0) Output1=1; OCR1A=Output1; //gan duty cycle cho OCR1A: update PWM pre_Err1=Err1; //luu lai gia tri error }
void Motor2_Speed_PID(long int des_Speed2){ rSpeed2=Pulse2-pre_Pulse2; //tinh van toc (trong sampling time) pre_Pulse2=Pulse2; //luu lai gia tri Pulse: so xung Err2=des_Speed2-abs(rSpeed2); //tinh error (loi) //cac thanh phan cua PID pPart2=Kp2*Err2; dPart2=Kd2*(Err2-pre_Err2)*inv_Sampling_time; iPart2+=Ki2*Sampling_time*(Err2+pre_Err2)/1000; Output2 +=pPart2+dPart2+iPart2; //cong thuc duoc bien doi vi la dieu khien van toc //saturation if (Output2 >=PWM_Period) Output2=PWM_Period-1; if (Output2 <=0) Output2=1; OCR1B=Output2; //gan duty cycle cho OCR1A: update PWM pre_Err2=Err2; //luu lai gia tri error }
void main(void){ unsigned char dis[5]; //bien tam, hien thi LCD Pulse1=Pulse2=0; //Encoder va cac chan nhap toc do DDRB=0x00; //set PORTB as a input port to use the T0 input pin and INT2 PORTB=0xFF; //dien tro keo len (nhat la encoder) //Motor MOTOR_DDR=0xF0; //ngat ngoai, cho encoder-------------------------------------------------------------- MCUCSR|=(0<<ISC11)|(0<<ISC01); //ngat INT1-INT0 la ngat canh xuong - Falling Edge GICR |=(1<<INT0)|(1<<INT1); //Enable INT0-INT0 //---------------------------------------------------------------------------------
//dung timer 2 lam bo dinh thi 25ms, sampling time-------------------------------- TCCR2=(1<<CS22)|(1<<CS21)|(1<<CS20); //CS22=1, CS21=1, CS20=1: chon Prescaler=1024 (trang 125 datasheet) TCNT2=60; //gan gia tri khoi tao cho T/C2 de duoc 25ms (truong hop f=8MHz) TIMSK=(1<<TOIE2); // cho phep ngat khi co tran o T/C2 //--------------------------------------------------------------------------------
//dung timer1 lam PWM generator, Fast PWM mode 14: ICR1 chua time period TCCR1A=(1<<COM1A1)|(1<<COM1B1)|(1<<WGM11); TCCR1B=(1<<WGM13) |(1<<WGM12) |(1<<CS10) ; //--------------------------------------------------------------------------------
//**********LCD************************* lcd_init(16); lcd_clear(); lcd_putsf("TEST PID"); lcd_gotoxy(0,1); lcd_putsf("DC Servo Motor"); delay_ms(100); lcd_clear(); lcd_putsf("-Ac1: "); lcd_putsf("-Ac2: "); lcd_gotoxy(0,1); lcd_putsf("-De1: "); lcd_putsf("-De2: "); //**********LCD************************* //---khoi dong gia tri PWM---------- OCR1A=1; OCR1B=1; ICR1H=PWM_Period/256; ICR1L=PWM_Period%256;
#asm ("sei") //---------------------------------- while (1){ if (sample_count>=10){ //hien thi moi 250ms Ctrl_Speed1=Ctrl_Speed2=PINB; //doc switches //------in van toc hien tai (tinh tren 25ms) absrSpeed1=abs(rSpeed1); sprintf(dis,"%i", absrSpeed1); lcd_gotoxy(5,0); lcd_puts(dis); lcd_putsf(" "); //------in van toc mong muon (tinh tren 25ms) sprintf(dis,"%i", Ctrl_Speed1); lcd_gotoxy(5,1); lcd_puts(dis); lcd_putsf(" "); //------in van toc mong muon (tinh tren 25ms) absrSpeed2=abs(rSpeed2); sprintf(dis,"%i", absrSpeed2); lcd_gotoxy(13,0); lcd_puts(dis); lcd_putsf(" "); //------in van toc mong muon (tinh tren 25ms) sprintf(dis,"%i", Ctrl_Speed2); lcd_gotoxy(13,1); lcd_puts(dis); lcd_putsf(" "); sample_count=0; } } }
interrupt [TIM2_OVF] void timer2_ovf_isr(void) { //update sampling time--------------- TCNT2=60; //gan gia tri khoi tao cho T/C2 sample_count++; Motor1_Speed_PID(Ctrl_Speed1); Motor2_Speed_PID(Ctrl_Speed2); }
interrupt [EXT_INT0] void encoder_1(void) { if (PINB==0) Pulse1++; else Pulse1--; } interrupt [EXT_INT1] void encoder_2(void) { if (PINB==0) Pulse2++; else Pulse2--; }
| 
