使用舵机和超声波模块实现小车自动避障_51舵机超声波避障代码

舵机摇头+超声波测距避障

实验效果

小车会自动避开障碍物，寻找合适的出口出去

程序

程序文件：

1.main.c：主函数中作摇头测距的逻辑设计

2.Motor.c：小车前进、后退、左转、右转和停止的函数，其中左转和右转可更改一下

3.Delay.c：延时函数

4.Sg90.c：定时器0初始化函数，中断PWM，舵机摇头的角度（封装成函数）

5.HC-SR04.c：定时器1初始化函数，不开启中断，超声波测距的函数

舵机摇头

1.要实现舵机摇头就是依次从0°摇到90°，然后再摇到180°，反复循环，因为舵机是用PWM控制的，在垃圾桶项目中已经使用过舵机，可以直接拿代码过来用，最好进行代码的封装，在工程中引入该舵机的源文件

Sg90.c：

#include <REGX52.H>

sbit Sg90_com = P1^0;
unsigned char count,compare;

/**
  * @brief 定时器0初始化函数，舵机PWM控制
  * @param 无
  * @retval无
  */
void Timer0Init(void)		//500微秒@11.0592MHz
{
	TMOD &= 0xF0;		//设置定时器模式
	TMOD |= 0x01;		//设置定时器模式
	TL0 = 0x33;		//设置定时初值
	TH0 = 0xFE;		//设置定时初值
	TF0 = 0;		//清除TF0标志
	TR0 = 1;		//定时器0开始计时
	ET0 = 1;
	EA = 1;
}

//180度
void SgLeft()
{
	compare = 5;
	count = 0;
}

//135度
void SgLeft135()
{
	compare = 4;
	count = 0;
}

//90度
void SgMiddle()
{
	compare = 3;
	count = 0;
}

//45度
void SgRight45()
{
	compare = 2;
	count = 0;
}

//0度
void SgRight()
{
	compare = 1;
	count = 0;
}

//中断处理函数
void Timer0_Rountine() interrupt 1	//每次定时器溢出时是0.5ms
{
	TL0 = 0x33;
	TH0 = 0xFE;
	count++;
	//PWM控制
	if(count < compare)		//通过比较值控制高电平占据周期的时间，也就是占空比大小
	{
		Sg90_com = 1;
	}
	else
	{
		Sg90_com = 0;
	}
	if(count == 40)	//每一个0.5mscount都会++，加了40次就20ms，是舵机控制的一个周期
	{
		count = 0;
		Sg90_com = 1;
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78

2.舵机封装好后，可以先进行测试，在主函数中调用函数让其摇头，测试没问题后，将这些测试代码删掉，准备加入超声波模块测距

main.c：

#include <REGX52.H>
#include "Motor.h"
#include "Delay.h"
#include "Sg90.h"

void main()
{
	Timer0Init();
	SgMiddle();		//一开始为90度，往前
	Delay1ms(1000);
	while(1)
	{
		SgLeft();			//180度，左扭头
		Delay1ms(500);
		SgMiddle();			//90度，往前
		Delay1ms(500);
		SgRight();			//0度，右扭头
		Delay1ms(500);
		SgMiddle();			//90度，往前
		Delay1ms(500);
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

超声波测距

1.超声波模块也在垃圾桶中使用过，可以把封装好的源文件复制到当前工程目录下直接使用

HC-SR04.c：

#include <REGX52.H>
#include "Delay.h"

sbit Trig = P1^1;
sbit Echo = P1^2;

/**
  * @brief定时器1初始化，超声波用
  * @param无
  * @retval无
  */
void Timer1Init(void)		//@11.0592MHz
{
	TMOD &= 0x0F;		//设置定时器模式
	TMOD |= 0x10;		//设置定时器模式
	TL1 = 0;		//设置定时初值
	TH1 = 0;		//设置定时初值
	TF1 = 0;		//清除TF1标志
}

//起始信号
void Trig_Start()
{
	Trig = 0;
	Trig = 1;
	Delay11us();
	Trig = 0;
}

/**
  * @brief超声波测距
  * @param无
  * @retval返回测到的距离，单位cm
  */
double ultrasonic()
{
	double time;
	double distance;
	TH1 = 0;
	TL1 = 0;
	Trig_Start();
	while(Echo == 0);
	TR1 = 1;
	while(Echo == 1);
	TR1 = 0;
	time = (TH1*256+TL1)*1.085;
	distance = time*0.017;
	return distance;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

小车转弯函数

在测距避障过程中，如果使用的是普通转弯函数，就是一个轮子转另一个轮子不转，这样找到出口的速度会慢点，有时候会在一个地方反复摇头测试，将左转或右转的函数修改一下，让小车在原地旋转，提高找到出口的速度

Motor.c：

修改的地方就是，左转时，右轮前进，左轮让其后退，右转同理，这样转弯时就会原地转，避免左右走动时对周围的测距距离产生影响，提高找到出口的速度

后续也可将这改动单独写一个函数出来，原来的左转右转函数保留，调用新封装的左转右转函数就行

/**
  * @brief控制小车左转
  * @param无
  * @retval无
  */
void GoLeft()
{
	//右轮前转
	rightA = 0;
	rightB = 1;
	//左轮后转
	leftA = 1;
	leftB = 0;
}

/**
  * @brief控制小车右转
  * @param无
  * @retval无
  */
void GoRight()
{
	//右轮后转
	rightA = 1;
	rightB = 0;
	//左轮前转
	leftA = 0;
	leftB = 1;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

主函数中实现同时摇头并测距

main.c：

后续也可以将这部分测距摇头的代码进行封装，做到模块化编程

#include <REGX52.H>
#include "Motor.h"
#include "Delay.h"
#include "Sg90.h"
#include "HC-SR04.h"

/*测试无数次得出的结论：摇头测速延时180ms左右，不能太慢，头摇慢的话很容易撞墙

Motor.c中将左转和右转的函数代码改一下，左轮前转时，右轮后转，反过来一样，这样转弯就很快
找到出口的速度大大提高
*/

unsigned char MiddleFlag;		//超声波方向朝前标志位

void main()
{
	double MiddleDis,LeftDis,RightDis;
	Timer0Init();
	Timer1Init();
	
	SgMiddle();		//一开始为90度，往前
	Delay1ms(300);
	MiddleFlag = 1;	//标志位置1
	while(1)
	{
		if(MiddleFlag != 1)				//如果标志位不为1，让超声波往前摆正
		{
			SgMiddle();
			Delay1ms(180);
			MiddleFlag = 1;
		}
		MiddleDis = ultrasonic();		//检测前方距离
		if(MiddleDis > 30)				//前方没有障碍
		{
			GoForward();				//前进
		}
		else if(MiddleDis < 10)			//如果距离过小
		{
			GoBack();					//后退
		}
		else							//前方有障碍
		{
			//停止
			Stop();
			SgLeft();					//180度，左扭头
			Delay1ms(180);
			LeftDis = ultrasonic();		//测左边距离
			
			SgMiddle();					//回到中间
			Delay1ms(180);
			
			SgRight();					//0度，右扭头
			Delay1ms(180);
			RightDis = ultrasonic();	//测右边距离
			
			//若左边距离大于右边距离，说明左边宽敞，向左转
			if(LeftDis > RightDis)	
			{
				GoLeft();
				Delay1ms(170);
				Stop();
			}
			//若右边距离大于左边距离，说明右边宽敞，向右转
			if(RightDis > LeftDis)
			{
				GoRight();
				Delay1ms(170);
				Stop();
			}
			MiddleFlag = 0;				//标志位置0，下一个循环先让超声波模块摆正
		}
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73

模块组装

用热熔胶把舵机固定在小车车头，然后把超声波模块接好线后，绑定在舵机上，可自己想办法固定，舵机转动角度对即可