双轮平衡小车项目

本文最后更新于：2022年5月29日 上午

一、项目概述

双轮平衡车的控制系统采用STM32F103C8T6作为核心控制器，在此基础上增加了各种接口电路板组成整个硬件系统，包括单片机最小系统，直流电机驱动控制模块，电源管理模块，角度测量模块。软件调试部分依次对应硬件各模块进行程序设计，包括PWM模块，PID控制算法，人机交互控制等。

MbQrYq.png

二、项目预算清单

产品名称 单位 数量 单价 金额
无线蓝牙模块 个 1 16.8 16.8
稳压模块 个 1 16.8 16.8
直流减速电机固定架 个 2 2.8 5.6
铜柱 个 12 0.4 4.8
3.3v超声波模块 个 1 2.8 2.8
有源蜂鸣器 个 1 0.48 0.48
排针 个 3 0.13 0.39
拨动开关 个 2 0.13 0.26
母座 个 3 0.59 1.77
6*8洞洞板 块 2 1.03 2.06
电压表0.36寸 个 1 2.84 2.84
18650三节电池盒 个 3 1.38 4.14
TB6612FNG驱动模块 个 1 4.3 4.3
MPU6050模块 个 1 2.8 2.8
杜邦线 排 3 1.62 4.86
STM32F103最小系统板 个 1 9.51 9.51
M3螺丝、螺母 袋 6 0.3 1.8
直流减速电机（减速比1：60） 个 2 49.9 99.8
18650充电锂电池 个 3 8.9 26.7
3.7v锂电池充电器 个 1 13.9 13.9
65mm大摩擦力轮胎 个 2 10.8 21.6
小车亚克力底盘 块 2 5.8 11.6
运费 1 8 8
总计(元)： 255.61

三、硬件电路设计

Mbu7p8.png

（1） 单片机最小系统：包括单片机，程序下载调试接口等；

（2） 陀螺仪：检测小车当前的状态，反馈信息给主控芯片；

（3） 电机驱动电路：驱动两个电机正常运行；

（4） 超声波：检测小车前面障碍物距离，反馈信息等；

（5） 蜂鸣器：提示报警，反馈信息让小车做出相应的反应动作；

（6） 串口调试：用于小车调试、设置作用；

（7） 蓝牙通讯接口（备用）；

（8） 电源：电源电压转换、稳压、滤波电路。

电源管理单元是系统硬件设计中的一个重要组成单元。根据系统各部分正常工作的需要，本系统输出电压值分别有11.2V、5V 和 3.3V 三个档。

四、模块程序设计

1，电机驱动模块

由于TB6612相对于传统的L298N效率上提高很多,体积上也大幅度减少，在额定范围内，芯片基本不发热，所以我们设计的时候选择了这款芯片。

MbQwwj.png

（1）motor.h
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
#ifndef __MOTOR_H
#define	__MOTOR_H

#include "stm32f10x.h"

//Pin definition
/*******************************************************/
#define MOTOR_OCPWM_GPIO_CLK			RCC_APB2Periph_GPIOA
#define MOTOR_OCPWM_GPIO_AF				RCC_APB2Periph_AFIO

#define MOTOR_OC1PWM_GPIO_PORT			GPIOA
#define MOTOR_OC1PWM_PIN				GPIO_Pin_0
#define MOTOR_OC1PWM_PINSOURCE			GPIO_PinSource0

#define MOTOR_OC2PWM_GPIO_PORT			GPIOA
#define MOTOR_OC2PWM_PIN				GPIO_Pin_1
#define MOTOR_OC2PWM_PINSOURCE			GPIO_PinSource1

#define MOTOR_TIM						TIM2
#define MOTOR_TIM_CLK					RCC_APB1Periph_TIM2

#define MOTOR_TIM_IRQn					TIM2_IRQn
#define MOTOR_TIM_IRQHandler			TIM2_IRQHandler


#define MOTOR_GPIO_CLK					RCC_APB2Periph_GPIOA
#define MOTOR_GPIO_PORT					GPIOA

#define MOTOR_AIN1_PIN					GPIO_Pin_5
#define MOTOR_AIN2_PIN					GPIO_Pin_4

#define MOTOR_BIN1_PIN					GPIO_Pin_6
#define MOTOR_BIN2_PIN					GPIO_Pin_7

/*******************************************************/

#define MOTOR_AIN1_1()	GPIO_SetBits(MOTOR_GPIO_PORT,MOTOR_AIN1_PIN)	/* AIN1 = 1 */
#define MOTOR_AIN1_0()	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_AIN1_PIN)	/* AIN1 = 0 */

#define MOTOR_AIN2_1()	GPIO_SetBits(MOTOR_GPIO_PORT,MOTOR_AIN2_PIN)	/* AIN2 = 1 */
#define MOTOR_AIN2_0()	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_AIN2_PIN)	/* AIN2 = 0 */

#define MOTOR_BIN1_1()	GPIO_SetBits(MOTOR_GPIO_PORT,MOTOR_BIN1_PIN)	/* BIN1 = 1 */
#define MOTOR_BIN1_0()	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_BIN1_PIN)	/* BIN1 = 0 */

#define MOTOR_BIN2_1()	GPIO_SetBits(MOTOR_GPIO_PORT,MOTOR_BIN2_PIN)	/* BIN2 = 1 */
#define MOTOR_BIN2_0()	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_BIN2_PIN)	/* BIN2 = 0 */

/*******************************************************/

void MOTOR_Init(void);
void MOTOR_TIM_SetPWM_Num(uint8_t chx,uint16_t value);

/********* Direction control function *********/
void Forward_Go(void);
void Backward_Go(void);
void Motor_Stop(void);

/********* PID control function *********/
uint16_t Position_PID(float currentAngle,float Target);
void SpeedRegulation(float currentAngle,float targetAngle);

/**********************************************/

#endif	/* __MOTOR_H */

（2）motor.c
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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
/**
  ******************************************************************************
  * @file    motor.c
  * @author  Soso
  * @date    2019-10-20
  * @brief   General purpose timer multi-channel PWM output configuration.
  * 		 DC motor pin GPIO mode configuration and Direction control configuration.
  *
  ******************************************************************************
  */

#include "motor.h"

extern float Kp,Ki,Kd,Pwm,error,lastErr,errSum,dErr,sumerror;
uint8_t derection = 0;

/**
  * @brief  Configuring motor pin IN1~IN8 GPIO mode.
  * @param  None
  * @retval None
  */
static void Motor_GPIO_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	/* Enable GPIO port clock */
	RCC_APB2PeriphClockCmd(MOTOR_GPIO_CLK,ENABLE);
	
	// GPIO configuration
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	
	// AIN1 pin initialzation
	GPIO_InitStructure.GPIO_Pin = MOTOR_AIN1_PIN;
	GPIO_Init(MOTOR_GPIO_PORT,&GPIO_InitStructure);
	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_AIN1_PIN);
	// AIN2 pin initialzation
	GPIO_InitStructure.GPIO_Pin = MOTOR_AIN2_PIN;
	GPIO_Init(MOTOR_GPIO_PORT,&GPIO_InitStructure);
	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_AIN2_PIN);
	
	// BIN1 pin initialzation
	GPIO_InitStructure.GPIO_Pin = MOTOR_BIN1_PIN;
	GPIO_Init(MOTOR_GPIO_PORT,&GPIO_InitStructure);
	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_BIN1_PIN);
	// BIN2 pin initialzation
	GPIO_InitStructure.GPIO_Pin = MOTOR_BIN2_PIN;
	GPIO_Init(MOTOR_GPIO_PORT,&GPIO_InitStructure);
	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_BIN2_PIN);
}


/**
  * @brief  Configure the I/O used when the TIM multiplexes the output PWM.
  * @param  None
  * @retval None
  */
static void TIM2_GPIO_Config(void) 
{
	GPIO_InitTypeDef GPIO_InitStructure;

	/* Enable the relevant GPIO peripheral clock */
	RCC_APB2PeriphClockCmd(MOTOR_OCPWM_GPIO_CLK|MOTOR_OCPWM_GPIO_AF, ENABLE); 
	
	/* Timer channel pin configuration */
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	
	/* Channel 1 initialization */
	GPIO_InitStructure.GPIO_Pin = MOTOR_OC1PWM_PIN;
	GPIO_Init(MOTOR_OC1PWM_GPIO_PORT, &GPIO_InitStructure);
	
	/* Channel 2 initialization */
	GPIO_InitStructure.GPIO_Pin = MOTOR_OC2PWM_PIN;
	GPIO_Init(MOTOR_OC2PWM_GPIO_PORT, &GPIO_InitStructure);
}


/**
  * @brief  Timer PWM output mode configuartion.
  * @param  None
  * @retval None
  */
static void TIM2_PWM_Config_Init(void)
{
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;

	/* Enable TIMx_CLK */
	RCC_APB1PeriphClockCmd(MOTOR_TIM_CLK, ENABLE); 

/*  1, PSC(prescaler) = 72-1, 
	   timer frequency = 72M/(PSC + 1) = 1,000KHZ, 
	   calculate the time of one pulse = 1 / 1,000,000 s
	2, ARR (automatic reload register) = 100-1, counted from 0 to 99, then counted 100 times.
	3, Time=100/1,000,000=0.0001s=0.1ms */ 
	TIM_TimeBaseStructure.TIM_Period = 100-1;
	TIM_TimeBaseStructure.TIM_Prescaler = 72-1;
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

	// Initialization TIMx
	TIM_TimeBaseInit(MOTOR_TIM, &TIM_TimeBaseStructure);

	/* PWM mode configuration */
	/* PWM1 Mode configuration */
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;	    // Configured as PWM mode 1
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;	
	TIM_OCInitStructure.TIM_Pulse = 1-1;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;  	  // High level when the timer count value is less than CCR1_Val
	
	/*	Enable channel 1 ~ 2*/
	TIM_OC1Init(MOTOR_TIM, &TIM_OCInitStructure);
	TIM_OC2Init(MOTOR_TIM, &TIM_OCInitStructure);

	/* Enable channel 1 ~ 2 overload */
	TIM_OC1PreloadConfig(MOTOR_TIM, TIM_OCPreload_Enable);
	TIM_OC2PreloadConfig(MOTOR_TIM, TIM_OCPreload_Enable);

	/* Enable timer */
	TIM_Cmd(MOTOR_TIM, ENABLE);
}


/**
  * @brief  Change the timer channel duty cycle.
  * @param  chx: where x can be  1
  * @param  value: new value of the TIMx channelx
  * @retval None
  */
void MOTOR_TIM_SetPWM_Num(uint8_t chx,uint16_t value)
{
	if (chx==1)
	{
		TIM_SetCompare1(MOTOR_TIM,value);
	}
	else if (chx==2)
	{
		TIM_SetCompare2(MOTOR_TIM,value);
	}
}


void MOTOR_Init(void)
{
	Motor_GPIO_Config();
	TIM2_GPIO_Config();
	TIM2_PWM_Config_Init();
}


/*********************Direction control function*********************/

// 逆：IN1: 1	IN2: 0
// 顺：IN1: 0 	IN2: 1
// STOP: IN1: 0 IN2: 0

void Forward_Go(void)
{
	MOTOR_AIN1_0();
	MOTOR_AIN2_1();
	
	MOTOR_BIN1_1();
	MOTOR_BIN2_0();
}

void Backward_Go(void)
{
	MOTOR_AIN1_1();
	MOTOR_AIN2_0();
	
	MOTOR_BIN1_0();
	MOTOR_BIN2_1();
}

void Motor_Stop(void)
{
	MOTOR_AIN1_0();
	MOTOR_AIN2_0();
	
	MOTOR_BIN1_0();
	MOTOR_BIN2_0();
}


/*********************PID control function*********************/

uint16_t Position_PID(float currentAngle,float Target)
{
	uint16_t currentAngle_int = currentAngle / 1;
	uint16_t Target_int = Target / 1,timeChange = 1;
	
	
	error = Target_int - currentAngle_int;
	if(error>1 || error <-1)
		errSum +=(error * timeChange);
	
	if(errSum > sumerror)
		errSum = sumerror;
	else if(errSum < -sumerror)
		errSum = -sumerror;
	
	dErr = (error -lastErr)/timeChange;
	
	Pwm = Kp * error + Ki * errSum + Kd * dErr;
	
	lastErr = error;
	
	if(Pwm < 0)
	{
		derection = 0;
		Pwm = 0 - Pwm;
	}
	else
		derection = 1 ;
		
	if(Pwm != 0)
		Pwm = Pwm*0.69+6;
	
	if(Pwm >= 70)	Pwm = 70;
	
	return Pwm;
}

void SpeedRegulation(float currentAngle,float targetAngle)
{
	uint16_t pwm = 0;
	
	pwm = Position_PID(currentAngle,targetAngle);
	// Change duty cycle
	MOTOR_TIM_SetPWM_Num(1,pwm);
	MOTOR_TIM_SetPWM_Num(2,pwm);
	
	// Change direction
	if(derection == 1)
		Forward_Go();
	else if(derection == 0)
		Backward_Go();
	if(pwm == 0)
	    Motor_Stop();
}

/*********************************************END OF FILE*********************************************/

2，MPU6050模块

偏移角度的测量，这里选择了常用的 MPU-6050 模块(三轴陀螺仪 + 三轴加速度)。

Mb36pt.png

（1）mpu6050.h
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
#ifndef __MPU6050_H
#define __MPU6050_H

#include "stm32f10x.h"
#include "i2c_gpio.h"
#include <math.h>


/* 坐标角度结构体 */
typedef struct Angle
{
    double X_Angle;
    double Y_Angle;
    double Z_Angle;
    
}MPU6050_Angle;


/* 传感器数据修正值（消除芯片固定误差，根据硬件进行调整） */
#define X_ACCEL_OFFSET 0 
#define Y_ACCEL_OFFSET 0 
#define Z_ACCEL_OFFSET 0 
#define X_GYRO_OFFSET 0 
#define Y_GYRO_OFFSET 0 
#define Z_GYRO_OFFSET 0


/*******************************************************/
#define DEV_ADDR		0xD0 // 6050 器件地址 
//----------------------------------------- 
// 定义MPU6050内部地址 
//----------------------------------------- 
#define SMPLRT_DIV		0x19 //陀螺仪采样率，典型值：0x07(125Hz) 
#define CONFIG			0x1A //低通滤波频率，典型值：0x06(5Hz) 
#define GYRO_CONFIG		0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s) 
#define ACCEL_CONFIG	0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz) 

/* 加速度相关寄存器地址 */
#define ACCEL_XOUT_H	0x3B 
#define ACCEL_XOUT_L	0x3C 
#define ACCEL_YOUT_H	0x3D 
#define ACCEL_YOUT_L	0x3E 
#define ACCEL_ZOUT_H	0x3F 
#define ACCEL_ZOUT_L	0x40 

/* 温度相关寄存器地址 */
#define TEMP_OUT_H		0x41 
#define TEMP_OUT_L		0x42 

/* 陀螺仪相关寄存器地址 */
#define GYRO_XOUT_H		0x43
#define GYRO_XOUT_L		0x44 
#define GYRO_YOUT_H 	0x45 
#define GYRO_YOUT_L 	0x46 
#define GYRO_ZOUT_H 	0x47 
#define GYRO_ZOUT_L 	0x48 

#define PWR_MGMT_1		0x6B  //电源管理，典型值：0x00(正常启用) 
#define WHO_AM_I		0x75 //IIC地址寄存器(默认数值0x68，只读) 
#define SlaveAddress	0xD0 //IIC写入时的地址字节数据，+1为读取 

/*******************************************************/

void MPU6050_WR_Reg(uint8_t regAddr, uint8_t regData);
uint8_t MPU6050_RD_Reg(uint8_t regAddr);
void MPU6050_Init(void);
int16_t MPU6050_Get_Data(uint8_t regAddr);
void MPU6050_Get_Angle(MPU6050_Angle *data);

#endif	/* __MPU6050_H */

（2）mpu6050.c
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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
/**
  ***************************************************************************************
  * @file    uasrt.c
  * @author  Soso
  * @date    2019-10-20
  * @brief   mpu6050 configuration.
  ***************************************************************************************
  */

#include "mpu6050.h"


/**
  * @brief  MPU6050 write register function.
  * @param  regAddr：Register address
  * @param  regData：Register value to be written
  * @retval None
  */
void MPU6050_WR_Reg(uint8_t regAddr, uint8_t regData)
{
    /* 发送起始信号 */
    I2C_Start();
    
    /* 发送设备地址 */ 
    I2C_Send_Byte(DEV_ADDR);
    if (I2C_Wait_Ack())
        goto stop;
    
    /* 发送寄存器地址 */
    I2C_Send_Byte(regAddr);
    if (I2C_Wait_Ack())
        goto stop;
    
    /* 写数据到寄存器 */
    I2C_Send_Byte(regData);
    if (I2C_Wait_Ack())
        goto stop;
    I2C_Stop();
	
stop:
    I2C_Stop();
}


/**
  * @brief  MPU6050 read register function.
  * @param  regAddr：Register address
  * @retval Register address data
  */
uint8_t MPU6050_RD_Reg(uint8_t regAddr)
{
    uint8_t regData;
    
    /* 发送起始信号 */
    I2C_Start();
    
    /* 发送设备地址 */
    I2C_Send_Byte(DEV_ADDR);
    if (I2C_Wait_Ack())
        goto stop;
    
    /* 发送寄存器地址 */
    I2C_Send_Byte(regAddr);
    if (I2C_Wait_Ack())
        goto stop;
    
    /* 发送重复起始信号 */
    I2C_Start();
    
    /* 发送读模式设备地址 */
    I2C_Send_Byte(DEV_ADDR | 0x01);
    if (I2C_Wait_Ack())
        goto stop;
    
    /* 读寄存器数据 */
    regData = I2C_Read_Byte(0);
	I2C_Stop();
	
stop:
    I2C_Stop();
    
    return regData;
}


/**
  * @brief  MPU6050 initialization function.
  * @param  None
  * @retval None
  */
void MPU6050_Init(void)
{ 
	I2C_Config();
	
    MPU6050_WR_Reg(PWR_MGMT_1, 0x00);    //解除休眠状态
    MPU6050_WR_Reg(SMPLRT_DIV, 0x07);    //陀螺仪采样率，典型值：0x07(125Hz)
    MPU6050_WR_Reg(CONFIG, 0x06);        //低通滤波频率，典型值：0x06(5Hz)
    MPU6050_WR_Reg(GYRO_CONFIG, 0x18);   //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
    MPU6050_WR_Reg(ACCEL_CONFIG, 0x01);  //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz) 
}


/**
  * @brief  MPU6050 get data function.
  * @param  regAddr：Register address
  * @retval Register data
  */
int16_t MPU6050_Get_Data(uint8_t regAddr)
{
    uint8_t Data_H, Data_L;
    uint16_t data;
    
    Data_H = MPU6050_RD_Reg(regAddr);
    Data_L = MPU6050_RD_Reg(regAddr + 1);
    data = (Data_H << 8) | Data_L;  // 合成数据 
	
    return data;
}


/**
  * @brief  Get MPU6050 angle function.
  * @param  data: MPU6050_Angle Structure
  * @retval None
  */
void MPU6050_Get_Angle(MPU6050_Angle *data)
{
    /* 计算x, y, z 轴倾角，返回弧度值*/
    data->X_Angle = acos((MPU6050_Get_Data(ACCEL_XOUT_H) + X_ACCEL_OFFSET) / 16384.0);
    data->Y_Angle = acos((MPU6050_Get_Data(ACCEL_YOUT_H) + Y_ACCEL_OFFSET) / 16384.0);
    data->Z_Angle = acos((MPU6050_Get_Data(ACCEL_ZOUT_H) + Z_ACCEL_OFFSET) / 16384.0);

    /* 弧度值转换为角度值 */
    data->X_Angle = data->X_Angle * 57.29577;
    data->Y_Angle = data->Y_Angle * 57.29577;
    data->Z_Angle = data->Z_Angle * 57.29577;
} 

/*********************************************END OF FILE*********************************************/

3，IIC通讯模块

（1）i2c_gpio.h
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
#ifndef __I2C_GPIO_H
#define __I2C_GPIO_H

#include "stm32f10x.h"


#define PIN_SCL					GPIO_Pin_12
#define PIN_SDA					GPIO_Pin_11
#define I2C_GPIO_PORT			GPIOA
#define I2C_GPIO_CLK			RCC_APB2Periph_GPIOA


//SDA的方向设置
void SDA_IN(void); 
void SDA_OUT(void);
 
//I2C所有操作函数
void I2C_Config(void);                 //初始化I2C的IO口
void I2C_Start(void);                  //发送I2C开始信号
void I2C_Stop(void);                   //发送I2C停止信号
void I2C_Send_Byte(u8 txd);            //I2C发送一个字节
uint8_t I2C_Read_Byte(unsigned char ack);   //I2C读取一个字节
uint8_t I2C_Wait_Ack(void);                 //I2C等待ACK信号
void I2C_Ack(void);                    //I2C发送ACK信号
void I2C_NAck(void);                   //I2C不发送ACK信号
void delay_us(u32 m);                              //延时函数

#endif	/* __I2C_GPIO_H */

（2）i2c_gpio.c
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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
/**
  ***************************************************************************************
  * @file    i2c_gpio.c
  * @author  Soso
  * @date    2019-10-20
  * @brief   GPIO simulation i2c configuration.
  ***************************************************************************************
  */

#include "i2c_gpio.h"
#include "systick.h"


void I2C_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_APB2PeriphClockCmd( I2C_GPIO_CLK, ENABLE ); //使能A口时钟

	GPIO_InitStructure.GPIO_Pin = PIN_SCL | PIN_SDA;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;       //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(I2C_GPIO_PORT, &GPIO_InitStructure);
	
	GPIO_SetBits(I2C_GPIO_PORT,PIN_SCL);
	GPIO_SetBits(I2C_GPIO_PORT,PIN_SDA);
}
 

//产生I2C开始信号
void I2C_Start(void)
{
   SDA_OUT();                    //sda线输出
   GPIO_SetBits(GPIOA,PIN_SDA);  //SDA =1
   delay_us(10);
   GPIO_SetBits(GPIOA,PIN_SCL);  //SCL=1
   delay_us(10);
   GPIO_ResetBits(GPIOA,PIN_SDA);//SDA=0
   delay_us(10);
   GPIO_ResetBits(GPIOA,PIN_SCL);//SCL=0
} 
 

//产生I2C停止信号
void I2C_Stop(void)
{
   SDA_OUT();  //sda线输出
   GPIO_ResetBits(GPIOA,PIN_SDA); 
   delay_us(10);
   GPIO_SetBits(GPIOA,PIN_SCL); 
   delay_us(10);
   GPIO_SetBits(GPIOA,PIN_SDA); 
   delay_us(10);
   GPIO_ResetBits(GPIOA, PIN_SCL);//SCL=0; 
   delay_us(10);
}


//等待应答信号到来
//返回值：1，接收应答失败
//        0，接收应答成功
uint8_t I2C_Wait_Ack(void)
{
   uint8_t ucErrTime=0;
   SDA_IN();      //SDA设置为输入 
   GPIO_SetBits(GPIOA,PIN_SDA);
   delay_us(5);
   GPIO_SetBits(GPIOA,PIN_SCL); 
   delay_us(5); 
   while(GPIO_ReadInputDataBit(GPIOA,PIN_SDA))
   {
      ucErrTime++;
      if(ucErrTime>250)
      {
         I2C_Stop();
         return 1;
      }
   }
   GPIO_ResetBits(GPIOA, PIN_SCL);
   return 0; 
}

 
//产生ACK应答
void I2C_Ack(void)
{
   GPIO_ResetBits(GPIOA,PIN_SCL);
   SDA_OUT();
   GPIO_ResetBits(GPIOA,PIN_SDA);
   delay_us(5);
   GPIO_SetBits(GPIOA,PIN_SCL);
   delay_us(5);
   GPIO_ResetBits(GPIOA,PIN_SCL);
}
 

//不产生ACK应答
void I2C_NAck(void)
{
   GPIO_ResetBits(GPIOA,PIN_SCL);
   SDA_OUT();
   GPIO_SetBits(GPIOA,PIN_SDA);
   delay_us(5);
   GPIO_SetBits(GPIOA,PIN_SCL); 
   delay_us(5);
   GPIO_ResetBits(GPIOA,PIN_SCL);
} 

             
//I2C发送一个字节
//返回从机有无应答
//1，有应答
//0，无应答
void I2C_Send_Byte(u8 txd)
{
    u8 t; 
    SDA_OUT();
    GPIO_ResetBits(GPIOA,PIN_SCL);//拉低时钟开始数据传输
    for(t=0;t<8;t++)
    {
		if((txd&0x80)==0x80)   
			GPIO_SetBits(GPIOA, PIN_SDA);	//SDA=1，写 1                
		else  
			GPIO_ResetBits(GPIOA, PIN_SDA);	//SDA=0，写 0 
		txd<<=1;    
		delay_us(5);   //对TEA5767这三个延时都是必须的  
		GPIO_SetBits(GPIOA,PIN_SCL); 
		delay_us(5); 
		GPIO_ResetBits(GPIOA,PIN_SCL);
		delay_us(5);
    }
}

 
//读1个字节，ack=1时，发送ACK，ack=0，发送nACK
uint8_t I2C_Read_Byte(unsigned char ack)
{
    unsigned char i,receive=0;
    SDA_IN();//SDA设置为输入
    for(i=0;i<8;i++ )
 {
        GPIO_ResetBits(GPIOA,PIN_SCL);
        delay_us(5);
        GPIO_SetBits(GPIOA,PIN_SCL);
        receive<<=1;
        if(GPIO_ReadInputDataBit(GPIOA,PIN_SDA))receive++;   
        delay_us(5); 
    }
    if (!ack)
        I2C_NAck();//发送nACK
    else
        I2C_Ack(); //发送ACK   
    return receive;
}


void SDA_IN(void)
{
   GPIO_InitTypeDef GPIO_InitStruct;
   GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IPU;    //上拉输入
   GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
   GPIO_InitStruct.GPIO_Pin = PIN_SDA;
   GPIO_Init(GPIOA, &GPIO_InitStruct );
}


void SDA_OUT(void)
{
   GPIO_InitTypeDef GPIO_InitStruct;
   GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP; 
   GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
   GPIO_InitStruct.GPIO_Pin = PIN_SDA;
   GPIO_Init(GPIOA, &GPIO_InitStruct );
}
/*********************************************END OF FILE*********************************************/

4，超声波模块

关于距离的测量，这里选择了常用的超声波模块 HC-SR04。

MbQdmQ.jpg

注意：我购买的这个超声波模块可接 3.3v 和 5v ，如果你买的超声波模块不支持接3.3v，请接到 5v 引脚。

（1）hcsr04.h
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
#ifndef __HCSR04_H
#define __HCSR04_H

#include "stm32f10x.h"


//Pin definition
/*******************************************************/
#define HCSR04_PORT     					GPIOB
#define HCSR04_CLK      					RCC_APB2Periph_GPIOB
#define HCSR04_TRIG     					GPIO_Pin_13
#define HCSR04_ECHO     					GPIO_Pin_12

#define HCSR04_EXTI_PORTSOURCE				GPIO_PortSourceGPIOB
#define HCSR04_EXTI_PINSOURCE				GPIO_PinSource12
#define HCSR04_EXTI_LINE					EXTI_Line12//EXTI_Line9
#define HCSR04_EXTI_IRQ						EXTI15_10_IRQn//EXTI9_5_IRQn
#define HCSR04_EXTI_IRQHandler				EXTI15_10_IRQHandler//EXTI9_5_IRQHandler

#define HCSR04_TIM							TIM4
#define HCSR04_TIM_CLK						RCC_APB1Periph_TIM4
#define HCSR04_TIM_IRQn						TIM4_IRQn
#define HCSR04_TIM_IRQHandler				TIM4_IRQHandler


#define HCSR04_TRIG_TIM						TIM2
#define HCSR04_TRIG_TIM_CLK					RCC_APB1Periph_TIM2
#define HCSR04_TRIG_TIM_IRQn				TIM2_IRQn
#define HCSR04_TRIG_TIM_IRQHandler			TIM2_IRQHandler

/************************************************************/

void HCSR04_TRIG_TIM_Config(void);
void HCSR04_TIM_Config(void);
void HCSR04_EXTI_Config(void);
void Hcsr04_Trig(void);
void HCSR04_Init(void);
float bubble_sort(float a[],uint8_t m);

#endif	/* __HCSR04_H */

（2）hcsr04.c
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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
/**
  ******************************************************************************
  * @file    hcsr04.c
  * @author  Soso
  * @date    2019-10-20
  * @brief   Ultrasonic module ranging configuration.
  ******************************************************************************
  */

#include "hcsr04.h"


// Configuring nested vectored interrupt controller(NVIC)
static void HCSR04_TRIG_Tim_NVIC_Config(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;
	/* Configure NVIC as a priority group 1 */
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
	
	/* Configure interrupt source*/
	NVIC_InitStructure.NVIC_IRQChannel = HCSR04_TRIG_TIM_IRQn;
	/* Configuration preemption priority： 1 */
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	/* Configure sub-priority： 0 */
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
	/* Enable interrupt channel */
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}


// Configuring nested vectored interrupt controller(NVIC)
static void HCSR04_TIM_NVIC_Config(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;
	/* Configure NVIC as a priority group 1 */
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
	
	/* Configure interrupt source*/
	NVIC_InitStructure.NVIC_IRQChannel = HCSR04_TIM_IRQn;
	/* Configuration preemption priority： 1 */
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	/* Configure sub-priority： 1 */
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	/* Enable interrupt channel */
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}


/* Configuring nested vectored interrupt controller(NVIC) */
static void HCSR04_EXTI_NVIC_Config(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;
	/* Configure NVIC as a priority group 1 */ 
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
	
	/* Configuration preemption priority： 0 */
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
	/* Configure sub-priority： 1 */
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	/* Enable interrupt channel */
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	
	/* Configure interrupt source: Echo pin */
	NVIC_InitStructure.NVIC_IRQChannel = HCSR04_EXTI_IRQ;
	NVIC_Init(&NVIC_InitStructure);
}


void HCSR04_TRIG_TIM_Config(void)
{
	SystemInit();
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
	// Enable TIM4_CLK 
	RCC_APB1PeriphClockCmd(HCSR04_TRIG_TIM_CLK,ENABLE);
	
/*  1, PSC(prescaler) = 720-1
	   timer frequency = 72M/(PSC + 1) = 100KHZ
	   calculate the time of one pulse = 1/1000,000 s
	2, ARR (automatic reload register) = 100-1, counted from 0 to 99, then counted 100 times.
	3, Time = 100/100,000 = 0.001s = 1ms */ 
	TIM_TimeBaseStructure.TIM_Period = 100-1;
	TIM_TimeBaseStructure.TIM_Prescaler = 720-1;
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	// Initialization TIM3
	TIM_TimeBaseInit(HCSR04_TRIG_TIM,&TIM_TimeBaseStructure);
	
	// Clear timer update interrupt flag
	TIM_ClearITPendingBit(HCSR04_TRIG_TIM,TIM_IT_Update);
	// Enable timer update interrupt
	TIM_ITConfig(HCSR04_TRIG_TIM,TIM_IT_Update,ENABLE);
	// Initialization enable timer
	TIM_Cmd(HCSR04_TRIG_TIM,ENABLE);
	
	HCSR04_TRIG_Tim_NVIC_Config();
}


void HCSR04_TIM_Config(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
	// Enable TIM4_CLK 
	RCC_APB1PeriphClockCmd(HCSR04_TIM_CLK,ENABLE);
	
/*  1, PSC(prescaler) = 1-1
	   timer frequency = 72M/(PSC + 1) = 72,000KHZ
	   calculate the time of one pulse = 1/72,000,000 s
	2, ARR (automatic reload register) = 100-1, counted from 0 to 99, then counted 100 times.
	3, Time = 72/72,000,000 = 0.000001‬s = 1us */ 
	TIM_TimeBaseStructure.TIM_Period = 72-1;
	TIM_TimeBaseStructure.TIM_Prescaler = 1-1;
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	// Initialization TIM3
	TIM_TimeBaseInit(HCSR04_TIM,&TIM_TimeBaseStructure);
	
	// Clear timer update interrupt flag
	TIM_ClearITPendingBit(HCSR04_TIM,TIM_IT_Update);
	// Enable timer update interrupt
	TIM_ITConfig(HCSR04_TIM,TIM_IT_Update,ENABLE);
	// Initialization disable timer
	TIM_Cmd(HCSR04_TIM,DISABLE);
	
	HCSR04_TIM_NVIC_Config();
}


void HCSR04_EXTI_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	EXTI_InitTypeDef EXTI_InitStructure;
	
	/* Enable KeyGPIO port clock */
	RCC_APB2PeriphClockCmd(HCSR04_CLK,ENABLE);
	
	/* Trig pin configuration */
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Pin = HCSR04_TRIG;
	GPIO_Init(HCSR04_PORT,&GPIO_InitStructure);
	
	/* Echo pin configuration */
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Pin = HCSR04_ECHO;
	GPIO_Init(HCSR04_PORT,&GPIO_InitStructure);
	
	/* Connect the exti interrupt source to the Echo pin */
	GPIO_EXTILineConfig(HCSR04_EXTI_PORTSOURCE,HCSR04_EXTI_PINSOURCE);
	
	/* EXTI mode configuration */
	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
	
	/* Select the exti interrupt source */
	EXTI_InitStructure.EXTI_Line = HCSR04_EXTI_LINE;
	EXTI_Init(&EXTI_InitStructure);
	
	HCSR04_EXTI_NVIC_Config();
}


void Hcsr04_Trig(void)
{
	uint16_t i;
	
	GPIO_SetBits(HCSR04_PORT,HCSR04_TRIG);
	for(i=0;i<0xf0;i++);
	GPIO_ResetBits(HCSR04_PORT,HCSR04_TRIG);
}


void HCSR04_Init(void)
{
	HCSR04_TRIG_TIM_Config();
	HCSR04_TIM_Config();
	HCSR04_EXTI_Config();
}



float bubble_sort(float a[],uint8_t m)
{
	uint8_t i = 0;
    uint8_t j = 0;
    uint8_t tmp = 0;
	uint32_t sum = 0;
	
    for (i = 0; i<m - 1; i++)
    {
        for (j = 0; j < m - i - 1; j++)
        {
            if (a[j]>a[j + 1])
            {
                tmp = a[j + 1];
                a[j + 1] = a[j];
                a[j] = tmp;
            }
        }
    }
	
	for(i = 5;i<m-5;i++)
		sum += a[i];
	
	return sum/5;
}
/*********************************************END OF FILE*********************************************/

5，有源蜂鸣器模块

（1）buzzer.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#ifndef __BUZZER_H
#define __BUZZER_H

#include "stm32f10x.h"

//Pin definition
/*******************************************************/

#define BUZZER_GPIO_PORT                GPIOB
#define BUZZER_GPIO_CLK                 RCC_APB2Periph_GPIOB
#define BUZZER_GPIO_PIN                 GPIO_Pin_9

/*******************************************************/
void Buzzer_GPIO_Config(void);

#endif /* __BUZZER_H */

（2）buzzer.c
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
/**
  ******************************************************************************
  * @file    buzzer.c
  * @author  Soso
  * @date    2019-10-20
  * @brief   buzzer applicatiom function interface.
  ******************************************************************************
  */

#include "buzzer.h"

void Buzzer_GPIO_Config(void)
{
	// Enable GPIO port clock
	RCC_APB2PeriphClockCmd(BUZZER_GPIO_CLK,ENABLE);
	GPIO_InitTypeDef GPIO_InitStructure;
	
	// GPIO configuration
	GPIO_InitStructure.GPIO_Pin = BUZZER_GPIO_PIN;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
	
	GPIO_Init(BUZZER_GPIO_PORT,&GPIO_InitStructure);
	
	// Close buzzer
	GPIO_ResetBits(BUZZER_GPIO_PORT,BUZZER_GPIO_PIN);
}

/*********************************************END OF FILE*********************************************/

6，串口调试模块

（1）usart.h
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
#ifndef __USART_H
#define __USART_H

#include "stm32f10x.h"
#include "stdio.h"

//引脚定义
/*******************************************************/
#define DEBUG_USART                             USART3
#define DEBUG_USART_CLK                         RCC_APB1Periph_USART3
#define DEBUG_USART_AF                       	RCC_APB2Periph_AFIO
#define DEBUG_USART_GPIO_CLK                 	RCC_APB2Periph_GPIOB
#define DEBUG_USART_BAUDRATE                    115200

#define DEBUG_USART_RX_GPIO_PORT                GPIOB
#define DEBUG_USART_RX_PIN                      GPIO_Pin_11

#define DEBUG_USART_TX_GPIO_PORT                GPIOB
#define DEBUG_USART_TX_PIN                      GPIO_Pin_10


#define DEBUG_USART_IRQ                 		USART3_IRQn
#define DEBUG_USART_IRQHandler					USART3_IRQHandler

/************************************************************/

void USART_Config(void);
void Usart_SendByte( USART_TypeDef * pUSARTx, uint8_t ch);
void Usart_SendString( USART_TypeDef * pUSARTx, char *str);

void Usart_SendHalfWord( USART_TypeDef * pUSARTx, uint16_t ch);

#endif	/* __USART_H */

（2）usart.c
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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
/**
  ******************************************************************************
  * @file    uasrt.c
  * @author  Soso
  * @date    2019-10-20
  * @brief   Redirect c library printf function to usart port, interrupt receiving mode.
  ******************************************************************************
  */

#include "usart.h"


/* Configuring nested vectored interrupt controller NVIC */
static void NVIC_Configuration(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;

	/* Nested vector interrupt controller group selection */
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);

	/* Preemptio Priority is 1 */
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	/* Sub Priority is 1 */
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	/* Enable interrupt */
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	
	/* Configure USART as the interrupt source */
	NVIC_InitStructure.NVIC_IRQChannel = DEBUG_USART_IRQ;
	/* Initialize configuration NVIC */
	NVIC_Init(&NVIC_InitStructure);
}

void USART_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;

	/* Enable port alternate function and enable USART GPIO clock */
	RCC_APB2PeriphClockCmd(DEBUG_USART_AF|DEBUG_USART_GPIO_CLK,ENABLE);
	/* Enable USART Clock */
	RCC_APB1PeriphClockCmd(DEBUG_USART_CLK,ENABLE);
	
	/* Configure the Tx pin for alternate function  */
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;

	GPIO_InitStructure.GPIO_Pin = DEBUG_USART_TX_PIN;
	GPIO_Init(DEBUG_USART_TX_GPIO_PORT, &GPIO_InitStructure);
	
	/* Configure the Rx pin for the alternate function */
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	
	GPIO_InitStructure.GPIO_Pin = DEBUG_USART_RX_PIN;
	GPIO_Init(DEBUG_USART_RX_GPIO_PORT, &GPIO_InitStructure);
	
	/* Configuration USART mode */
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	/* USART mode control: simultaneous enable send and receive */
	USART_InitStructure.USART_Mode = USART_Mode_Tx|USART_Mode_Rx;
	
	USART_InitStructure.USART_BaudRate = DEBUG_USART_BAUDRATE;
	USART_Init(DEBUG_USART, &USART_InitStructure);
	
	/* Configure the serial port to receive interrupts. */
	NVIC_Configuration();
	
	/* Enable serial port receive interrupt */
	USART_ITConfig(DEBUG_USART, USART_IT_RXNE, ENABLE);
	
	/* Enable serial */
	USART_Cmd(DEBUG_USART, ENABLE);
}


/*****************  Send a character **********************/
void Usart_SendByte( USART_TypeDef * pUSARTx, uint8_t ch)
{
	/* Send a byte of data to the USART */
	USART_SendData(pUSARTx,ch);
		
	/* Waiting for the transmit data register to be empty */
	while (USART_GetFlagStatus(pUSARTx, USART_FLAG_TXE) == RESET);	
}

/*****************  Send string **********************/
void Usart_SendString( USART_TypeDef * pUSARTx, char *str)
{
	unsigned int k=0;
  do 
  {
      Usart_SendByte( pUSARTx, *(str + k) );
      k++;
  } while(*(str + k)!='\0');
  
  /* Waiting for transmission to complete */
  while(USART_GetFlagStatus(pUSARTx,USART_FLAG_TC)==RESET)
  {}
}

/*****************  Send a 16-digit number **********************/
void Usart_SendHalfWord( USART_TypeDef * pUSARTx, uint16_t ch)
{
	uint8_t temp_h, temp_l;
	
	/* Take out the high eight */
	temp_h = (ch&0XFF00)>>8;
	/* Take the lower eight */
	temp_l = ch&0XFF;
	
	/* Send high eight */
	USART_SendData(pUSARTx,temp_h);	
	while (USART_GetFlagStatus(pUSARTx, USART_FLAG_TXE) == RESET);
	
	/* Send the lower eight digits */
	USART_SendData(pUSARTx,temp_l);	
	while (USART_GetFlagStatus(pUSARTx, USART_FLAG_TXE) == RESET);	
}

/* Redirect the c library function printf to the serial port, 
   use the printf function after redirection.	*/
int fputc(int ch, FILE *f)
{
	/* Send a byte of data to the serial port */
	USART_SendData(DEBUG_USART, (uint8_t) ch);

	/* Waiting to send */
	while (USART_GetFlagStatus(DEBUG_USART, USART_FLAG_TXE) == RESET);		

	return (ch);
}

/* Redirect the c library function scanf to the serial port, 
   rewrite backwards can use scanf, getchar and other functions. */
int fgetc(FILE *f)
{
	/* Waiting for serial input data */
	while (USART_GetFlagStatus(DEBUG_USART, USART_FLAG_RXNE) == RESET);

	return (int)USART_ReceiveData(DEBUG_USART);
}
/*********************************************END OF FILE*********************************************/

7，延时函数配置模块

（1）systick.h
1
2
3
4
5
6
7
8
9
10
11
#ifndef __SYSTICK_H
#define __SYSTICK_H

#include"stm32f10x.h"

void delay_us(u32 i);
void delay_ms(u32 i);


#endif	/* __SYSTICK_DELAY_H */

（2）systick.c
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
#include "systick.h"

void delay_us(u32 i)
{
	u32 temp;
	SysTick->LOAD=9*i;		 //Set the reload value, at 72MHZ
	SysTick->CTRL=0X01;		 //Enable, reduce to zero is no action, use external clock source
	SysTick->VAL=0;		   	 //Clear counter
	do
	{
		temp=SysTick->CTRL;		//Read current countdown value
	}
	while((temp&0x01)&&(!(temp&(1<<16))));	 //Waiting time to arrive
	SysTick->CTRL=0;	//Close counter
	SysTick->VAL=0;		//Empty counter
}
void delay_ms(u32 i)
{
	u32 temp;
	SysTick->LOAD=9000*i;	//Set the reload value, at 72MHZ
	SysTick->CTRL=0X01;		//Enable, reduce to zero is no action, use external clock source
	SysTick->VAL=0;			//Clear counter
	do
	{
		temp=SysTick->CTRL;	   //Read current countdown value
	}
	while((temp&0x01)&&(!(temp&(1<<16))));	//Waiting time to arrive
	SysTick->CTRL=0;	//Close counter
	SysTick->VAL=0;		//Empty counter
}

8，中断配置模块

把下面的代码添加到 stm32f10x_it.c 文件中。

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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
#include "stm32f10x_it.h"
#include "usart.h"
#include "motor.h"
#include "hcsr04.h"

extern float distance;
float distSum = 0;
uint32_t usHcCount = 0,HcTrigCount = 0,Hcsr04_TrigTime = 0;
uint8_t filterCount = 0;


/* 1ms */
void HCSR04_TRIG_TIM_IRQHandler(void)
{
	if ( TIM_GetITStatus( HCSR04_TRIG_TIM, TIM_IT_Update) != RESET ) 
	{	
		HcTrigCount++;
		if(HcTrigCount >= 60)
		{
			Hcsr04_Trig();
			HcTrigCount = 0;
		}
		TIM_ClearITPendingBit(HCSR04_TRIG_TIM , TIM_IT_Update); 		 
	}
} 

/* 1us */
void HCSR04_TIM_IRQHandler(void)
{
	if ( TIM_GetITStatus( HCSR04_TIM, TIM_IT_Update) != RESET ) 
	{	
		usHcCount++;
		TIM_ClearITPendingBit(HCSR04_TIM , TIM_IT_Update);
	}
}


extern float disBuf[15];

void HCSR04_EXTI_IRQHandler(void)
{
	static uint8_t flagState = 0;
	
	if(EXTI_GetITStatus(HCSR04_EXTI_LINE) != RESET)
	{
		EXTI_ClearITPendingBit(HCSR04_EXTI_LINE);
		if(GPIO_ReadInputDataBit(HCSR04_PORT,HCSR04_ECHO) == 1)
		{
			TIM_SetCounter(HCSR04_TIM,0);
			flagState = 1;
			TIM_Cmd(HCSR04_TIM,ENABLE);
		}
		else
		{
			TIM_Cmd(HCSR04_TIM,DISABLE);
			if(flagState)
			{
				/**************** Bubble filter ****************/
				disBuf[filterCount] = usHcCount/58.0;	//cm
				filterCount++;
				if(filterCount == 15)
				{
					distance = bubble_sort(disBuf,filterCount)*2.0;
					
					if(distance > 400)
						distance = 400;
					
					filterCount = 0;
				}
				usHcCount = 0 ;
			}
			flagState = 0;
		}
		
	}
}

extern float Kp,Ki,Kd,errSum; 
/**
  * @brief  This function handles USART3_IRQHandler interrupt request.
  * @param  None
  * @retval None
  */
void DEBUG_USART_IRQHandler(void)
{
	uint8_t RxTemp = 0;
	
	if(USART_GetITStatus(DEBUG_USART,USART_IT_RXNE) != RESET)
	{
		RxTemp = USART_ReceiveData(DEBUG_USART);
		USART_SendData(DEBUG_USART,RxTemp);
		
		if(RxTemp == 'q')
			Kp += 1;
		else if(RxTemp == 'w')
			Kp -= 1;
		else if(RxTemp == 'a')
			Kp += 0.1;
		else if(RxTemp == 's')
			Kp -= 0.1;
		else if(RxTemp == 'z')
			Kp += 0.01;
		else if(RxTemp == 'x')
			Kp -= 0.01;
		else if(RxTemp == 'e')
			Ki += 0.0001;
		else if(RxTemp == 'r')
			Ki -= 0.0001;
		else if(RxTemp == 'd')
			Ki += 0.000000001;
		else if(RxTemp == 'f')
			Ki -= 0.000000001;
		else if(RxTemp == 't')
			Kd += 10;
		else if(RxTemp == 'y')
			Kd -= 10;
		else if(RxTemp == 'g')
			Kd += 1;
		else if(RxTemp == 'h')
			Kd -= 1;
		else if(RxTemp == 'b')
			Kd += 0.1;
		else if(RxTemp == 'n')
			Kd -= 0.1;
		else if(RxTemp == 'u')
			Kp = 0;
		else if(RxTemp == 'i')
			Ki = 0;
		else if(RxTemp == 'o')
			Kd = 0;
		else if(RxTemp == 'p')
			errSum = 0;
	}
}

9，main.c

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
/**
  ******************************************************************************
  * @file    main.c
  * @author  Soso
  * @date    2019-10-20
  * @brief   Balance car control.
  *
  ******************************************************************************
  */
  
#include "stm32f10x.h"
#include "motor.h"
#include "usart.h"
#include "systick.h"
#include "hcsr04.h"
#include "i2c_gpio.h"
#include "mpu6050.h"
#include "buzzer.h"

float Pwm = 0,Kp = 10,Ki = 0,Kd = 0,error = 0,lastErr = 0,errSum = 0,dErr = 0,sumerror = 4000;
float TargetAngle = 90;
float distance = 0,disBuf[15] = {0};

int main(void)
{
//	MOTOR_Init();
//	HCSR04_Init();
	USART_Config();
	I2C_Config();
	MPU6050_Init();
//	Buzzer_GPIO_Config();
	
	// Change duty cycle
	MOTOR_TIM_SetPWM_Num(1,Pwm);
	MOTOR_TIM_SetPWM_Num(2,Pwm);
	
	MPU6050_Angle angle;
	
	// Buzzer ring
//	GPIO_SetBits(BUZZER_GPIO_PORT,BUZZER_GPIO_PIN);	// Open
//	delay_ms(500);
//	GPIO_ResetBits(BUZZER_GPIO_PORT,BUZZER_GPIO_PIN);	// Close
	
	printf("initialization successful.\r\n");
	
	while(1)
	{
		MPU6050_Get_Angle(&angle);  // 计算三轴倾角 
		
//		printf("distance:%0.2f cm\r\n",distance);
//		printf("Pwm1: %d    Pwm2: %d\r\n",Pwm1,Pwm2);
//		printf("Pwm: %d\r\n",Pwm);
//		printf("X_Angle = %lf°    Y_Angle = %lf°    Z_Angle = %lf°\r\n", angle.X_Angle,angle.Y_Angle,angle.Z_Angle);
		
//		delay_ms(200);
		
		SpeedRegulation(angle.Y_Angle,TargetAngle);			// PID

		printf("Kp: %0.2f  Ki: %0.9f  Kd: %0.2f  error:%0.1f  Pwm: %0.2f CurrentAngle: %0.2f errSum: %0.2f\r\n",Kp,Ki,Kd,error,Pwm,angle.Y_Angle,errSum);

	}
}




/*********************************************END OF FILE*********************************************/

五、元器件接线说明

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
【*】 引脚分配
			
			STM32	     MPU6050
			PA12    ->    SCL PIN
			PA11    ->    SDA PIN
			+3.3V   ->    VCC
			GND     ->    GND
			
			STM32	     TB6612FNG
			PA1     ->     PWMA
			PA5     ->     AIN1
			PA4     ->     AIN2
			PA0     ->     PWMB
			PA6     ->     BIN1
			PA7     ->     BIN2
			+3.3V	->	   VCC
			GND		->	   GND
	
			电源		TB6612FNG
			12V     ->     VM
			GND     ->     GND
			
		DC Geared motor	    TB6612FNG
			①+      ->     A01
			①-      ->     A02
			②+      ->     B01
			②-      ->     B02
			
			STM32		串口模块
			PB11    ->     Tx
			PB10    ->     Rx
			+3.3V   ->     +3.3V
			GND     ->     GND
			波特率：115200
			
			STM32		超声波模块
			PB13    -> 	  Trig
			PB12    -> 	  Echo
			+3.3V   -> 	  Vcc
			GND    	-> 	  GND
			
			STM32		蓝牙模块
		        	-> 	  Rx
			    	-> 	  Tx
			+3.3V   -> 	  Vcc
			GND    	-> 	  GND

附录 PID控制原理简单介绍

PID（比例、积分、微分）控制是建立在经典控制理论基础上，对过去、现在和未来信息进行估计的控制算法。PID控制策略其结构简单，稳定性好，可靠性高，并且易于实现。通过将理想输入与实际输出的误差信号送到PID控制器对误差信号分别进行比例（P）、积分（I）、微分（D）运算，其结果的加权和构成系统的控制信号。

MbGBRI.jpg

嵌入式 > STM32
#STM32 #双轮平衡车
双轮平衡小车项目
https://kevinloongc.github.io/posts/4588.html
作者
Kevin Loongc
发布于
2019年8月28日
许可协议