小车巡线代码

单片机是正点原子STM32，三路传感器

#include "stm32f10x.h"
#include "L298N.h"
#include "sys.h"
#include "delay.h"
#include "LED.h"
#include "KEY.h"
#include "SENSOR.h"

int MortorRun;

int main(void)
{
  L298N_Init(7199,0);
	delay_init();
	KEY_Init();
	Init_LED();
	Init_SENSOR();
	//***********LRK**************//
	//传感器PG3---PG4---PG5
	while(1)
	{
    if((PGin(3)==0)&&(PGin(4)==0)&&(PGin(5)==0))
		{
      SetPWM_MotorB(0);
			SetPWM_MotorA(0);
			LED1=1;
			LED0=1;
		}
		if((PGin(5)!=0)&&(PGin(4)==0)&&(PGin(3)==0))//大幅度右转
		{
			SetPWM_MotorB(3000);
			SetPWM_MotorA(4900);
		}
		if((PGin(3)!=0)&&(PGin(4)==0)&&(PGin(5)==0))//大幅度左转
		{
      SetPWM_MotorB(4800);
			SetPWM_MotorA(2600);
		}
		if((PGin(5)!=0)&&(PGin(4)!=0)&&(PGin(3)==0))//绿灯亮，右转
		{
			SetPWM_MotorB(3000);
			SetPWM_MotorA(3500);
			LED1=0;
			LED0=1;
		}
		if((PGin(3)!=0)&&(PGin(4)!=0)&&(PGin(5)==0))//红灯亮，左转
		{
      SetPWM_MotorB(3300);
			SetPWM_MotorA(2600);
			LED1=1;
			LED0=0;
		}
		 if((PGin(4)!=0)&&(PGin(3)==0)&&(PGin(5)==0))//LED两灯亮，直走
		{
      SetPWM_MotorB(4000);
			SetPWM_MotorA(3750);
			LED1=0;
			LED0=0;
		}
	} 
}