Chương trình ví dụ điều xung mẫu
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (2.55 MB, 79 trang )
set_power_pwm6_duty(800); // duty = 80%
WHILE(true)
{
}
}
3. Thuật toán điều khiển RôBôt
a.Lưu đồ thuật toán
b.Chương trình lập trình
#include<18f4431.h>
#fuses NOWDT,NOPROTECT,NODEBUG,NOLVP,HS,HPOL_LOW
#device *=16 ADC=10
#use delay(clock=20000000)
#byte TRISE = 0xF96
#byte TRISD = 0xF95
#byte TRISC = 0xF94
#byte TRISB = 0xF93
#byte TRISA = 0xF92
#byte PORTB = 0xF81
#byte PORTD = 0xF83
#byte PORTA = 0xF80
#byte PORTC = 0xF82
#byte PORTE = 0xF84
#byte LATD = 0xF8C
#byte LATE = 0xF8D
#byte LATA = 0xF89
#byte LATC = 0xF8B
#byte LATB = 0xF8A
#bit cb1 = PORTE.2
#bit cb2 = PORTE.1
#bit cb3 = PORTE.0
#bit cb4 = PORTA.5
#bit cb5 = PORTA.4
#bit cb6 = PORTA.3
#bit cb7 = PORTA.2
#bit cb8 = PORTA.1
#bit servo = Latb.0
int8 cb[8];
int8 so_cb,so_vach,bit_nho;
void chaythang();
void so_cb_on();
void demvachngang();
void so_cb_on()
{
int8 i;
so_cb=0;
cb[0]=cb1;cb[1]=cb2;cb[2]=cb3;cb[3]=cb4;cb[4]=cb5;cb[5]=cb6;
for(i=0;i<=5;i++)
{
if(cb[i]==1)
{so_cb=so_cb+1;}
}
}
void demvachngang()
{
so_cb_on();
if(so_cb>3)
{
do
{
so_cb_on();
}
while(so_cb>3);
so_vach=so_vach+1;
}
}
void chaythang()
{
if( (cb1==0)&&(cb2==0)&&(cb3==0)&&(cb4==1)&&(cb5==1)&&(cb6==0)&&(cb7
==0)&&(cb8==0))
{
bit_nho=0;
servo=1;
delay_us(1300);
servo=0;
}
// lech trai
else if
( (cb1==0)&&(cb2==0)&&(cb3==0)&&(cb4==1)&&(cb5==0)&&(cb6==0)&&(cb7=
=0)&&(cb8==0))
{
bit_nho=1;
servo=1;
delay_us(1250);
servo=0;
}
else if
( (cb1==0)&&(cb2==0)&&(cb3==1)&&(cb4==1)&&(cb5==0)&&(cb6==0)&&(cb7=
=0)&&(cb8==0))
{
bit_nho=1;
servo=1;
delay_us(1200);
servo=0;
}
else if
( (cb1==0)&&(cb2==0)&&(cb3==1)&&(cb4==0)&&(cb5==0)&&(cb6==0)&&(cb7=
=0)&&(cb8==0))
{
bit_nho=1;
servo=1;
delay_us(1150);
servo=0;
}
else if
( (cb1==0)&&(cb2==1)&&(cb3==1)&&(cb4==0)&&(cb5==0)&&(cb6==0)&&(cb7=
=0)&&(cb8==0))
{
bit_nho=1;
servo=1;
delay_us(1100);
servo=0;
}
else if
( (cb1==0)&&(cb2==1)&&(cb3==0)&&(cb4==0)&&(cb5==0)&&(cb6==0)&&(cb7=
=0)&&(cb8==0))
{
bit_nho=1;
servo=1;
delay_us(1050);
servo=0;
}
else if
( (cb1==1)&&(cb2==1)&&(cb3==0)&&(cb4==0)&&(cb5==0)&&(cb6==0)&&(cb7=
=0)&&(cb8==0))
{
bit_nho=1;
servo=1;
delay_us(1000);
servo=0;
}
else if
( (cb1==1)&&(cb2==0)&&(cb3==0)&&(cb4==0)&&(cb5==0)&&(cb6==0)&&(cb7=
=0)&&(cb8==0))
{
bit_nho=1;
servo=1;
delay_us(950);
servo=0;
}
// lech phai
else if
( (cb1==0)&&(cb2==0)&&(cb3==0)&&(cb4==0)&&(cb5==1)&&(cb6==0)&&(cb7=
=0)&&(cb8==0))
{
bit_nho=2;
servo=1;
delay_us(1350);
servo=0;
}
else if
( (cb1==0)&&(cb2==0)&&(cb3==0)&&(cb4==0)&&(cb5==1)&&(cb6==1)&&(cb7=
=0)&&(cb8==0))
{
bit_nho=2;
servo=1;
delay_us(1400);
servo=0;
}
else if
( (cb1==0)&&(cb2==0)&&(cb3==0)&&(cb4==0)&&(cb5==0)&&(cb6==1)&&(cb7=
=0)&&(cb8==0))
