两轮自平衡小车-资料整理

一、需求分析

1. 需要实现的功能

  （1）小车自平衡；

  （2）遥控两轮自平衡车移动行驶；

  （3）显示电池电压、平衡倾角、角速度等小车状态信息。

2. 如何实现需求

  （1）最首要的一点，小车得能动起来，所以要用PWM波控制电机驱动轮子；

  （2）要实现平衡，首先得获取小车的姿态，并进行姿态解算，才能进行对应的校正；

  （3）关于平衡控制，本项目使用PID算法调整PWM波来实现；

  （4）使用PID算法控制电机实现平衡，要求获取电机转速作为反馈，所以需要编码器测量电机转速；

  （5）通过蓝牙无线传输技术使用手机APP蓝牙遥控操纵小车的行驶；

  （6）使用OLED显示屏显示小车状态信息。

二、物料清单

1. 主控芯片：STM32F103C8T6
2. 姿态检测：MPU6050（通信接口：I2C）
3. 直流电机驱动：TB6612（直流电机属于大功率器件，GPIO口无法直接驱动，所以需要接电机驱动芯片）
4. 带霍尔编码器的直流电机（型号：MG513,额定电压：12V）
5. 12V锂电池
6. 稳压芯片：LM2596（12V的电压环境对单片机、各传感器和各模块来说电压太高，直接供电会烧毁各模块，所以还需要对电压进行降压处理）
7. 蓝牙模块：JDY-31（通信接口：USART）
8. 0.96寸OLED（I2C接口）
9. 小车车架，轮子等

三、用到的STM32单片机的片上资源

1. NVIC(Nested vectoredinterrupt controller,嵌套向量中断控制器)

    用于管理中断

2. RCC(Reset and clock control,复位与时钟控制)

3. GPIO(General-purpose input/output,通用IO口)

4. AFIO(Alternatefunction I/O,复用IO口)

    用于引脚重定义与中断引脚选择

5.EXTI(External Interrupts,外部中断)

   小车的控制动作基本上都放在了由MPU6050触发的外部中断函数中。

6.TIM(定时器)

    使用输出比较产生PWM波形，使用编码器接口获取电机转速

7. ADC(Analog-to-Digital Converter,模数转换)

     将电池电压信息转换为数字信息

8. USART(Universal Synchronous/Asynchronous Receiver/Transmitter,通用同步/异步收发器)

      用于与蓝牙模块通信

9. I2C(Inter IC Bus,I2C通信)

       用于与MPU6050通信

四、PID算法

1. P , I , D 这三个控制器的简介

k时刻的偏差e(k)=期望值-当前值

P控制器（比例）：

是缩小当前值和期望值之间偏差的主要手段，但无法消除稳态误差。

 I控制器（积分）：

可消除稳态误差，但容易造成振荡。

D控制器（微分）：

当前偏差减去上一时刻的偏差，偏差的微分可反映偏差的变化趋势。D控制器能根据偏差的变化趋势进行超前调节，抑制振荡，防止系统出现超调，但有可能造成调节周期过长，也会放大噪声。

这方面更详细的讲解可看以下视频，讲得很简单易懂

https://www.bilibili.com/video/av382551634

2. 直立环

作用：让小车角度趋近0

一个很直观的现象是当人要往前倒时，得往前移动才能恢复平衡。控制小车平衡也是一样的，直立环的调节过程就是当小车往前倒时，驱动轮子，让小车往前动，当小车往后倒时，则让小车往后动。

直立环使用PD控制：，e(k)为k时刻小车目标倾角与当前倾角的差值

为什么直立环使用PD控制？

因为小车存在惯性，所以当小车角度为0时还会存在角速度，所以需要加入D控制器，预测下一时刻的偏差，进行超前调节。这里也可以把D控制器的作用理解为阻尼力，能抑制振荡。

那么为什么直立环不用积分呢？是因为当比例系数很大的时候系统几乎没有稳态误差。平衡小车直立控制需要快速性，所以P是很大的，积分的作用是消除稳态误差，而稳态误差对于直立控制的影响很小，所以也就不需要积分了。

3. 速度环

作用：让电机速度趋近0

之所以需要速度环，是因为直立环只保证了小车能够直立，但没有保证它静止不动。而且小车立起来之后，那就自然会有向前移动和向后移动的趋势，因为电机的移动速度不受控。如果加上速度环配合直立环一起工作，那么就可以让小车在直立起来的同时，还能够抑制小车前后移动。

速度环使用PI控制：,e(k)为k时刻小车目标速度与当前速度的差值

为什么速度环使用 PI 控制？

对于速度控制当然是越精确越好，平衡小车当然希望速度一直为0。但是光有P控制器是不够的，因为P控制器的主要缺点是无法消除稳态误差。那么就需要用I积分控制器来消除。

虽然D控制器可以预测未来，提前响应，但是微分控制的缺点是会放大噪声。而速度环的偏差是由电机运动引起的，噪声很大，所以不加入微分控制。
 

4. 串级控制

本项目的串级控制即把直立环和速度环串联起来。控制框图如下：

注意一点，角度反馈是基于MPU6050，而速度环的速度反馈是基于电机的编码器。

速度环输入：1.给定速度。2.速度反馈。
输出：角度值（直立环的期望速度输入）

直立环输入：1.给定角度（速度环输出）。2.角度反馈
输出：PWM（直接控制小车）

5. 转向环

作用：令小车进行前/后/左/右的位移

要向左转，则让左轮反转，右轮正转。要向右转，则让右轮反转，左轮正转。

五、程序设计

1. 编码器

当电机旋转时， 编码器会输出A相和B相相差90度的正交信号脉冲。在本项目中，我们使用编码器接口模式 3，即上升沿、下降沿均计数。把A相和B相的所有边沿作为计数器的计数时钟，根据另一相的状态判定是计数自增还是自减。如当A相为上升沿时，若B相为低电平，则计数自增，可知此时电机正转，若B相为高电平，则计数自减，可知电机反转。

​

1.1 初始化

第一步，开启RCC时钟,包括GPIO和TIM的时钟

第二步，配置GPIO

第三步，配置时基单元

第四步，配置输入捕获单元，只需配置滤波器和极性

第五步，配置编码器接口模式

第六步，启动定时器

void Encoder_Init_TIM2(void)
{RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE); //开启时钟，TIM2挂载在APB1总线RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);/*配置GPIO*/GPIO_InitTypeDef GPIO_InitStructure;GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPU ; //上拉输入GPIO_InitStructure.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1 ;GPIO_Init(GPIOA,&GPIO_InitStructure);/*配置时基单元*/TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure; //定义TIM结构体TIM_TimeBaseStructInit(&TIM_TimeBaseInitStructure); //赋初始值/*采样频率可由内部时钟加一个时钟分频而来，这里就是指定这个时钟分频的，可见此参数与时基单元关系并不大*/TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //配置计数模式为向上计数/*PWM频率=72MHz/(PSC+1)/(ARR+1)PWM占空比=CCR/(ARR+1)PWM分辨率=1/(ARR+1)*/TIM_TimeBaseInitStructure.TIM_Period=65536-1;  //满量程计数，即使用最大计数范围，也方便换算为负数TIM_TimeBaseInitStructure.TIM_Prescaler=1-1;  //不分频TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure); /*配置输入捕获单元*///STM32会直接占用整个TIMx定时器通道，编码器引脚也是固定的ch1和ch2，通道已被硬件固化，不需配置TIM_ICInitTypeDef Tim_ICInitStructure;TIM_ICStructInit(&Tim_ICInitStructure); //赋初始值Tim_ICInitStructure.TIM_ICFilter=0xf;  //配置滤波器采样频率，采样频率f=fDTS/32，N=8 TIM_ICInit(TIM2,&Tim_ICInitStructure);/*配置编码器接口*/TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//写在后面的会覆盖前面的，所以配置编码器接口的函数要放在配置输入捕获单元的函数后面TIM_ClearFlag(TIM2,TIM_FLAG_Update);TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //配置中断类型为溢出更新TIM_SetCounter(TIM2,0);TIM_Cmd(TIM2,ENABLE);
}

1.2 编码器速度读取函数

在单位时间内读取编码器脉冲数可得到速度，实现方法就是每隔一段固定的时间触发一次定时中断，采集编码器计数值并保存，再将定时器的计数值清零。

为什么要在TIM_GetCounter前加上（short）进行强制类型转换呢？

猜想是因为函数TIM_GetCounter的返回值为uint16_t（unsigned short int）数据类型，即无符号数，而我们的项目在测速方面需要的是有符号数，所以得通过强制类型转换为有符号数。

int Read_Encoder(int TIMx) 
{int value_1;switch(TIMx){case 2:value_1=(short)TIM_GetCounter(TIM2);TIM_SetCounter(TIM2,0);break; 
//if是编码器2，1.采集编码器计数值并保存；2.将定时器的计数值清零case 4:value_1=(short)TIM_GetCounter(TIM4);TIM_SetCounter(TIM4,0);break;default:value_1=0;}return value_1;
}void TIM2_IRQHandler(void) 
{if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET) //检查中断标志位{TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中断标志位}
}void TIM4_IRQHandler(void) 
{if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET) //检查中断标志位{TIM_ClearITPendingBit(TIM4,TIM_IT_Update); //清除中断标志位}
}

2. PWM

PWM(Pulsw Width Modulation)，脉冲宽度调制，在具有惯性的系统中，可通过对一系列脉冲宽度进行调制，来等效获得所需模拟参量。

数字量由0和1组成，是离散的。模拟量则是在一定范围内连续变化的量。用模拟量点灯可以通过调节电位器让灯呈现不同的亮度。而用数字量点灯，按理说LED只有亮或灭两种状态。比如说给的是5V电压，那就呈现5V的亮度。但只要以很快的频率让LED不断点亮、熄灭、点亮、熄灭，由于视觉暂留，人眼就看不出灯在闪烁了，灯可以呈现出0~5V之间的一个亮度。通过脉冲宽度调制，调节高低电平的时间比例，就能让LED呈现出不同的亮度级别，实现和用模拟量点灯一样的效果。

电机调速也是一样的，由于电机转动具有惯性，所以只要脉冲的频率够快，电机就能维持在一个速度，调节高低电平的时间比例，就能调节电机的转速。

PWM参数：
频率=1/一个周期的时间，占空比=高电平时间/一个周期的时间，分辨率=占空比变化步距

PWM频率越快，等效模拟的信号就越平稳，但性能开销也越大。

占空比越大，等效模拟的电压就趋近于高电平，占空比越小，等效模拟的电压就越趋近于低电平。

分辨率指占空比变化的精细程度，如1%分辨率就是1%→2%→3%，这样以1%的步距跳变，0.1%就是以0.1%的步距跳变。

我们可以使用STM32单片机的输出比较通道来输出PWM波，本项目的输出比较模式为PWM模式1，向上计数时，若CNT<CCR，置有效电平，CNT>=CCR，置无效电平。

我们设置PWM波，设定CCR值即可，CNT会自增。以下是涉及到的公式：

        PWM频率=72MHz/(PSC+1)/(ARR+1) （STM32F103C8T6的主频为72MHz）
        PWM占空比=CCR/(ARR+1)
         PWM分辨率=1/(ARR+1)

2.1 初始化

第一步，开启RCC时钟,包括GPIO和TIM的时钟

第二步，配置GPIO

第三步，配置时基单元

第四步，配置输出比较单元，设置CCR、输出比较模式、极性选择和输出使能

第五步，主输出使能

第六步，使能OC和ARR的预装载寄存器

第七步，启动定时器

void MiniBalance_PWM_Init(uint16_t Arr,uint16_t Psc)
{RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1,ENABLE); //开启时钟，TIM1挂载在APB2总线/*配置GPIO*/GPIO_InitTypeDef GPIO_InitStructure;GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP ; //复用推挽输出GPIO_InitStructure.GPIO_Pin=GPIO_Pin_8 | GPIO_Pin_11 ;GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;GPIO_Init(GPIOA,&GPIO_InitStructure);/*配置时基单元*/TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure; //定义TIM结构体TIM_TimeBaseStructInit(&TIM_TimeBaseInitStructure); //赋初始值/*采样频率可由内部时钟加一个时钟分频而来，这里就是指定这个时钟分频的，可见此参数与时基单元关系并不大*/TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //配置计数模式为向上计数/*PWM频率=72MHz/(PSC+1)/(ARR+1)PWM占空比=CCR/(ARR+1)PWM分辨率=1/(ARR+1)*/TIM_TimeBaseInitStructure.TIM_Period=Arr-1;  //设定ARR值，取值范围是0~65535TIM_TimeBaseInitStructure.TIM_Prescaler=Psc-1;  //设定PSC值，取值范围是0~65535TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStructure); /*配置输出比较单元*/TIM_OCInitTypeDef Tim_OcInitStructure;TIM_OCStructInit(&Tim_OcInitStructure); //赋初始值Tim_OcInitStructure.TIM_OCMode=TIM_OCMode_PWM1; //设置输出比较模式为PWM模式1Tim_OcInitStructure.TIM_OCPolarity=TIM_OCPolarity_High; //设置为高极性，即极性不翻转，REF波形直接输出，或者说有效电平为高电平Tim_OcInitStructure.TIM_OutputState=TIM_OutputState_Enable; //输出使能Tim_OcInitStructure.TIM_Pulse=0; //设定CCR值TIM_OC1Init(TIM1,&Tim_OcInitStructure);TIM_OC4Init(TIM1,&Tim_OcInitStructure);TIM_CtrlPWMOutputs(TIM1,ENABLE); //TIM1是高级定时器，必须得主输出使能才可以输出PWM波形TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);	//使能OC1预装载寄存器TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);	//使能OC4预装载寄存器TIM_ARRPreloadConfig(TIM1,ENABLE);	//使能TIM1在ARR上的预装载寄存器TIM_Cmd(TIM1,ENABLE); //使能计数器
}

3. 电机驱动

PWMB、PWMA 分别用于控制左右电机的转速。BIN1、BIN2 用于控制左电机的正反转，BO1、BO2驱动左电机。AIN1、AIN2 用于控制右电机的正反转，AO1、AO2驱动右电机。注意，必须要有 PWM 的输入才会有电机线 O1 和 O2 的输出，才能驱动电机转动。PWM 的大小对应电机转速大小。

3.1 初始化

第一步，开启GPIO的RCC时钟

第二步，配置GPIO

void MiniBalance_Motor_Init(void)
{RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE); //开启时钟/*配置GPIO*/GPIO_InitTypeDef GPIO_InitStructure;GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP ; //推挽输出GPIO_InitStructure.GPIO_Pin=GPIO_Pin_12 | GPIO_Pin_13 |GPIO_Pin_14 | GPIO_Pin_15;GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;GPIO_Init(GPIOB,&GPIO_InitStructure);
}

3.2 赋值函数

PWM频率=72MHz/(PSC+1)/(ARR+1)，频率为10KHz。

MiniBalance_PWM_Init(7200,1);   //初始化PWM 10KHZ与电机硬件接口，用于驱动电机

#define PWMB   TIM1->CCR4  
#define PWMA   TIM1->CCR1  

赋值给CCR，占空比值即PWM值。

void Set_Pwm(int motor_left,int motor_right)
{if(motor_left>0){BIN1=0;BIN2=1; //前进 }else  {BIN1=1;BIN2=0; //后退}		PWMB=myabs(motor_left);if(motor_right>0){AIN2=0;AIN1=1;	//前进  }else{AIN2=1;AIN1=0; //后退}		PWMA=myabs(motor_right);
}

3.3 绝对值函数

赋给CCR的值不能为负数，所以需要在赋值之前进行绝对值处理。

int myabs(int a)
{ 		   int temp;if(a<0)  temp=-a;  else temp=a;return temp;
}

3.4 限幅函数

防止电机转速过大，需要有限幅函数。之所以设置为-7000~7000，是因为PWM占空比=CCR/(ARR+1)，PWM值的范围为0~7200，因此要将幅值限制在-7200~7200。又因为最大转速容易损伤电机，所以设置为-7000~7000。

//PWM限幅
Motor_Left=PWM_Limit(Motor_Left,7000,-7000);
Motor_Right=PWM_Limit(Motor_Right,7000,-7000);			    

int PWM_Limit(int IN,int max,int min)
{int OUT = IN;if(OUT>max) OUT = max;if(OUT<min) OUT = min;return OUT;
}

4.EXTI

EXTI基本结构：

4.1 初始化

第一步，开启RCC时钟，包括GPIO和AFIO的时钟，EXTI和NVIC的时钟是一直开着的，不需要我们来开

第二步，配置GPIO

第三步，配置AFIO,即调用GPIO_EXTILineConfig函数选择中断引脚，该函数虽然名字里带有GPIO，实际上与GPIO无关，而是与AFIO有关

第四步，配置EXTI，选择触发响应模式和边沿触发方式

第五步，配置NVIC，给中断选择优先级

void MPU6050_EXTI_Init(void)
{RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO,ENABLE); //开启时钟/*配置GPIO*/GPIO_InitTypeDef GPIO_InitStructure;GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPU; //上拉输入GPIO_InitStructure.GPIO_Pin=GPIO_Pin_12 ;GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;GPIO_Init(GPIOA,&GPIO_InitStructure);/*中断引脚选择，该函数与AFIO相关*/GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource12);EXTI_InitTypeDef EXTI_InitStructure;EXTI_InitStructure.EXTI_Line=EXTI_Line12; //将EXTI的12号线路配置为中断线EXTI_InitStructure.EXTI_LineCmd=ENABLE;   //开启中断EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt; //指定触发响应模式，这里选择中断响应EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Falling; //下降沿触发EXTI_Init(&EXTI_InitStructure);}

5. I2C通信

使用MPU6050需要先编写I2C通信层代码。

I2C是一种同步、半双工，带数据应答的通用数据总线，有两根通信线：SCL(时钟线)、SDA(数据线)。硬件电路为挂载的所有设备的SCL连接在一起，SDA连接在一起，SCL和SDA各添加一个上拉电阻。

5.1时序基本单元

起始条件：SCL高电平期间，SDA从高电平切换到低电平

终止条件：SCL高电平期间，SDA从低电平切换到高电平

发送一个字节：SCL低电平期间，主机将数据位依次放到SDA线上（高位先行），然后释放SCL(即主机放开对SCL的控制，让其恢复高电平)，从机将在SCL高电平期间读取数据位，所以SCL高电平期间SDA不允许有数据变化，依次循环上述过程8次，即可发送一个字节。

接收一个字节：SCL低电平期间，从机将数据位依次放到SDA线上（高位先行），然后释放SCL，主机将在SCL高电平期间读取数据位，所以SCL高电平期间SDA不允许有数据变化，依次循环上述过程8次，即可接收一个字节（主机在接收之前，需要释放SDA）。

发送应答：主机在接收完一个字节之后，在下一个时钟发送一位数据，数据0表示应答，数据1表示非应答。

接收应答：主机在发送完一个字节之后，在下一个时钟接收一位数据，判断从机是否应答，数据0表示应答，数据1表示非应答（主机在接收之前，需要释放SDA）。

发送完数据需要调接收应答，接收完数据需要调发送应答。

关于应答，我是这么理解的：以接收应答为例，SDA默认高电平，主机和从机都可以使其变为低电平，若主机释放SDA，SDA仍为低电平，说明从机在使其为低电平，那么主机就知道从机有应答了。若主机释放SDA，SDA恢复为高电平，说明从机没有动作，即没有应答。

5.2 I2C时序

指定地址写：

指定地址读：

5.3 初始化

开启时钟及配置GPIO

void IIC_Init(void)
{			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //开启时钟GPIO_InitTypeDef GPIO_InitStructure;GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9;	//端口配置GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;      //推挽输出GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;    GPIO_Init(GPIOB, &GPIO_InitStructure);				
}

5.4 引脚操作的封装和改名

void MyI2C_W_SCL(uint8_t BitValue)
{GPIO_WriteBit(GPIOB, GPIO_Pin_10, (BitAction)BitValue); 
//BitAction可将操作对象转换为位域，或者说位变量Delay_us(10);
}void MyI2C_W_SDA(uint8_t BitValue)
{GPIO_WriteBit(GPIOB, GPIO_Pin_11, (BitAction)BitValue);Delay_us(10);
}uint8_t MyI2C_R_SDA(void)
{uint8_t BitValue;BitValue = GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_11);Delay_us(10);return BitValue;
}

5.5 模拟I2C时序基本单元

5.5.1 起始条件

void MyI2C_Start(void)
{MyI2C_W_SDA(1); //为了兼容起始条件和重复起始条件，先释放SDA再释放SCLMyI2C_W_SCL(1);MyI2C_W_SDA(0);MyI2C_W_SCL(0);
}

5.5.2 I2C终止条件

void MyI2C_Stop(void)
{MyI2C_W_SDA(0); //为了确保之后释放SDA能产生上升沿，要先拉低SDA,然后释放SCL，再释放SDAMyI2C_W_SCL(1);MyI2C_W_SDA(1);
}

5.5.3 发送一个字节

void MyI2C_SendByte(uint8_t Byte)
{uint8_t i;for(i=0;i<8;i++){MyI2C_W_SDA(Byte & (0x80>>i)); //主机依次将Byte的各位数据放到SDAMyI2C_W_SCL(1);    //每次释放SCL，从机都会把放在SDA的数据读走MyI2C_W_SCL(0);    //进入下一个时序单元}	
}

5.5.4 接收一个字节

uint8_t MyI2C_ReceiveByte(void)
{uint8_t i,Byte=0x00;MyI2C_W_SDA(1); //主机在接收之前，要先释放SDAfor(i=0;i<8;i++){MyI2C_W_SCL(1);    //释放SCLif(MyI2C_R_SDA()==1){Byte |= (0x80>>i);} //读SDA，如果读到的值为1，则在Byte的对应位置1，为0，则条件不成立，Byte该位为默认值0MyI2C_W_SCL(0); //进入下一个时序单元}return Byte;
}

5.5.5  发送应答

void MyI2C_SendAck(uint8_t AckBit)
{uint8_t i;MyI2C_W_SDA(AckBit); //主机把AckBit放到SDA MyI2C_W_SCL(1);     //从机读取应答MyI2C_W_SCL(0);     //进入下一个时序单元
}

5.5.6 接收应答

uint8_t MyI2C_ReceiveAck(void)
{uint8_t AckBit;MyI2C_W_SDA(1); 		//主机释放SDA，同时从机把应答位放到SDA上MyI2C_W_SCL(1);    		//释放SCLAckBit=MyI2C_R_SDA(); //主机读取应答。读到0，说明从机把SDA拉低了，有应答；读到1，则说明从机没有应答MyI2C_W_SCL(0);			//进入下一个时序单元return AckBit;
}

5.6 I2C时序

5.6.1 指定地址写

#define MPU6050_ADDRESS 	 0xD0void I2C_Write(uint8_t RegAddress,uint8_t Data)
{MyI2C_Start();MyI2C_SendByte(MPU6050_ADDRESS);MyI2C_ReceiveAck();MyI2C_SendByte(RegAddress);MyI2C_ReceiveAck();MyI2C_SendByte(Data);MyI2C_ReceiveAck();MyI2C_Stop();
}

5.6.2 指定地址读

uint8_t I2C_Read(uint8_t RegAddress)
{uint8_t Data;MyI2C_Start();MyI2C_SendByte(MPU6050_ADDRESS);MyI2C_ReceiveAck();MyI2C_SendByte(RegAddress);MyI2C_ReceiveAck();MyI2C_Start();MyI2C_SendByte(MPU6050_ADDRESS | 0x01);MyI2C_ReceiveAck();Data = MyI2C_ReceiveByte();MyI2C_SendAck(1);MyI2C_Stop();return Data;
}

6. MPU6050

MPU6050是一个六轴姿态传感器，使用I2C通信接口，可用于测量小车X、Y、Z轴的加速度值和角速度参数，通过内置的DMP(数字运动处理器)得到四元数，再在程序中计算出小车的俯仰角、横滚角和偏航角，就能得出小车姿态。

MPU6050的程序，尤其是DMP的代码非常复杂，因此本项目移植了正点原子的MPU6050程序文件。以下为摘选出的两个关键函数。

6.1  初始化

void MPU6050_initialize(void) 
{MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); //设置时钟MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺仪量程设置MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);	//加速度度最大量程 +-2GMPU6050_setSleepEnabled(0); //进入工作状态MPU6050_setI2CMasterModeEnabled(0);	 //不让MPU6050 控制AUXI2CMPU6050_setI2CBypassEnabled(0);	 //主控制器的I2C与	MPU6050的AUXI2C	直通关闭
}

6.2 读取MPU6050内置DMP的姿态信息

void Read_DMP(void)
{
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
//读取 DMP 数据
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30; 
//读取到的四元数是放大了 2^30 倍的，需要除以2^30,变换为原来的数据
Roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //计算出横滚角
Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2*
q2 + 1)* 57.3; // 计算出俯仰角
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
//计算出偏航角
}
}

7. 蓝牙模块

使用蓝牙模块需要先编写USART通信层代码。

USART是STM32内部集成的串口通信设备。串口通信需要两根数据线，连接发送端TX和接收端RX，TX和RX要交叉连接。

7.1 串口参数及时序

波特率：串口通信的速率

起始位：标志一个数据帧的开始，固定为低电平

数据位：数据帧的有效载荷，1为高电平，0为低电平，低位先行

校验位：用于数据验证，根据数据位计算得来

停止位：用于数据帧间隔，固定为高电平

7.2  初始化

第一步，开启RCC时钟,包括GPIO和USART的时钟

第二步，配置GPIO

第三步，配置USART

第四步，需要接收的话，要配置中断，加上ITConfig和NVIC的代码再开启USART。

第五步，开启USART

void uart3_init(u32 bound)
{  	 //GPIO端口设置GPIO_InitTypeDef GPIO_InitStructure;USART_InitTypeDef USART_InitStructure;NVIC_InitTypeDef NVIC_InitStructure;RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);	//使能UGPIOB时钟RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);	//使能USART3时钟//USART3_TX  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; //PB.10GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;	//复用推挽输出GPIO_Init(GPIOB, &GPIO_InitStructure);//USART3_RX	  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;//PB11GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入GPIO_Init(GPIOB, &GPIO_InitStructure);//Usart3 NVIC 配置NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0 ;//抢占优先级NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;		//子优先级NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			//IRQ通道使能NVIC_Init(&NVIC_InitStructure);	//根据指定的参数初始化VIC寄存器//USART 初始化设置USART_InitStructure.USART_BaudRate = bound;//串口波特率USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//收发模式USART_Init(USART3, &USART_InitStructure);     //初始化串口3USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//开启串口接受中断USART_Cmd(USART3, ENABLE);                    //使能串口3 }

7.3 串口3获取APP控制指令

使用中断函数获取

void USART3_IRQHandler(void)

收到数据后就进行判断，操作相应的前、后、左、右、刹标志位

if(uart_receive==0x5A) Flag_front=0,Flag_back=0,Flag_Left=0,Flag_Right=0;//刹车
else if(uart_receive==0x41)	Flag_front=1,Flag_back=0,Flag_Left=0,Flag_Right=0;//前
else if(uart_receive==0x45)	Flag_front=0,Flag_back=1,Flag_Left=0,Flag_Right=0;//后
else if(uart_receive==0x42||uart_receive==0x43||uart_receive==0x44)																 Flag_front=0,Flag_back=0,Flag_Left=0,Flag_Right=1;  //左
else if(uart_receive==0x46||uart_receive==0x47||uart_receive==0x48)	    																	 Flag_front=0,Flag_back=0,Flag_Left=1,Flag_Right=0;  //右
else Flag_front=0,Flag_back=0,Flag_Left=0,Flag_Right=0;//刹车

设置起始位和停止位

if(Usart3_Receive==0x7B) Flag_PID=1;   //APP参数指令起始位
if(Usart3_Receive==0x7D) Flag_PID=2;   //APP参数指令停止位

数据包

if(Flag_PID==1)  //采集数据{Receive[i]=Usart3_Receive;i++;}if(Flag_PID==2)  //采集结束，开始分析数据{if(Receive[3]==0x50) 				 PID_Send=1;else if(Receive[1]!=0x23) {								for(j=i;j>=4;j--){Data+=(Receive[j-1]-48)*pow(10,i-j);}switch(Receive[1]){case 0x30:  Balance_Kp=Data;break;case 0x31:  Balance_Kd=Data;break;case 0x32:  Velocity_Kp=Data;break;case 0x33:  Velocity_Ki=Data;break;case 0x34:  Turn_Kp=Data;break; case 0x35:  Turn_Kd=Data;break; case 0x36:  break; //预留case 0x37:  break; //预留case 0x38:  break; //预留}}				 Flag_PID=0;i=0;j=0;Data=0;memset(Receive, 0, sizeof(u8)*50);//数组清零} }  											 
} 

8.PID算法

8.1 直立环

角度的微分就是角速度，所以D控制=Kd*(期望角速度-当前角速度)

int Balance(float Med,float Angle,float Gyro)
{  float Angle_bias,Gyro_bias;int balance;Angle_bias=Med-Angle; //求出期望角度与当前角度的偏差Gyro_bias=0-Gyro; balance=-Balance_Kp/100*Angle_bias-Gyro_bias*Balance_Kd/100; //计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数 return balance;
}

8.2 速度环

int Velocity(int encoder_left,int encoder_right)
{  static float velocity,Encoder_Least,Encoder_bias,Movement;static float Encoder_Integral,Target_Velocity;//================遥控前进后退部分====================// if(Flag_front==1)    	Movement=Target_Velocity/Flag_velocity;	  //收到前进信号else if(Flag_back==1)	Movement=-Target_Velocity/Flag_velocity;  //收到后退信号else  Movement=0;//================速度PI控制器=====================//Encoder_Least =0-(encoder_left+encoder_right); //获取最新速度偏差=目标速度（此处为零）-测量速度（左右编码器之和）Encoder_bias *= 0.86;	                     //一阶低通滤波器       Encoder_bias += Encoder_Least*0.14;	         //一阶低通滤波器，减缓速度变化 Encoder_Integral +=Encoder_bias;             //积分出位移 积分时间：10msEncoder_Integral=Encoder_Integral+Movement;  //接收遥控器数据，控制前进后退if(Encoder_Integral>10000)    Encoder_Integral=10000;             //积分限幅if(Encoder_Integral<-10000)	  Encoder_Integral=-10000;            //积分限幅	velocity=-Encoder_bias*Velocity_Kp/100-Encoder_Integral*Velocity_Ki/100;     //速度控制	if(Turn_Off(Angle_Balance,Voltage)==1||Flag_Stop==1) Encoder_Integral=0;//电机关闭后清除积分return velocity;
}

8.3 转向环

int Turn(float gyro)
{static float Turn_Target,turn,Turn_Amplitude=54;float Kp=Turn_Kp,Kd;	 //修改转向速度，修改Turn_Amplitude即可//===================遥控左右旋转部分=================//if(1==Flag_Left)	        Turn_Target=-Turn_Amplitude/Flag_velocity;else if(1==Flag_Right)	 Turn_Target=Turn_Amplitude/Flag_velocity; else Turn_Target=0;if(1==Flag_front||1==Flag_back)  Kd=Turn_Kd;        else Kd=0;   //转向的时候取消陀螺仪的纠正 有点模糊PID的思想//===================转向PD控制器=================//turn=Turn_Target*Kp/100+gyro*Kd/100;//结合Z轴陀螺仪进行PD控制return turn;		 //转向环PWM右转为正，左转为负
}

8.4 计算PWM值

机械中值：能使小车真正平衡住的角度。由于小车的安装往往存在一定的误差，导致重心会有一定的偏移，所以让小车最能平衡的角度不一定是0度。所以直立环的期望角度=速度环输出+机械中值

Velocity_Out=Velocity(Encoder_Left,Encoder_Right);  //速度环PI控制	
Balance_Out=Balance(Velocity_Out+Middle_angle,Angle_Balance,Gyro_Balance);            
//直立环PD控制，直立环的期望角度=速度环输出+机械中值
Turn_Out=Turn(Gyro_Turn);	     //转向环PID控制     
PWM_Out=Balance_Out;			 //串级PID最终输出量		
Motor_Left=PWM_Out+Turn_Out;     //计算左轮电机最终PWM
Motor_Right=PWM_Out-Turn_Out;    //计算右轮电机最终PWM
//PWM值正数使小车前进，负数使小车后退

9.电池电压检测

电池电压通过 ADC 进行测量。

9.1 初始化

第一步，开启RCC时钟,包括GPIO和ADC的时钟

第二步，配置GPIO

第三步，配置多路开关

第四步，配置ADC转换器

第五步，开关控制，开启ADC

void AD_Init(void)
{RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1,ENABLE);RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);RCC_ADCCLKConfig(RCC_PCLK2_Div6); //ADCCLK=PCLK2(72MHz)/6=12MHz，ADC最大时间不能超过14MHzGPIO_InitTypeDef GPIO_InitStructure;GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; //模拟输入GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;GPIO_Init(GPIOA, &GPIO_InitStructure);ADC_InitTypeDef ADC_InitStructure;ADC_DeInit(ADC1);  //复位ADC1,将外设 ADC1 的全部寄存器重设为缺省值	ADC_InitStructure.ADC_Mode=ADC_Mode_Independent; //独立模式ADC_InitStructure.ADC_DataAlign=ADC_DataAlign_Right ; //右对齐ADC_InitStructure.ADC_ExternalTrigConv=ADC_ExternalTrigConv_None; //不使用外部触发，即使用内部软件触发ADC_InitStructure.ADC_ContinuousConvMode=DISABLE; //单次转换ADC_InitStructure.ADC_ScanConvMode=DISABLE; //非扫描模式ADC_InitStructure.ADC_NbrOfChannel=1; //顺序进行规则转换的ADC通道的数目ADC_Init(ADC1,&ADC_InitStructure);ADC_Cmd(ADC1,ENABLE); //使能指定的ADC1/*ADC校准*/ADC_ResetCalibration(ADC1); //复位校准，把RSTCAL位置1while (ADC_GetResetCalibrationStatus(ADC1)); //复位校准完成后，RSTCAL位清零，然后跳出该循环；ADC_StartCalibration(ADC1); //启动校准while(ADC_GetCalibrationStatus(ADC1)); //校准完成后，校准标志位清零，跳出该循环
}

9.2 AD采样

uint16_t Get_Adc(uint8_t ADC_Channel)
{ADC_RegularChannelConfig(ADC1,ADC_Channel,1,ADC_SampleTime_239Cycles5);//ADC1,ADC通道,采样时间为239.5周期ADC_SoftwareStartConvCmd(ADC1,ENABLE); //软件触发转换while(ADC_GetFlagStatus(ADC1,ADC_FLAG_EOC)==RESET); //转换完成后，转换结束位会置1return ADC_GetConversionValue(ADC1); //获取转换值
}

9.3 读取电池电压

int Get_battery_volt(void)   
{  int Volt;//电池电压Volt=Get_Adc(Battery_Ch)*3.3*67/4096*10;	//电阻分压，具体根据原理图简单分析可以得到	return Volt;
}

10. main函数

主函数主要做一些初始化工作，完成后进入一个死循环。主要控制功能在外部中断函数里面，5ms 定时中断由 MPU6050 的 INT 引脚触发。我 们把比较耗时的 OLED 显示和 APP 发送数据等函数放在主函数里面，因为 5ms 定时中断由 MPU6050 的 INT 引脚触发，需严格保证采样和数据处理时间的同步。

六、PID调参

1. 确定小车的机械中值

先从后往前绕电机轴轻推小车，直到小车向前倒，记录倒下时的角度。然后从前往后绕电机轴轻推小车，直到小车向后倒，记录倒下时的角度。取两次角度的中间值作为机械中值。

2. 注意事项

在调试【速度环参数极性】时：需要去掉(注释掉) 【直立环运算】

在调试【速度环参数大小】时：再次引入(取消注释) 【直立环运算】

【转向环运算】始终是去掉(注释) 的一个状态。若转向环已提前将参数调试好，则未注释也影响不大。

3. 直立环参数整定

确定Kp极性
极性错误：小车往哪边倒，车轮就往反方向开，会使得小车加速倒下。

极性正确：小车往哪边倒，车轮就往哪边开，以保证小车有直立的趋势。

确定Kp大小

一直增加Kp，直到出现大幅低频震荡。

确定Kd大小

极性错误：拿起小车绕电机轴旋转，车轮反向转动，无跟随。

极性正确：拿起小车绕电机轴旋转，车轮同向转动，有跟随。

确定Kd大小

Kd一直增加，直到出现高频震荡。

直立环调试完毕后，对所有确立的参数乘以0.6作为最终参数。
原因:之前得到的参数都是Kp、Kd 最大值，根据工程经验平衡小车的理想参数为最大参数乘以0.6求得。

结果:乘以0.6后,小车的抖动消失，但同时直立效果也变差。待下面加入速度环就能得到更好的性能。

4. 速度环参数整定

Kp&Ki:

线性关系、Ki=(1/200)*Kp、仅调Kp即可。

Kp&Ki极性:

极性错误:手动转动其中一个车轮，另一车轮会以同样速度反向旋转——典型负反馈。

极性正确:手动转动其中一个车轮，两个车伦会同向加速，直至电机最大速度——典型正反馈。

 Kp&Ki大小:

增加Kp&Ki，直至小车保持平衡的同时，速度接近于零。且回位效果较好。

5. 转向环参数整定

确定Kp极性

极性错误：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势一致——典型正反馈。

极性正确：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势相反——典型负反馈。

确定Kp大小

加大Kp，直至走直线效果较好，且无剧烈抖动。