help ! robot dò đường dùng 16f877a

[tuonghiendk7.1]

Nguyên văn bởi l6nngu

ko ai giúp e à

#INCLUDE <18F4431.H>
#DEVICE *=16
#FUSES HS,NOLVP,NOWDT,NOPROTECT
#USE DELAY(clock=20000000)

#BYTE PORTA=0XF80
#BYTE PORTB=0XF81
#BYTE PORTC=0XF82
#byte PORTD=0XF83
#BYTE PORTE=0XF84

#BIT R4 = PORTA.0
#BIT R3 = PORTA.1
#BIT R2 = PORTA.2
#BIT R1 = PORTA.3
//---------------
#BIT L1 = PORTA.4
#BIT L2 = PORTA.5
#BIT L3 = PORTE.0
#BIT L4 = PORTE.1


#BIT CBANH_TRAI = PORTB.2
#BIT CBANH_PHAI = PORTB.0
#BIT LED1 = PORTD.0
#BIT LED2 = PORTD.1
#BIT LED3 = PORTD.2


#DEFINE QUAY_THUAN 0
#DEFINE QUAY_NGUOC 1


VOID SO_CB_TICH_CUC (VOID);
VOID DEM_VACH_NGANG (VOID);
VOID DO_DUONG (VOID);

INT8 TH,SO_VACH,NL,NR;
INT16 BANH_TRAI,BANH_PHAI ;
//0----1---2---3---4---5---6---7---8---9---10-11---12-13
INT16 B_TRAI [14] = {1023,970,780,700,600,550,480,450,400,300,270,220, 170,5};
INT16 B_PHAI [14] = {1023,950,760,730,580,530,460,430,380,280,250,200, 150,5};


VOID SO_CB_TICH_CUC ()
{
TH = 0;
IF (R3 == 0 ) TH ++;
IF (R2 == 0 ) TH ++;
IF (R1 == 0 ) TH ++;
IF (L1 == 0 ) TH ++;
IF (L2 == 0 ) TH ++;
IF (L3 == 0 ) TH ++;
}

VOID DEM_VACH_NGANG ()
{
SO_CB_TICH_CUC ();
IF (TH >=5 )
{
LOVE:
SO_CB_TICH_CUC ();
IF (TH >3 ) GOTO LOVE;

ELSE
{
SO_VACH ++;
IF (SO_VACH == 1 ) LED1 = 1 ;
ELSE IF (SO_VACH == 2 ) LED2 = 1 ;
ELSE IF (SO_VACH == 3 ) LED3 = 1 ;
ELSE LED1=LED2=LED3 =0;
}

}

}

VOID DO_DUONG ()
{
NL =NR = 0;
IF ( R1 == 0 && L1 ==0 || R1 == 0 ||L1 ==0 ) //11 10 01 11
{
BANH_TRAI = B_TRAI [2];
BANH_PHAI = B_PHAI [2];
NL= NR = 0;
}
//-----------LECH TRAI----------------------------------------------------------
ELSE IF ( L2 ==0 && L1 == 0|| L2 ==0) // 11 00 11 11
{
BANH_TRAI = B_TRAI [3];
BANH_PHAI = B_PHAI [2];
NL = 1;
}
ELSE IF ( L3 == 0 && L2 ==0|| L3 ==0 ) // 10 01 11 11
{
BANH_TRAI = B_TRAI [4];
BANH_PHAI = B_PHAI [2];
NL = 2;
}
ELSE IF (L3 == 0 && L4 ==0) // 00 11 11 11
{
BANH_TRAI = B_TRAI [5];
BANH_PHAI = B_PHAI [1];
NL = 3;
}
ELSE IF (L4==0) // 01 11 11 11
{
BANH_TRAI = B_TRAI [6];
BANH_PHAI = B_PHAI [0];
NL = 4;
}
//-----------LECH PHAI----------------------------------------------------------
ELSE IF ( R1 ==0 && R2 ==0 ||R2 ==0) // 11 10 01 11
{
BANH_TRAI = B_TRAI [2];
BANH_PHAI = B_PHAI [3];
NR =1 ;
}
ELSE IF ( R3 ==0&& R2 ==0||R3 ==0) // 11 11 10 01
{
BANH_TRAI = B_TRAI [2];
BANH_PHAI = B_PHAI [4];
NR =2 ;
}
ELSE IF ( R3 ==0 && R4 ==0 ) // 11 11 11 00
{
BANH_TRAI = B_TRAI [1];
BANH_PHAI = B_PHAI [5];
NR =3 ;
}
ELSE IF ( R4 ==0) // 11 11 11 10
{
BANH_TRAI = B_TRAI [0];
BANH_PHAI = B_PHAI [6];
NR = 4 ;
}
//---------------NHO DUONG------------------------------------------------------

ELSE IF ( PORTA ==0B111111 && PORTE == 0B111 && NL == 4 && NR ==0)
{
BANH_TRAI = B_TRAI [0];
BANH_PHAI = B_PHAI [6];

}
ELSE IF ( PORTA ==0B111111 && PORTE == 0B111 && NL == 0 && NR ==4)
{
BANH_TRAI = B_TRAI [6];
BANH_PHAI = B_PHAI [0];
}
ELSE IF (PORTA == 0B111111 && PORTE == 0B111)
{
BANH_TRAI = B_TRAI [0];
BANH_PHAI = B_PHAI [0];
}

//------------XUAT XUNG---------------------------------------------------------
SET_POWER_PWM0_DUTY (BANH_PHAI);
SET_POWER_PWM2_DUTY (BANH_TRAI);
}
//------------------------------------------------------------------------------
VOID MAIN ()
{
SET_TRIS_A (0B111111);
SET_TRIS_B (0X00);
SET_TRIS_C (0XFF);
SET_TRIS_D (0X00);
SET_TRIS_E (0X7);
SETUP_POWER_PWM_PINS (PWM_ODD_ON,PWM_ODD_ON,PWM_ODD_ON,PWM_ODD_ON );
SETUP_POWER_PWM (PWM_CLOCK_DIV_16|PWM_FREE_RUN,1,0,250,0,1,0);
SET_POWER_PWM2_DUTY (1023);
SET_POWER_PWM0_DUTY (1023);

CBANH_TRAI = QUAY_THUAN ;
CBANH_PHAI = QUAY_THUAN ;
SO_VACH = 0;
//------------------------------------------------------------------------------
WHILE (TRUE)
{
CBANH_TRAI = QUAY_THUAN ;
CBANH_PHAI = QUAY_THUAN ;
SO_VACH = 0;
WHILE (SO_VACH <=3)
{
DO_DUONG ();
DEM_VACH_NGANG ();
}
SET_POWER_PWM2_DUTY (1023);
SET_POWER_PWM0_DUTY (1023);


}

}
đây là một bài toán dò đường mà mình đã hoàn thiện dùng trong robocon bạn tham khảo nhé !