C51--摇头测距小车

摇头测距小车——舵机和超声波封装

#include "reg52.h"

#include "HC04.h"
#include "Delay.h"
#include "sg90.h"
#include "motor.h"


#define MIDDLE 0
#define LEFT   1
#define RIGHT  2

void main()
{
	char dir;
	
	double disMiddle;//距离
	double disLeft;
	double disRight;
	
	Time0Init();//定时器0初始化
	Time1Init();//定时器1初始化
	
	sg_Middle();			//正前方
	Delay300ms();
	Delay300ms();
	
	dir = MIDDLE;
	
	while(1){
		
		if(dir != MIDDLE){
			sg_Middle();			//正前方
			dir = MIDDLE;
			Delay300ms();
		}
		disMiddle = get_distance();//超声波测距

		if(disMiddle > 35){					
			//前进
			go_Forward();
			
		}else{		
			//停止，摇头左边测距
			go_Stop();
			
			sg_Left();				//左边
			
			Delay300ms();
			disLeft = get_distance();
			
			sg_Middle();			//正前方
			Delay300ms();
			
			sg_Right();				//右边
			dir = RIGHT;
			Delay300ms();
			disRight = get_distance();
			
			if(disLeft < disRight){//测距左边小，向右转
				go_Right();
				Delay150ms();
				go_Stop();
			}
			if(disRight < disLeft){//测距右边小，向左转
				go_Left();
				Delay150ms();
				go_Stop();
			}
			}
		
	}
	
} 

调试：正面距离过小的时候让小车向后退

加入一个判断即可

		if(disMiddle < 10){					
			//后退
			go_Back();