void main(void)
{
vInitialize();
myInit();
while(!pushsw_get())
{
if(cnt1<100)
led_out(0x1);
else if(cnt1<200)
led_out(0x2);
else cnt1 = 0;
}
cnt1 = 0;
while(startbar_get())
{
if(cnt1<50)
led_out(0x1);
else if(cnt1<100)
led_out(0x2);
else cnt1 = 0;
}
led_out(0);
start();
pattern = 10;
while(1)
{
switch(pattern)
{
case 10: //直线
if(check_crossline()){ pattern = 30;break; }
if(check_blackArea()){ pattern = 40;break; }
straight_run(); //直线前行，弯道检测，盲区检测
break;
case 20: //弯道
switch(turn_direction)
{
case LEFT:
handle(-1*midDegree_st4);
left_turn();
break;
case RIGHT:
handle(midDegree_st4);
right_turn();
break;
default:break;
}
break;
case 30: //直角
rightAngle();
break;
case 40: //盲区
switch(blackArea_direction)
{
case LEFT:
left_blackArea();
break;
case RIGHT:
right_blackArea();
break;
default:break;
}
break;
default:break;
}//end switch
}//end while
}
int main()
{
init_MCU();
sel_mode();
while (1)
{
switch (pattern)
{
case 1:
if ( check_crossline() ) //Be 90
{
pattern = 21;
break;
}
if ( check_rightline() ) // Chuyen lan phai
{
pattern = 51;
break;
}
if ( check_leftline() ) // Chuyen lan trai
{
pattern = 61;
break;
}
led7(10);
set_encoder(19);
switch (doc_cam_bien(0b01111110))
{
case 0b01111110:
toc_do(0, 0);
goc(0);
break;
case 0b00011000: // Chay thang
goc( 0 );
toc_do( 100 , 100 );
break;
//lech phai
case 0b00011100:
case 0b00001000:
toc_do(90, 80);
goc(8);
break;
case 0b00001100:
toc_do(90, 70);
goc(10);
break;
case 0b00001110:
case 0b00000100:
toc_do(90, 60);
goc(12);
break;
case 0b00000110:
toc_do(90, 50);
goc(15);
break;
case 0b00000010:
toc_do(90, 30);
goc(20);
pattern = 11; //Lệch phải góc lớn
break;
//////////////////////////////////////////
//lech trai
case 0b00111000:
case 0b00010000:
toc_do(80, 90);
goc(-8);
break;
case 0b00110000:
toc_do(70, 90);
goc(-10);
break;
case 0b01110000:
case 0b00100000:
toc_do(60, 90);
goc(-12);
break;
case 0b01100000:
toc_do(50, 90);
goc(-15);
break;
case 0b01000000:
toc_do(30, 90);
goc(-20);
pattern = 12; //Lệch trái góc lớn
break;
default:
break;
}
break;
case 11: //Lệch phải góc lớn
led7(11);
set_encoder(23);
switch (doc_cam_bien(0b11001100))
{
case 0b11000000:
toc_do(95, 15);
goc(75);
break;
case 0b10000000:
toc_do(95, 20);
goc(51);
break;
case 0b00000000:
toc_do(95, 25);
goc(48);
break;
case 0b00000100:
toc_do(95, 25);
goc(37);
break;
case 0b00001100:
toc_do(95, 25);
goc(29);
pattern = 1;
break;
default: pattern = 11;
}
break;
case 12: //Lệch trái góc lớn
led7(12);
set_encoder(23);
switch (doc_cam_bien(0b00110011))
{
case 0b00000011:
toc_do(15, 95);
goc(-73);
break;
case 0b00000001:
toc_do(20, 95);
goc(-50);
break;
case 0b00000000:
toc_do(25, 95);
goc(-47);
break;
case 0b00100000:
toc_do(25, 95);
goc(-36);
break;
case 0b00110000:
toc_do(25, 95);
goc(-28);
pattern = 1;
break;
default: pattern = 12;
}
break;
case 21: //Xử lý khi gặp vạch tín hiệu cua vuông đầu tiên
led7(18);
set_encoder(20);
goc( 0 );
toc_do( 60 , 60);
pattern = 22;
cnt_1ms = 0;
xung = 0;
break;
case 22: // Delay hết vạch
led7(22);
set_encoder(20);
goc( 0 );
toc_do( 60 , 60);
if ( cnt_1ms > 150 || xung > 10 )
{
pattern = 23;
cnt_1ms = 0;
xung = 0;
}
break;
case 23:
led7(23);
//cua trái
if ( (xung > 100) && ((doc_cam_bien(0b11111111) == 0b11111000) || (doc_cam_bien(0b11111111) == 0b11110000) || (doc_cam_bien(0b11111111) == 0b11100000) || (doc_cam_bien(0b11111111) == 0b11111100))) // Neu gap tin hieu nay la goc cua 90 trai thi be
{
pattern = 24;
xung = 0;
cnt_1ms = 0;
break;
}
//cua phải
if ( (xung > 100) && ((doc_cam_bien(0b11111111) == 0b00011111 ) || (doc_cam_bien(0b11111111) == 0b00000111) || (doc_cam_bien(0b11111111) == 0b00001111) || (doc_cam_bien(0b11111111) == 0b00111111))) // Neu gap tin hieu nay la goc cua 90 phai thi be
{
pattern = 25;
xung = 0;
cnt_1ms = 0;
break;
}
if ( (xung > 30) && (xung < 100) && check_crossline() == 1)
{
pattern = 71;
xung = 0;
cnt_1ms = 0;
break;
}
// Nguoc lai thi chinh thang cho xe
switch (doc_cam_bien(0b01111110))
{
case 0b00011000:
goc( 0 );
toc_do(30, 30 );
break;
//lech phai
case 0b00001000:
toc_do(23, 3);
goc(8);
break;
case 0b00001100:
toc_do(23, -3);
goc(10);
break;
case 0b00000100:
toc_do(23, -8);
goc(13);
break;
case 0b00000110:
case 0b00000010:
toc_do(23, -13);
goc(18);
break;
////////////////////////////////
case 0b00010000:
toc_do(5, 20);
goc(-8);
break;
case 0b00110000:
toc_do(0, 20);
goc(-10);
break;
case 0b00100000:
toc_do(-5, 20);
goc(-13);
break;
case 0b01100000:
case 0b01000000:
toc_do(10, 20);
goc(-18);
break;
default:
break;
}
break;
case 24://delay het vach line TRAI
set_encoder(10);
toc_do(50, 50);
if ( (xung > 100) && ((doc_cam_bien(0b00000111) == 0b00000111) || (doc_cam_bien(0b00001111) == 0b00001111)))
{
pattern = 23 ;
break;
}
if (cnt_1ms > 100 || xung > 1)
{
pattern = 26;
break;
}
break;
case 25: //delay het vach line PHAI
set_encoder(10);
toc_do(50, 50);
if ( (xung > 100 ) && ((doc_cam_bien(0b11100000) == 0b11100000) || (doc_cam_bien(0b11110000) == 0b11110000)))
{
pattern = 23 ;
break;
}
if (cnt_1ms > 100 || xung > 1)
{
pattern = 27;
break;
}
break;
case 26: //vuông trái
led7(24);
goc( - 140 );
set_encoder(8);
toc_do( -20 , 90 );
led7(1);
pattern = 31;
cnt_1ms = 0;
break;
case 27://vuông phải
led7(25);
goc( 140 );
set_encoder(8);
toc_do( 90 , -20 );
led7(2);
pattern = 41;
cnt_1ms = 0;
break;
case 31: //delay cho xe kịp bẻ vuông trái
led7(31);
if ( cnt_1ms > 50 )
{
pattern = 32;
cnt_1ms = 0;
}
break;
case 32: //chờ tín hiệu để về trường hợp chạy thẳng
led7(32);
if ( doc_cam_bien(0b11100111) == 0b00100000 )
{
pattern = 1;
cnt_1ms = 0;
}
break;
case 41: //delay cho xe kịp bẻ vuông phải
led7(41);
if ( cnt_1ms > 50 )
{
pattern = 42;
cnt_1ms = 0;
}
break;
case 42: //chờ tín hiệu để về trường hợp chạy thẳng
led7(42);
if ( doc_cam_bien(0b11100111) == 0b00000100 )
{
pattern = 1;
cnt_1ms = 0;
}
break;
case 51: //xử lý khi gặp vạch tín hiệu chuyển làn PHẢI
set_encoder(20);
if ((doc_cam_bien(0b11100000) == 0b11100000) || (doc_cam_bien(0b11110000) == 0b11110000))
{
pattern = 21 ;
break;
}
led7(51);
goc( 0 );
toc_do( 70 , 70 );
pattern = 52;
cnt_1ms = 0;
xung = 0;
break;
case 52: //delay hết vạch
if ( cnt_1ms > 100 || xung > 10)
{
pattern = 53;
cnt_1ms = 0;
}
if ((doc_cam_bien(0b11100000) == 0b11100000) || (doc_cam_bien(0b11110000) == 0b11110000))
{
pattern = 21 ;
break;
}
break;
case 53: //hết vạch trắng
if ( doc_cam_bien(0b11111111) == 0b00000000 )
{
goc( 75 );
toc_do( 60 , 50 );
pattern = 54;
cnt_1ms = 0;
break;
}
switch ( doc_cam_bien(0b11100111) )
{
case 0x00:
goc( 0 );
toc_do( 30 , 30 );
break;
case 0x04:
case 0x06:
case 0x07:
case 0x03:
goc( 15 );
toc_do( 30 , 20 );
break;
case 0x20:
case 0x60:
case 0xe0:
case 0xc0:
goc( -15 );
toc_do( 20 , 30 );
break;
default:
break;
}
break;
case 54: //bắt được line, chuyển về chạy thẳng
if ( doc_cam_bien( 0b00001100 ) == 0b00001100 )
{
pattern = 1;
cnt_1ms = 0;
led7(0);
}
break;
case 61: //xử lý khi gặp vạch tín hiệu chuyển làn TRÁI
set_encoder(20);
if ( (doc_cam_bien(0b00000111) == 0b00000111) || (doc_cam_bien(0b00001111) == 0b00001111) )
{
pattern = 21 ;
break;
}
led7(61);
goc( 0 );
toc_do( 90 , 90 );
pattern = 62;
cnt_1ms = 0;
xung = 0;
break;
case 62: //delay hết vạch
if ( cnt_1ms > 100 || xung > 10)
{
pattern = 63;
cnt_1ms = 0;
}
if ((doc_cam_bien(0b00000111) == 0b00000111) || (doc_cam_bien(0b00001111) == 0b00001111))
{
pattern = 21 ;
break;
}
break;
case 63: //hết vạch trắng
if ( doc_cam_bien(0b11111111) == 0x00 )
{
goc( -75 );
toc_do( 50 , 60 );
pattern = 64;
cnt_1ms = 0;
break;
}
switch ( doc_cam_bien(0b11100111) )
{
case 0x00:
goc( 0 );
toc_do( 30 , 30 );
break;
case 0x04:
case 0x06:
case 0x07:
case 0x03:
goc( 15 );
toc_do( 30 , 20 );
break;
case 0x20:
case 0x60:
case 0xe0:
case 0xc0:
goc( -15 );
toc_do( 20 , 30 );
break;
default:
break;
}
break;
case 64: //bắt được line, chuyển về chạy thẳng
if ( doc_cam_bien( 0b00110000 ) == 0b00110000 )
{
pattern = 1;
cnt_1ms = 0;
led7(0);
}
break;
case 71: // Delay het vach
set_encoder(30);
toc_do(100, 100);
led7(71);
if ( cnt_1ms > 100 || xung > 10)
{
pattern = 72;
cnt_1ms = 0;
}
break;
case 72:
if (doc_cam_bien(0xff) == 0b00000000)
{
goc(0);
toc_do(100, 100);
pattern = 73;
break;
}
switch (doc_cam_bien(0b01111110))
{
case 0b00011000:
goc( 0 );
toc_do(100, 100 );
break;
//lech phai
case 0b00001000:
toc_do(90, 17);
goc(8);
break;
case 0b00001100:
toc_do(90, 70);
goc(13);
break;
case 0b00000100:
toc_do(90, 65);
goc(18);
break;
case 0b00000110:
case 0b00000010:
toc_do(90, 60);
goc(26);
break;
//////////////////////////////////////////////////////////////////////////
case 0b00010000:
toc_do(75, 90);
goc(-8);
break;
case 0b00110000:
toc_do(70, 90);
goc(-13);
break;
case 0b00100000:
toc_do(65, 90);
goc(-18);
break;
case 0b01100000:
case 0b01000000:
toc_do(60, 90);
goc(-26);
break;
default:
break;
}
break;
case 73:
goc(0);
toc_do(100, 100);
if (doc_cam_bien(0b11111111) > 0)
{
pattern = 1;
}
break;
default:
pattern = 1;
break;
}
}
}
int main(void)
{
Init();
ADC_init();
Sel_Mode();
while(1)
{
switch(pattern)
{
case 1:
if( check_crossline() ) { /*Be 90*/
pattern = 21;
break;
}
if( check_rightline() ) { /* Chuyen lan phai */
pattern = 51;
break;
}
if( check_leftline() ) { /* Chuyen lan trai */
pattern = 61;
break;
}
switch( sensor_inp(MASK3_3) ) {
case 0x00:
/* Chay thang */
handle( 0 );
speed( 100 ,100 );
break;
case 0x04:
/* Be phai nhe */
handle( 5 );
speed( 100 ,100 );
break;
case 0x06:
/* Be phai goc nho */
handle( 10 );
speed( 80 ,67 );
break;
case 0x07:
/* Be phai goc trung binh */
handle( 15 );
speed( 50 ,38 );
break;
case 0x03:
/* Be phai goc lon */
handle( 25 );
speed( 30 ,19 );
pattern = 12;
break;
case 0x20:
/* Be trai nhe */
handle( -5 );
speed( 100 ,100 );
break;
case 0x60:
/* Be trai goc nho */
handle( -10 );
speed( 67 ,80 );
break;
case 0xe0:
/* Be trai goc trung binh */
handle( -15 );
speed( 38 ,50 );
break;
case 0xc0:
/* Be trai goc lon */
handle( -25 );
speed( 19 ,30 );
pattern = 13;
break;
default:
break;
}
break;
case 12:
/* Kiem tra trong luc be qua ben phai voi goc lon */
if( check_crossline() ) { /* Trong khi be thi kiem tra tin hieu 90 */
pattern = 21;
break;
}
if( check_rightline() ) { /* Trong khi be thi kiem tra tin hieu chuyen lan phai */
pattern = 51;
break;
}
if( check_leftline() ) { /* Trong khi be thi kiem tra tin hieu chuyen lan trai */
pattern = 61;
break;
}
if( sensor_inp(MASK3_3) == 0x06 ) { //Neu gap truong hop nay thi tro ve xu ly duong thang va duong cong
pattern = 1;
}
break;
case 13:
/* Kiem tra trong luc be qua ben trai voi goc lon */
if( check_crossline() ) { /* Trong khi be thi kiem tra tin hieu 90 */
pattern = 21;
break;
}
if( check_rightline() ) { /* Trong khi be thi kiem tra tin hieu chuyen lan phai */
pattern = 51;
break;
}
if( check_leftline() ) { /* Trong khi be thi kiem tra tin hieu chuyen lan trai */
pattern = 61;
break;
}
if( sensor_inp(MASK3_3) == 0x60 ) { //Neu gap truong hop nay thi tro ve xu ly duong thang va duong cong
pattern = 1;
}
break;
case 21:
/* Xu ly khi gap vach tin hieu 90 dau tien */
handle( 0 );
speed( 0 ,0 );
pattern = 22;
cnt1 = 0;
break;
case 22:
/* Cho 100ms de bo qua vach tin hieu thu 2 */
if( cnt1 > 100 ) {
pattern = 23;
cnt1 = 0;
}
break;
case 23:
/* Xu ly sau khi da qua 2 vach tin hieu */
if( sensor_inp(MASK4_4)==0xf8 ) {
/* Neu gap tin hieu nay la goc cua 90 trai thi be */
handle( -38 );
speed( 10 ,50 );
pattern = 31;
cnt1 = 0;
break;
}
if( sensor_inp(MASK4_4)==0x1f ) {
/* Neu gap tin hieu nay la goc cua 90 phai thi be */
handle( 38 );
speed( 50 ,10 );
pattern = 41;
cnt1 = 0;
break;
}
/* Nguoc lai thi chinh thang cho xe */
switch( sensor_inp(MASK3_3) ) {
case 0x00:
handle( 0 );
speed( 40 ,40 );
break;
case 0x04:
case 0x06:
case 0x07:
case 0x03:
handle( 8 );
speed( 40 ,35 );
break;
case 0x20:
case 0x60:
case 0xe0:
case 0xc0:
handle( -8 );
speed( 35 ,40 );
break;
}
break;
case 31:
/* Cho 200ms de xe kip be cua 90 */
if( cnt1 > 200 ) {
pattern = 32;
cnt1 = 0;
}
break;
case 32:
/* Cho tin hieu de ve truong hop chay thang va be cong */
if( sensor_inp(MASK3_3) == 0x60 ) {
pattern = 1;
cnt1 = 0;
}
break;
case 41:
if( cnt1 > 200 ) {
pattern = 42;
cnt1 = 0;
}
break;
case 42:
if( sensor_inp(MASK3_3) == 0x06 ) {
pattern = 1;
cnt1 = 0;
}
break;
case 51:
/* Xu ly khi gap vach tin hieu chuyen lan phai dau tien */
handle( 0 );
speed( 0 ,0 );
pattern = 52;
cnt1 = 0;
break;
case 52:
if( cnt1 > 100 ) {
pattern = 53;
cnt1 = 0;
}
break;
case 53:
if( sensor_inp(MASK4_4) == 0x00 ) {
handle( 15 );
speed( 40 ,31 );
pattern = 54;
cnt1 = 0;
break;
}
switch( sensor_inp(MASK3_3) ) {
case 0x00:
handle( 0 );
speed( 40 ,40 );
break;
case 0x04:
case 0x06:
case 0x07:
case 0x03:
handle( 8 );
speed( 40 ,35 );
break;
case 0x20:
case 0x60:
case 0xe0:
case 0xc0:
handle( -8 );
speed( 35 ,40 );
break;
default:
break;
}
break;
case 54:
if( sensor_inp( MASK4_4 ) == 0x3c ) {
pattern = 1;
cnt1 = 0;
}
break;
case 61:
/* Xu ly khi gap vach tin hieu chuyen lan trai dau tien */
handle( 0 );
speed( 0 ,0 );
pattern = 62;
cnt1 = 0;
break;
case 62:
if( cnt1 > 100 ) {
pattern = 63;
cnt1 = 0;
}
break;
case 63:
if( sensor_inp(MASK4_4) == 0x00 ) {
handle( -15 );
speed( 31 ,40 );
pattern = 64;
cnt1 = 0;
break;
}
switch( sensor_inp(MASK3_3) ) {
case 0x00:
handle( 0 );
speed( 40 ,40 );
break;
case 0x04:
case 0x06:
case 0x07:
case 0x03:
handle( 8 );
speed( 40 ,35 );
break;
case 0x20:
case 0x60:
case 0xe0:
case 0xc0:
handle( -8 );
speed( 35 ,40 );
break;
default:
break;
}
break;
case 64:
if( sensor_inp( MASK4_4 ) == 0x3c ) {
pattern = 1;
cnt1 = 0;
}
break;
default:
pattern = 1;
break;
}
}
}
