#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--;
}
| 
