code cho robot dò đường

[tuonghiendk7.1]

Bạn tham khảo code này nhé ! Mình viết cho pic8f4431 điều xung 2 động cơ bạn tham khảo



// BANH_TRAI "PWM0";BANH_PHAI "PWM2";
#INCLUDE <18F4431.h>
#FUSES HS,NOWDT,NOPROTECT,NOLVP
#USE DELAY(clock=20000000)
//***** KHAI BAO THANH GHI *********

#byte PORTA =0xF80
#byte PORTB =0xF81
#byte PORTC =0xF82
#byte PORTD =0xF83
#byte PORTE =0xF84
#BIT CB1 = PORTA.0
#BIT CB2 = PORTA.1
#BIT CB3 = PORTA.2
#BIT CB4 = PORTA.3
#BIT CB5 = PORTA.4
#BIT CB6 = PORTA.5
#BIT CB7 = PORTE.0
#BIT CB8 = PORTE.1
#BIT NUTNHAN = PORTE.2
#BIT BANHPHAI = PORTB.0 //BANH TRAI PWM0
#BIT BANHTRAI = PORTB.2 //BANH PHAI PWM2
INT SOCB_THU_DC,I=0,DEM_T0;
INT16 BANH_TRAI [8] = {1000,980,945,900,850,800,750,720};
INT16 BANH_PHAI [8] = {1000,985,960,920,880,830,775,745};
VOID DEM_CHECK_POINT (VOID);
VOID TOC_DO (VOID);
VOID DEM_CHECK_POINT (VOID)
{
SOCB_THU_DC = 0;
IF(CB2==0) SOCB_THU_DC ++;
IF(CB3==0) SOCB_THU_DC ++;
IF(CB4==0) SOCB_THU_DC ++;
IF(CB5==0) SOCB_THU_DC ++;
IF(CB6==0) SOCB_THU_DC ++;
IF(CB7==0) SOCB_THU_DC ++;
IF(SOCB_THU_DC >=3) I++;
DO{}
WHILE((CB2==0&&CB3==0&&CB4==0)||(CB3==0&&CB4==0&&C B5==0)||(CB5==0&&CB6==0&&CB7==0)||(CB4==0&&CB5==0& &CB6==0));
}
VOID TOC_DO (VOID)
{
DEM_CHECK_POINT ();
WHILE(TRUE)
{
DEM_CHECK_POINT ();
IF(CB2==0&&CB3==1&&CB4==1&&CB5==1&&CB6==1&&CB7==1)
{
SET_POWER_PWM0_DUTY (BANH_TRAI [5]);
SET_POWER_PWM2_DUTY (BANH_PHAI [1]);
}
IF((CB2==1&&CB3==0&&CB4==1&&CB5==1&&CB6==1&&CB7==1 )||(CB2==0&&CB3==0&&CB4==1&&CB5==1&&CB6==1&&CB7==1 ))
{
SET_POWER_PWM0_DUTY (BANH_TRAI [4]);
SET_POWER_PWM2_DUTY (BANH_PHAI [1]);
}
IF((CB2==1&&CB3==0&&CB4==1&&CB5==1&&CB6==1&&CB7==1 )||(CB2==1&&CB3==0&&CB4==0&&CB5==1&&CB6==1&&CB7==1 ))
{
SET_POWER_PWM0_DUTY (BANH_TRAI [3]);
SET_POWER_PWM2_DUTY (BANH_PHAI [1]);
}
IF((CB2==1&&CB3==1&&CB4==0&&CB5==1&&CB6==1&&CB7==1 )||(CB2==1&&CB3==1&&CB4==0&&CB5==0&&CB6==1&&CB7==1 )||(CB2==1&&CB3==1&&CB4==1&&CB5==0&&CB6==1&&CB7==1 ))
{
SET_POWER_PWM0_DUTY (BANH_TRAI [5]);
SET_POWER_PWM2_DUTY (BANH_PHAI [5]);
}
IF((CB2==1&&CB3==1&&CB4==1&&CB5==0&&CB6==0&&CB7==1 )||(CB2==1&&CB3==1&&CB4==1&&CB5==1&&CB6==0&&CB7==1 ))
{
SET_POWER_PWM0_DUTY (BANH_TRAI [1]);
SET_POWER_PWM2_DUTY (BANH_PHAI [4]);
}
IF(CB2==1&&CB3==1&&CB4==1&&CB5==0&&CB6==1&&CB7==1)
{
SET_POWER_PWM0_DUTY (BANH_TRAI [1]);
SET_POWER_PWM2_DUTY (BANH_PHAI [3]);
}
IF((CB2==1&&CB3==1&&CB4==1&&CB5==1&&CB6==1&&CB7==0 )||(CB2==1&&CB3==1&&CB4==1&&CB5==1&&CB6==0&&CB7==0 ))
{
SET_POWER_PWM0_DUTY (BANH_TRAI [1]);
SET_POWER_PWM2_DUTY (BANH_PHAI [5]);
}
}
WHILE(I==2||I==3||I==6||I==7||I==10||I==11||I==14| |I==15)
{
DEM_CHECK_POINT ();
IF(CB2==1&&CB3==1&&CB4==1&&CB5==1&&CB6==1&&CB7==0)
{
SET_POWER_PWM0_DUTY (BANH_TRAI [7]);
SET_POWER_PWM2_DUTY (BANH_PHAI [2]);
}
IF((CB2==1&&CB3==1&&CB4==1&&CB5==1&&CB6==0&&CB7==0 )||(CB2==1&&CB3==1&&CB4==1&&CB5==1&&CB6==0&&CB7==1 ))
{
SET_POWER_PWM0_DUTY (BANH_TRAI [7]);
SET_POWER_PWM2_DUTY (BANH_PHAI [3]);
}
IF((CB2==1&&CB3==1&&CB4==1&&CB5==0&&CB6==1&&CB7==1 )||(CB2==1&&CB3==1&&CB4==0&&CB5==1&&CB6==1&&CB7==1 )||(CB2==1&&CB3==1&&CB4==0&&CB5==0&&CB6==1&&CB7==1 ))
{
SET_POWER_PWM0_DUTY (BANH_TRAI [7]);
SET_POWER_PWM2_DUTY (BANH_PHAI [7]);
}
IF(CB2==1&&CB3==1&&CB4==1&&CB5==0&&CB6==0&&CB7==1)
{
SET_POWER_PWM0_DUTY (BANH_TRAI [7]);
SET_POWER_PWM2_DUTY (BANH_PHAI [4]);
}

IF((CB2==1&&CB3==0&&CB4==0&&CB5==1&&CB6==1&&CB7==1 )||(CB2==1&&CB3==0&&CB4==1&&CB5==1&&CB6==1&&CB7==1 ))
{
SET_POWER_PWM0_DUTY (BANH_TRAI [4]);
SET_POWER_PWM2_DUTY (BANH_PHAI [7]);
}
IF((CB2==0&&CB3==0&&CB4==1&&CB5==1&&CB6==1&&CB7==1 )||(CB2==0&&CB3==1&&CB4==1&&CB5==1&&CB6==1&&CB7==1 ))
{
SET_POWER_PWM0_DUTY (BANH_TRAI [2]);
SET_POWER_PWM2_DUTY (BANH_PHAI [7]);
}

}
}

// CHUONG TRINH CHINH //

VOID MAIN ()
{

SETUP_POWER_PWM_PINS (PWM_BOTH_ON, PWM_BOTH_ON, PWM_BOTH_ON, PWM_BOTH_ON, );
SETUP_POWER_PWM(PWM_CLOCK_DIV_4|PWM_FREE_RUN,1,0,2 50,0,1,0);
SET_TRIS_A (0XFF);
SET_TRIS_B (0B00000000);
SET_TRIS_C (0B10001000);
SET_TRIS_D (0B00000011);
SET_TRIS_E (0B111);
I=0;
// WHILE(TRUE)
// {
// DEM_T0 = GET_RTCC ();
// LCD_PUTCMD (0X80);
// PRINTF ( LCD_PUTCHAR ,"SO CP = %D",DEM_T0);
// IF(NUTNHAN == 0) GOTO BATDAU;
SET_POWER_PWM0_DUTY (BANH_TRAI [1]);
SET_POWER_PWM2_DUTY (BANH_PHAI [1]);
// }
// BATDAU:I=0;
WHILE(TRUE)
{
DEM_CHECK_POINT ();
TOC_DO ();
}
}