thiết kế phương pháp điều khiển robot tự hành dựa trên cơ sở logic mờ, chương 15 pps

-1 -
Chương 15: Sơ đồ nguyên lý mạch động
lực
Các linh kiện sử dụng và số lượng :
-Opto P521 : 8
- Điện trở 47 Ω: 8
- Điện trở 1K : 2
- Điện trở 10K : 12
- Tụ gốm 1000uF: 3
-Diode 2A: 8
- Tụ gốm 104 : 2
- Mosfet IRF 9630: 4
- Mosfet IRF 630N: 4
- Điện áp nguồn : 12V
-2 -
-Tụ hóa 1000uF 25V : 2 - LM7812: 1
B.Sơ đồ nguyên lý mạch điều khiển
Các linh kiện sử dụng và số lượng:
- VĐK PIC18F4331 : 1
- Công tắc 6 chân : 2
- LM7805 : 1
- Diode 2A : 1
- Led đỏ : 1
- Điện trở 1K: 1
- Điện trở 10K: 1
- Biến trở 10K :1
- Tụ 33uF : 3
- Tụ gốm 1000uF: 1
-3 -
-4 -
C. Chương trình áp dụng logic mờ cho Mobile robot

1.Chương trình chính
// DE TAI TOT NHGIEP
-
// SU DUNG FUZZY LOGIC DIEU KHIEN MOBILE
ROBOT
// SVTH: NGUYEN XUAN HOANG
// GVHD: AN TRI TAN
#include "E:\ProgramHoang\fuzzy
chicken\main.h"
#include "E:\ProgramHoang\lcd_lib_4bit.c"
#include "E:\ProgramHoang\Thiet ke mo
hinh\program\led7.c"
#include "E:\ProgramHoang\Thiet ke mo
hinh\sieu am.c"
#include <math.h>
// KHAI BAO CAC CHUONG TRINH CON
float min(float, float);
float max(float, float);
float dokhoangcach(float, float);
float dogoc(float, float);
void sailechduong(float, float);
void sailecham(float,float);
// chuong trinh chinh
void main()
{
setup_adc_ports(NO_ANALOGS|VSS_VDD);

setup_adc(ADC_OFF|ADC_TAD_MUL_0|ADC_WHEN_INT
0|ADC_INT_EVERY_OTHER);
setup_spi(FALSE);

setup_wdt(WDT_OFF);
setup_timer_0(RTCC_INTERNAL);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DIV_BY_16,255,1);
setup_ccp1(CCP_PWM);
setup_ccp2(CCP_PWM);
set_pwm1_duty(0);
-5 -
set_pwm2_duty(0);
setup_oscillator(False);
set_tris_a(0x00);
set_tris_b(0x00);
set_tris_c(0x00);
set_tris_d(0x00);
set_tris_e(0x00);
// TODO: USER CODE!!
LCD_init();

while(1)
{
int8 r1,r2; //khoang cach cac sieu am
do duoc milimet
float ex,et; //sailech khoang cach
va sai lech goc
r1=kt_sieuam1(); //sieu am truoc
r2=kt_sieuam2(); //sieu am sau
ex=dokhoangcach(r1,r2);
et=dogoc(r1,r2);
LCD_putcmd(0x80);
LCD_putchar("Et=");

LCD_putcmd(0x83);
LCD3(et);//hien thi Ex
LCD_putcmd(0x88);
LCD_putchar("Ex=");
LCD_putcmd(0x8c);
LCD3(ex);//hien thi Ex

LCD_PUTCMD(0XC0);
LCD_putchar("S1:");
LCD_putcmd(0xc3);
LCD3(r1);
LCD_PUTCMD(0XC8);
LCD_putchar("S2: ");
LCD_putcmd(0xcb);
LCD3(r2);
// DOAN DIEU KHIEN DONG CO
// banh trai=pwm1/banhphai=pwm2
if(ex==0&&et==0)
{
-6 -
set_pwm1_duty(1000);
set_pwm2_duty(1000);//di thang
}
else
{
if(ex>=0)
sailechduong(ex,et);
else
sailecham(ex,et);
}

}

} //Ket thuc chuong trinh chinh
//chuong trinh tinh gia tri MAX,MIN
float max(float r,float g)
{
if(r>g)
return(r);
else
return(g);
}
float min(float r,float g)
{
if(r>g)
return(g);
else return(r);
}

//chuong trinh do khoang cach
float dokhoangcach(float r1,float r2)
{
float distance,edistance;
distance=max(r1,r2)+ abs(r1-r2)/2;
edistance=3-distance;//khoang cach dat
la 300mm
edistance=floor(edistance);//lam tron
so
if(edistance>3)
edistance==3;
if(edistance<-3)

edistance==-3;
return(edistance);
}
-7 -
float dogoc(float r1,float r2)
{
float etheta;
etheta=0 -(r1-r2); //0 la goc dat
ban dau
if(etheta>2)
etheta==2;
if(etheta<-2)
etheta==-2;

return(etheta);
}
//chuong trinh con dieu khien dong co
// sai lech duong
void sailechduong(float ex, float et)
{ //Ex=3
if(ex==3&&et==2)
{
set_pwm2_duty(300);
set_pwm1_duty(1000);
}
if(ex==3&&et==1)
{
set_pwm2_duty(300);
set_pwm1_duty(850);
}

if(ex==3&&et==0)
{
set_pwm2_duty(300);
set_pwm1_duty(700);
}
if(ex==3&&et==-1)
{
set_pwm2_duty(300);
set_pwm1_duty(600);
}
if(ex==3&&et==-2)
{
set_pwm2_duty(300);
set_pwm1_duty(500);
}

//Ex=2
-8 -
if(ex==2&&et==2)
{
set_pwm2_duty(300);
set_pwm1_duty(900);
}
if(ex==2&&et==2)
{
set_pwm2_duty(300);
set_pwm1_duty(1000);
}
if(ex==2&&et==1)
{

set_pwm2_duty(300);
set_pwm1_duty(800);
}
if(ex==2&&et==0)
{
set_pwm2_duty(300);
set_pwm1_duty(700);
}
if(ex==2&&et==-1)
{
set_pwm2_duty(300);
set_pwm1_duty(600);
}
if(ex==2&&et==-2)
{
set_pwm2_duty(300);
set_pwm1_duty(500);
}
//Ex=1
if(ex==1&&et==2)
{
set_pwm2_duty(500);
set_pwm1_duty(800);
}
if(ex==1&&et==1)
{
set_pwm2_duty(450);
set_pwm1_duty(700);
}
if(ex==1&&et==0)

{
-9 -
set_pwm2_duty(400);
set_pwm1_duty(600);
}
if(ex==1&&et==-1)
{
set_pwm2_duty(350);
set_pwm1_duty(500);
}
if(ex==1&&et==-2)
{
set_pwm2_duty(300);
set_pwm1_duty(450);
}
//Ex=0
if(ex==0&&et==2)
{
set_pwm2_duty(500);
set_pwm1_duty(800);
}
if(ex==0&&et==1)
{
set_pwm2_duty(500);
set_pwm1_duty(700);
}
if(ex==0&&et==-1)
{
set_pwm2_duty(700);
set_pwm1_duty(500);

}
if(ex==0&&et==2)
{
set_pwm2_duty(800);
set_pwm1_duty(500);
}

}//end sailechduong
void sailecham(float ex, float et)
{
//Ex=-1
if(ex==-1&&et==2)
{
set_pwm2_duty(500);
set_pwm1_duty(700);
-10 -
}
if(ex==-1&&et==1)
{
set_pwm2_duty(500);
set_pwm1_duty(650);
}
if(ex==-1&&et==0)
{
set_pwm2_duty(500);
set_pwm1_duty(600);
}
if(ex==-1&&et==-1)
{
set_pwm2_duty(600);

set_pwm1_duty(500);
}
if(ex==-1&&et==-2)
{
set_pwm2_duty(700);
set_pwm1_duty(500);
}
//Ex=-2
if(ex==-2&&et==2)
{
set_pwm2_duty(500);
set_pwm1_duty(800);
}
if(ex==-2&&et==1)
{
set_pwm2_duty(700);
set_pwm1_duty(500);
}
if(ex==-2&&et==0)
{
set_pwm2_duty(600);
set_pwm1_duty(500);
}
if(ex==-1&&et==-1)
{
set_pwm2_duty(500);
set_pwm1_duty(650);
}
if(ex==-1&&et==-2)
-11 -

{
set_pwm2_duty(400);
set_pwm1_duty(700);
}
//Ex=-2
if(ex==-2&&et==2)
{
set_pwm2_duty(800);
set_pwm1_duty(500);
}
if(ex==-2&&et==1)
{
set_pwm2_duty(900);
set_pwm1_duty(500);
}
if(ex==-2&&et==0)
{
set_pwm2_duty(400);
set_pwm1_duty(700);
}
if(ex==-2&&et==-1)
{
set_pwm2_duty(500);
set_pwm1_duty(800);
}
if(ex==-2&&et==-2)
{
set_pwm2_duty(500);
set_pwm1_duty(900);
}

//Ex=-3
if(ex==-3&&et==2)
{
set_pwm2_duty(700);
set_pwm1_duty(500);
}
if(ex==-3&&et==1)
{
set_pwm2_duty(600);
set_pwm1_duty(500);
}
if(ex==-3&&et==0)
{
-12 -
set_pwm2_duty(500);
set_pwm1_duty(700);
}
if(ex==-3&&et==-1)
{
set_pwm2_duty(400);
set_pwm1_duty(900);
}
if(ex==-3&&et==-2)
{
set_pwm2_duty(400);
set_pwm1_duty(1000);
}

}//End sailecham
2.Thư viện LCD

// Thu vien 4bit LCD 2x16
#include <stddef.h>
#define LCD_RS PIN_D2
//#define LCD_RW PIN_A1
#define LCD_EN PIN_D3
#define LCD_D4 PIN_D4
#define LCD_D5 PIN_D5
#define LCD_D6 PIN_D6
#define LCD_D7 PIN_D7
// misc display defines-
#define Line_1 0x80
#define Line_2 0xC0
#define Clear_Scr 0x01
// prototype statements
#separate void LCD_Init ( void );// ham khoi
tao LCD
#separate void LCD_SetPosition ( unsigned
int cX );//Thiet lap vi tri con tro
#separate void LCD_PutChar ( unsigned int cX
);// Ham viet1kitu/1chuoi len LCD
-13 -
#separate void LCD_PutCmd ( unsigned int cX)
;// Ham gui lenh len LCD
#separate void LCD_PulseEnable ( void );//
Xung kich hoat
#separate void LCD_SetData ( unsigned int cX
);// Dat du lieu len chan Data
// D/n Cong
#use standard_io ( B )
#use standard_io (A)

//khoi tao
LCD********************************************
**
#separate void LCD_Init ( void )
{
LCD_SetData ( 0x00 );
delay_ms(200); /* wait enough time
after Vdd rise >> 15ms */
output_low ( LCD_RS );// che do gui lenh
LCD_SetData ( 0x03 ); /* init with
specific nibbles to start 4-bit mode */
LCD_PulseEnable();
LCD_PulseEnable();
LCD_PulseEnable();
LCD_SetData ( 0x02 ); /* set 4-bit
interface */
LCD_PulseEnable(); /* send dual
nibbles hereafter, MSN first */
LCD_PutCmd ( 0x2C ); /* function set
(all lines, 5x7 characters) */
LCD_PutCmd ( 0b00001100); /* display
ON, cursor off, no blink */
LCD_PutCmd ( 0x06 ); /* entry mode
set, increment & scroll left */
LCD_PutCmd ( 0x01 ); /* clear display
*/
}
#separate void LCD_SetPosition ( unsigned
int cX )
{

/* this subroutine works specifically
for 4-bit Port A */
-14 -
LCD_SetData ( swap ( cX ) | 0x08 );
LCD_PulseEnable();
LCD_SetData ( swap ( cX ) );
LCD_PulseEnable();
}
#separate void LCD_PutChar ( unsigned int cX
)
{
/* this subroutine works specifically
for 4-bit Port A */
output_high ( LCD_RS );
LCD_PutCmd( cX );
output_low ( LCD_RS );
}
#separate void LCD_PutCmd ( unsigned int cX
)
{
/* this subroutine works specifically
for 4-bit Port A */
LCD_SetData ( swap ( cX ) ); /* send
high nibble */
LCD_PulseEnable();
LCD_SetData ( swap ( cX ) ); /* send
low nibble */
LCD_PulseEnable();
}
#separate void LCD_PulseEnable ( void )

{
output_high ( LCD_EN );
delay_us ( 3 ); // was 10
output_low ( LCD_EN );
delay_ms ( 3 ); // was 5
}
#separate void LCD_SetData ( unsigned int cX
)
{
output_bit ( LCD_D4, cX & 0x01 );
output_bit ( LCD_D5, cX & 0x02 );
output_bit ( LCD_D6, cX & 0x04 );
output_bit ( LCD_D7, cX & 0x08 );
-15 -
}
void LCD3(int16 m)
{
int a,b,c,d;
a = m/1000 + 48;
LCD_PutChar(a);
b = (m/100)%10 + 48;
LCD_PutChar(b);
c = (m/10)%10 + 48;
LCD_PutChar(c);
d = m%10 + 48;
LCD_Putchar(d);
}
void LCD1(int m)
{
int a;

a = m%10 + 48;
LCD_Putchar(a);
}
3.Chương trình hiển thị trên LED 7x4
// Chuong trinh Qet led 7x4
int16 n;
//Chuong trinh cho led 7x4
char ma[]={0x00, 0x10, 0x20, 0x30, 0x40,
0x50, 0x60, 0x70, 0x80, 0x90};
//khai bao mang chua cac so tu 0-9
long dv,tram,chuc,ngan;
void call(void)//ham tra ve cac gia tri can
hien thi
{
ngan= n/1000;
tram=(n/100)%10;
chuc=(n/10)%10;
dv=n%10;
}
void quet(void)//hien thi cac so
{
call();
-16 -
output_b(ma[dv]+1);
delay_ms(3);
output_b(0x00);
delay_ms(3);
if(n>9)
{
output_b(ma[chuc]+2);

delay_ms(3);
output_b(0x00);
delay_ms(3);
}
if(n>99)
{
output_b(ma[tram]+4);
delay_ms(3);
output_b(0x00);
delay_ms(3);
}
if(n>999)
{
output_b(ma[ngan]+8);
delay_ms(3);
output_b(0x00);
delay_ms(3);
}
} //end
4.Chương trình cho 2 cảm biến siêu âm
//Chuong trinh do khoang cach bang 2 cam
bien sieu am SRF05
//su dung 2 chân c6 và c7 làm dau vào và dau
ra cho siêu am 1 và a1,a2 cho siêu âm 2
#define IN1_05 pin_c6
#define OUT1_05 pin_c7
#define IN2_05 pin_a1
#define OUT2_05 pin_a2
//clockrate/(4 clock per
intruction*T1_div_by_4)*1000000

#define CTM 1.25
//Convert To Microsesor
//KIEM TRA TINH TOAN CHO SIEU AM 1
int16 kt_sieuam1()
-17 -
{
int16 time;
output_high(IN1_05);
delay_us(10);
output_low(IN1_05);//phat xung 10us
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4);
while(!input(OUT1_05)){}//cho cho khi co
tin hieu tai dau ra sieu am
set_timer0(0);//bat dau hoat dong timer1
while(input(OUT1_05)){}
time=get_timer0();//dem thoi gian nhan dc
xung
time=time/CTM;
if(time>=42000)
{
//ko lam ji;
return 0;
}
else
{
return time/148; //chuyen ra ink
}
}
//KIEM TRA TINH TOAN CHO SIEU AM 2
int16 kt_sieuam2()

{
int16 time;
output_high(IN2_05);
delay_us(10);
output_low(IN2_05);//phat xung 10us
setup_timer_1(T1_INTERNAL | T1_DIV_BY_4);
while(!input(OUT2_05)){}//cho cho khi co
tin hieu tai dau ra sieu am
set_timer1(0);//bat dau hoat dong timer1
while(input(OUT2_05)){}
time=get_timer1();//dem thoi gian nhan dc
xung
time=time/CTM;
if(time>=42000)
{
//ko lam ji;
return 0;
-18 -
}
else
{
return time/148; //chuyen ra inh
}
}