巡线小车直角转弯

int main(void)
{
  L298N_Init(7199,0);
	delay_init();
	KEY_Init();
	Init_LED();
	Init_SENSOR();
	
	//***********LRK**************//
	//Ö÷´«¸ÐÆ÷ PG2--PG3---PG4---PG5
	//          µç»úA       µç»úB
	//¸±´«¸ÐÆ÷ PG13--PG14
	//ÓÒת×ó±ßµç»ú¿ì/×óתÓұߵç»ú¿ì
	while(1)
	{
		
		
		//Ö÷¡¢¸±´«¸ÐÆ÷²»ÁÁ£¬Ð¡³µÍêÈ«Í£Ö¹
		if((PGin(2)==0)
			&&(PGin(3)==0)
		  &&(PGin(4)==0)
		  &&(PGin(5)==0)
		  &&(PGin(13)==0)
		  &&(PGin(14)==0))
		{
			//Í£Ö¹¡£Á½µÆ°µ
			LED1=1;
			LED0=1;
			//Í£Ö¹
			SetPWM_MotorA(0);
			SetPWM_MotorB(0);
		}
		
		
		
		//Ö÷´«¸ÐÆ÷ÓÐÈÎÒâһ·ÏìÓ¦£¬¸±´«¸ÐÆ÷ÎÞÏìÓ¦---(ѲÏß)
		if(((PGin(2)!=0)
		 ||(PGin(3)!=0)
		 ||(PGin(4)!=0)
		 ||(PGin(5)!=0))
		 &&((PGin(13)==0)
		 &&(PGin(14)==0)))
		{
			//ѲÏß,Â̵ÆÁÁ
			LED1=0;
			LED0=1;
			//ѲÏß
			if((PGin(3)!=0)
				&&(PGin(4)!=0)
			  &&(PGin(5)==0)
        &&(PGin(2)==0))
			{ 
				//Ö±Ïß
				SetPWM_MotorA(4000);
				SetPWM_MotorB(4000);
			} 
			if((PGin(3)==0)
			  &&(PGin(4)==0)
			  &&(PGin(5)==0)
        &&(PGin(2)!=0))
			{
				//´ó·ù¶ÈÓÒת
				SetPWM_MotorA(4000);
				SetPWM_MotorB(1000);
			}
	    if((PGin(3)==0)
			  &&(PGin(4)==0)
			  &&(PGin(5)!=0)
        &&(PGin(2)==0))
			{
				//´ó·ù¶È×óת
				SetPWM_MotorA(1000);
				SetPWM_MotorB(4000);
			}
			if((PGin(3)==0)
			  &&(PGin(4)!=0)
			  &&(PGin(5)!=0)
        &&(PGin(2)==0))
			{
				//Öзù¶ÈÓÒת
				SetPWM_MotorA(4000);
				SetPWM_MotorB(2500);
			}
			if((PGin(3)!=0)
			  &&(PGin(4)==0)
			  &&(PGin(5)==0)
        &&(PGin(2)!=0))
			{
				//Öзù¶È×óת
				SetPWM_MotorA(2500);
				SetPWM_MotorB(4000);
			}
			if((PGin(3)!=0)
			  &&(PGin(4)!=0)
			  &&(PGin(5)!=0)
        &&(PGin(2)==0))
			{
				//С·ù¶ÈÓÒת
				SetPWM_MotorA(4000);
				SetPWM_MotorB(3400);
			}
			if((PGin(3)!=0)
			  &&(PGin(4)!=0)
			  &&(PGin(5)==0)
        &&(PGin(2)!=0))
			{
				//С·ù¶È×óת
				SetPWM_MotorA(3400);
				SetPWM_MotorB(4000);
			}
			else
			{
				//ºóÍË
				SetPWM_MotorA(-2000);
				SetPWM_MotorB(-2000);
			}
			

		}	
		
		
		
		//¸±´«¸ÐÆ÷ÁÁ£¬Ö±½ÇתÍ䣬×óת
		if((PGin(14)!=0)||(PGin(14)!=0))
		{
			//Ö±½ÇתÍä
			SetPWM_MotorB(0);
			SetPWM_MotorA(0);
			//С³µÍ£Ö¹
			delay_ms(1000);
			//µÈ1Ãë,µÈС³µÍ£ÎÈ
			while(1)
			{
				//Ö±½ÇתÍ䣬ºìµÆÁÁ
			  LED0=0;
			  LED1=1;
				//¿ªÊ¼Ö±½ÇתÍä
			  SetPWM_MotorB(6000);
			  SetPWM_MotorA(0);
				if((PGin(2)!=0)
		       ||(PGin(3)!=0)
		       ||(PGin(4)!=0)
		       ||(PGin(5)!=0))
				{
					break;
				}
			}
		}
		
		
		
	}
}