基于stm32的两轮自平衡小车4（软件调试篇）
本篇是软件调试篇，接上一篇硬件篇：基于stm32的两轮自平衡小车3（硬件篇），本篇内容是对硬件部分的软件实现，具体模块详见目录。这里先上效果：转B站
目录
- 定时器PWM驱动程序
- 定时器编码器模式驱动程序
- MPU6050驱动程序
- 运动控制算法实现
- 调试
定时器PWM驱动程序
根据硬件篇分配的GPIO口，对相应的GPIO口进行配置。因为A4950电机驱动模块需要四路PWM才可以控制前进和后退，即两路PWM控制前进、两路PWM控制后退，分配的GPIO口为PA0、PA1、PA8、PA11，分别对应定时器2通道1、定时器2通道2、定时器1通道1、定时器1通道4。程序设计如下：
/********************************************************************************************
**函数功能：利用定时器产生PWM波，输出PWM波控制外设
**相关说明：脉冲宽度调制(PWM)，是英文“Pulse Width Modulation” 的缩写，简称脉宽调制，是利用
微处理器的数字输出来对模拟电路进行控制的一种非常有效的技术.通用定时器也能同时产生
多达 4路的 PWM 输出.
**资源使用：GPIO 定时器通道 A4950输入 A4950输出 电机接线
PA0 --> TIM2_CH1 --> AIN1 --> AOUT1 --> X2_M1-
PA1 --> TIM2_CH2 --> AIN2 --> AOUT2 --> X2_M1+
PA8 --> TIM1_CH1 --> BIN1 --> BOUT1 --> X1_M2-
PA11 --> TIM1_CH4 --> BIN2 --> BOUT2 --> X1_M2+
PC13 --> LED0
**电机构成：1 --> M1 MOTOR-
2 --> ENCODER GND
3 --> ENCODER A PHASE
4 --> ENCODER B PHASE
5 --> 3.3V ENCODER
6 --> M1 MOTOR+
**电机参数：额定DC 12V（大于7.4V应该就可以了）， 空载转速366rpm，减速比：1/30，精度：13*30
**相关解释：占空比： 输出的方波周期就是自动重装载值的值（us为单位），占空比 = compare_num / arr
PWM模式1：向上计数时，一旦TIMx_CNTTIMx_CCR1时通道1为无效电平(OC1REF=0)，否
则为有效电平(OC1REF=1)
PWM模式2：向上计数时，一旦TIMx_CNTTIMx_CCR1时通道1为有效电平，否
则为无效电平
**调试注意：如果需要接LED灯调试电机IO口，则接线方法为GPIO口接LED的“-”，“+”接3.3V
********************************************************************************************/
#include "pwm.h"
#include "delay.h"
GPIO_InitTypeDef GPIOA_Initstructure;
TIM_TimeBaseInitTypeDef TIM1_Initsturcture;
TIM_TimeBaseInitTypeDef TIM2_Initsturcture;
TIM_OCInitTypeDef TIM1_OC1_Initstructure;
TIM_OCInitTypeDef TIM1_OC4_Initstructure;
TIM_OCInitTypeDef TIM2_OC1_Initstructure;
TIM_OCInitTypeDef TIM2_OC2_Initstructure;
//定时器1PWM函数初始化
void TIM1_PWM_Init(u16 arr, u16 psc)
{
//初始化TIM2时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE); //使能TIM1
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//使能GPIOA
//GPIO复用初始化
GPIOA_Initstructure.GPIO_Pin=GPIO_Pin_8|GPIO_Pin_11;//PA8、PA11
GPIOA_Initstructure.GPIO_Mode=GPIO_Mode_AF_PP; //复用推挽输出
GPIOA_Initstructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIOA_Initstructure);
//TIM1初始化
TIM1_Initsturcture.TIM_Prescaler=psc; //预分频系数
TIM1_Initsturcture.TIM_Period=arr; //自动装载值
TIM1_Initsturcture.TIM_CounterMode=TIM_CounterMode_Up; //向上计数
TIM1_Initsturcture.TIM_ClockDivision=0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseInit(TIM1,&TIM1_Initsturcture);
//设置TIM1_CH1的PWM模式及通道方向
TIM1_OC1_Initstructure.TIM_OCMode=TIM_OCMode_PWM2; //选择PWM模式2
TIM1_OC1_Initstructure.TIM_OutputState=TIM_OutputState_Enable;//比较输出使能
TIM1_OC1_Initstructure.TIM_OCPolarity=TIM_OCPolarity_High; //输出极性高
TIM1_OC1_Initstructure.TIM_Pulse=0; //设置待装入捕获比较寄存器的脉冲值
TIM_OC1Init(TIM1,&TIM1_OC1_Initstructure); //CH1
//设置TIM1_CH4的PWM模式及通道方向
TIM1_OC4_Initstructure.TIM_OCMode=TIM_OCMode_PWM2; //选择PWM模式2
TIM1_OC4_Initstructure.TIM_OutputState=TIM_OutputState_Enable;//比较输出使能
TIM1_OC4_Initstructure.TIM_OCPolarity=TIM_OCPolarity_High; //输出极性高
TIM1_OC4_Initstructure.TIM_Pulse=0; //设置待装入捕获比较寄存器的脉冲值
TIM_OC4Init(TIM1,&TIM1_OC4_Initstructure); //CH4
TIM_CtrlPWMOutputs(TIM1,ENABLE); //MOE 主输出使能，高级定时器使用
TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable); //CH1 预装载使能
TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable); //CH2 预装载使能
TIM_ARRPreloadConfig(TIM1,ENABLE); //使能 TIM2 在 ARR 上的预装载寄存器
//使能TIM1
TIM_Cmd(TIM1,ENABLE);
}
//定时器2PWM函数初始化
void TIM2_PWM_Init(u16 arr, u16 psc)
{
//初始化TIM2时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE); //使能TIM2
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); //使能GPIOA
//GPIO复用初始化
GPIOA_Initstructure.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1; //PA0、PA1
GPIOA_Initstructure.GPIO_Mode=GPIO_Mode_AF_PP; //复用推挽输出
GPIOA_Initstructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIOA_Initstructure);
//TIM2初始化
TIM2_Initsturcture.TIM_Prescaler=psc; //预分频系数
TIM2_Initsturcture.TIM_Period=arr; //自动装载值
TIM2_Initsturcture.TIM_CounterMode=TIM_CounterMode_Up; //向上计数
TIM2_Initsturcture.TIM_ClockDivision=0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseInit(TIM2,&TIM2_Initsturcture);
//设置TIM2_CH1的PWM模式及通道方向
TIM2_OC1_Initstructure.TIM_OCMode=TIM_OCMode_PWM2; //选择PWM模式2
TIM2_OC1_Initstructure.TIM_OutputState=TIM_OutputState_Enable;//比较输出使能
TIM2_OC1_Initstructure.TIM_OCPolarity=TIM_OCPolarity_High; //输出极性高
TIM2_OC1_Initstructure.TIM_Pulse=0; //设置待装入捕获比较寄存器的脉冲值
TIM_OC1Init(TIM2,&TIM2_OC1_Initstructure); //CH1
//设置TIM2_CH2的PWM模式及通道方向
TIM2_OC2_Initstructure.TIM_OCMode=TIM_OCMode_PWM2; //选择PWM模式2
TIM2_OC2_Initstructure.TIM_OutputState=TIM_OutputState_Enable;//比较输出使能
TIM2_OC2_Initstructure.TIM_OCPolarity=TIM_OCPolarity_High; //输出极性高
TIM2_OC2_Initstructure.TIM_Pulse=0; //设置待装入捕获比较寄存器的脉冲值
TIM_OC2Init(TIM2,&TIM2_OC2_Initstructure); //CH2
TIM_OC1PreloadConfig(TIM2,TIM_OCPreload_Enable); //CH1 预装载使能
TIM_OC2PreloadConfig(TIM2,TIM_OCPreload_Enable); //CH2 预装载使能
TIM_ARRPreloadConfig(TIM2,ENABLE); //使能 TIM2 在 ARR 上的预装载寄存器
//使能TIM2
TIM_Cmd(TIM2,ENABLE);
}
需要注意的是，定时器1是高级定时器，在初始化的时候和通用定时器不完全相同，具体的说明在原理篇有写。
初始化PWM定时器后，再赋值给PWM寄存器，这里用一个函数实现：
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：PWM
返回 值：无
**************************************************************************/
void Set_Pwm(int moto1,int moto2)
{
//电机1
if(moto1<0)
{
TIM1_CCR4=myabs(moto1); //前进
TIM1_CCR1=0;
}
if(moto1>=0)
{
TIM1_CCR1=myabs(moto1); //后退
TIM1_CCR4=0;
}
//电机2
if(moto2<0)
{
TIM2_CCR1=myabs(moto2); //前进
TIM2_CCR2=0;
}
if(moto2>=0)
{
TIM2_CCR2=myabs(moto2); //后退
TIM2_CCR1=0;
}
}
/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回 值：unsigned int
**************************************************************************/
int myabs(int a)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
定时器编码器模式驱动程序
一个电机的编码器有AB两相，因此定时器编码器模式同样需要用到四个定时器通道，这里再次强调：定时器的编码器模式只能由通用定时器和高级定时器的通道1和通道2配置！使用到的GPIO口为：PA6、PA7、PB6、PB7，分别对应定时器3通道1、定时器3通道2、定时器4通道1和定时器4通道2。驱动程序如下：
/*********************************************************************************
**函数功能：利用32自带的编码器模式获取电机转速，用到的定时器是3和4
**电机构成：1 --> M1 MOTOR-
2 --> ENCODER GND
3 --> ENCODER A PHASE
4 --> ENCODER B PHASE
5 --> 3.3V ENCODER
6 --> M1 MOTOR+
**电机参数：额定DC 12V（大于7.4V应该就可以了）， 空载转速366rpm，减速比：1：30
**管脚使用： PA6 --> TIM3_CH1 --> 电机 1 B相
PA7 --> TIM3_CH2 --> 电机 1 A相
PB6 --> TIM4_CH1 --> 电机 2 B相
PB7 --> TIM4_CH2 --> 电机 2 A相
**注意事项：编码器模式只有定时器的通道1和通道2可以用,编码器必须用定时器的通道1、2来捕获
**********************************************************************************/
#include "encoder.h"
#include "stdio.h"
#include "pwm.h"
//TIM3
GPIO_InitTypeDef GPIOA_Initure;
TIM_TimeBaseInitTypeDef TIM3_Base_Initstructure;
TIM_ICInitTypeDef TIM3_ICInitstructure;
NVIC_InitTypeDef NVIC_TIM3_Initstructure;
//TIM4
GPIO_InitTypeDef GPIOB_Initure;
TIM_TimeBaseInitTypeDef TIM4_Base_Initstructure;
TIM_ICInitTypeDef TIM4_ICInitstructure;
NVIC_InitTypeDef NVIC_TIM4_Initstructure;
//定时器3编码器模式初始化
void TIM3_Encoder_Init(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); //开启GPIOA时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE); //开启TIM3时钟
//PA6和PA7初始化
GPIOA_Initure.GPIO_Pin=GPIO_Pin_6|GPIO_Pin_7; //PA6和PA7
GPIOA_Initure.GPIO_Mode=GPIO_Mode_IN_FLOATING; //浮空输入
//GPIOA_Initure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIOA_Initure);
//TIM3复位
//TIM_DeInit(TIM3);
//TIM3初始化
TIM_TimeBaseStructInit(&TIM3_Base_Initstructure); //设置缺省值,这一步最好加上防止放到串口初始化后出问题
TIM3_Base_Initstructure.TIM_Period=ENCODER_TIM_PERIOD; //自动装载值，设置为65536-1
TIM3_Base_Initstructure.TIM_Prescaler=0x0; //预分频系数，不分频，设置为0
TIM3_Base_Initstructure.TIM_CounterMode=TIM_CounterMode_Up;//向上计数
TIM3_Base_Initstructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM3,&TIM3_Base_Initstructure);
//配置为编码器模式,TIM_ICPolarity_Rising 表示极性不反相,TIM_ICPolarity_Falling:表示极性反相
TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM3_ICInitstructure); //设置缺省值,这一步最好加上防止放到串口初始化后出问题
TIM3_ICInitstructure.TIM_ICFilter=10; //配置输入滤波器
TIM_ICInit(TIM3,&TIM3_ICInitstructure);
//中断优先级设置
//NVIC_TIM3_Initstructure.NVIC_IRQChannel=TIM3_IRQn;
//NVIC_TIM3_Initstructure.NVIC_IRQChannelCmd=ENABLE;
//NVIC_TIM3_Initstructure.NVIC_IRQChannelPreemptionPriority=2; //抢占优先级
//NVIC_TIM3_Initstructure.NVIC_IRQChannelSubPriority=3; //子优先级
//NVIC_Init(&NVIC_TIM3_Initstructure);
TIM_ClearFlag(TIM3,TIM_FLAG_Update); //清除更新标志位
TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE); //运行更新中断
TIM_SetCounter(TIM3,0);//该语句与TIM3->CNT=0一样
//TIM3->CNT=0;
TIM_Cmd(TIM3,ENABLE);
}
//定时器4编码器模式初始化
void TIM4_Encoder_Init(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE); //开启GPIOB时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE); //开启TIM4时钟
//PB6和PB7初始化
GPIOB_Initure.GPIO_Pin=GPIO_Pin_6|GPIO_Pin_7; //PB6和PB7
GPIOB_Initure.GPIO_Mode=GPIO_Mode_IN_FLOATING; //浮空输入
//GPIOB_Initure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIOB_Initure);
//TIM4复位
//TIM_DeInit(TIM4);
//TIM3初始化
TIM_TimeBaseStructInit(&TIM4_Base_Initstructure); //设置缺省值,这一步最好加上防止放到串口初始化后出问题
TIM4_Base_Initstructure.TIM_Period=ENCODER_TIM_PERIOD; //自动装载值，设置为65536-1
TIM4_Base_Initstructure.TIM_Prescaler=0x0; //预分频系数，不分频，设置为0
TIM4_Base_Initstructure.TIM_CounterMode=TIM_CounterMode_Up;//向上计数
TIM4_Base_Initstructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM4,&TIM4_Base_Initstructure);
//配置为编码器模式,TIM_ICPolarity_Rising 表示极性不反相,TIM_ICPolarity_Falling:表示极性反相
TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Falling,TIM_ICPolarity_Falling);
TIM_ICStructInit(&TIM4_ICInitstructure); //设置缺省值,这一步最好加上防止放到串口初始化后出问题
TIM4_ICInitstructure.TIM_ICFilter=10; //配置输入滤波器
TIM_ICInit(TIM4,&TIM4_ICInitstructure);
//中断优先级设置
//NVIC_TIM4_Initstructure.NVIC_IRQChannel=TIM4_IRQn;
//NVIC_TIM4_Initstructure.NVIC_IRQChannelCmd=ENABLE;
//NVIC_TIM4_Initstructure.NVIC_IRQChannelPreemptionPriority=2; //抢占优先级
//NVIC_TIM4_Initstructure.NVIC_IRQChannelSubPriority=3; //子优先级
//NVIC_Init(&NVIC_TIM4_Initstructure);
TIM_ClearFlag(TIM4,TIM_FLAG_Update); //清除更新标志位
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE); //运行更新中断
TIM_SetCounter(TIM4,0);//该语句与TIM4->CNT=0一样
//TIM4->CNT=0;
TIM_Cmd(TIM4,ENABLE);
}
//TIM3中断服务函数
void TIM3_IRQHandler(void)
{
if(TIM3->SR&0X0001)//溢出中断
{
}
TIM3->SR&=~(1<<0);//清除中断标志位
}
//TIM4中断服务函数
void TIM4_IRQHandler(void)
{
if(TIM4->SR&0X0001)//溢出中断
{
}
TIM4->SR&=~(1<<0);//清除中断标志位
}
/*
//TIM3中断服务函数
void TIM3_IRQHandler(void)
{
if(TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
{
}
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
}
//TIM4中断服务函数
void TIM4_IRQHandler(void)
{
if(TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
{
}
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
}
*/
//单位时间读取编码器计数函数
//入口参数：定时器值
//返回值： 速度值
int Read_Encoder(u8 TIMx)
{
int Encoder_TIMx;
switch(TIMx)
{
case 3:Encoder_TIMx=(short)TIM3->CNT; TIM3->CNT=0; break;
case 4:Encoder_TIMx=(short)TIM4->CNT; TIM4->CNT=0; break;
default:Encoder_TIMx=0;
}
return Encoder_TIMx;
}
MPU6050驱动程序
MPU6050模块使用的是IIC通信的，IIC又分为硬件IIC和模拟IIC，这里用到的是硬件IIC（当然也可以用模拟IIC，模拟IIC的好处是分配GPIO口更加灵活）。stm32f103c8t6有两个硬件IIC接口IIC1和IIC2，对应的GPIO口为：
PB6 – IIC1_SCL 、PB7 – IIC1_SDA
PB10–IIC2_SCL 、PB11–IIC2_SDA
由于在PB6和PB7已用于电机编码器捕获，因此这里选取PB10和PB11与MPU6050通信。IIC初始化程序如下：
#include "mpuiic.h"
#include "delay.h"
//MPU IIC 延时函数
void MPU_IIC_Delay(void)
{
delay_us(2);
}
//初始化IIC
void MPU_IIC_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//先使能外设IO PORTB时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11; // 端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIO
GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11); //PB10,PB11 输出高
}
//产生IIC起始信号
void MPU_IIC_Start(void)
{
MPU_SDA_OUT(); //sda线输出
MPU_IIC_SDA=1;
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low
MPU_IIC_Delay();
MPU_IIC_SCL=0;//钳住I2C总线，准备发送或接收数据
}
//产生IIC停止信号
void MPU_IIC_Stop(void)
{
MPU_SDA_OUT();//sda线输出
MPU_IIC_SCL=0;
MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_SDA=1;//发送I2C总线结束信号
MPU_IIC_Delay();
}
//等待应答信号到来
//返回值：1，接收应答失败
// 0，接收应答成功
u8 MPU_IIC_Wait_Ack(void)
{
u8 ucErrTime=0;
MPU_SDA_IN(); //SDA设置为输入
MPU_IIC_SDA=1;MPU_IIC_Delay();
MPU_IIC_SCL=1;MPU_IIC_Delay();
while(MPU_READ_SDA)
{
ucErrTime++;
if(ucErrTime>250)
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_SCL=0;//时钟输出0
return 0;
}
//产生ACK应答
void MPU_IIC_Ack(void)
{
MPU_IIC_SCL=0;
MPU_SDA_OUT();
MPU_IIC_SDA=0;
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SCL=0;
}
//不产生ACK应答
void MPU_IIC_NAck(void)
{
MPU_IIC_SCL=0;
MPU_SDA_OUT();
MPU_IIC_SDA=1;
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SCL=0;
}
//IIC发送一个字节
//返回从机有无应答
//1，有应答
//0，无应答
void MPU_IIC_Send_Byte(u8 txd)
{
u8 t;
MPU_SDA_OUT();
MPU_IIC_SCL=0;//拉低时钟开始数据传输
for(t=0;t<8;t++)
{
MPU_IIC_SDA=(txd&0x80)>>7;
txd<<=1;
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SCL=0;
MPU_IIC_Delay();
}
}
//读1个字节，ack=1时，发送ACK，ack=0，发送nACK
u8 MPU_IIC_Read_Byte(unsigned char ack)
{
unsigned char i,receive=0;
MPU_SDA_IN();//SDA设置为输入
for(i=0;i<8;i++ )
{
MPU_IIC_SCL=0;
MPU_IIC_Delay();
MPU_IIC_SCL=1;
receive<<=1;
if(MPU_READ_SDA)receive++;
MPU_IIC_Delay();
}
if (!ack)
MPU_IIC_NAck();//发送nACK
else
MPU_IIC_Ack(); //发送ACK
return receive;
}
其中，IO口的方向设置和操作函数如下：
//IO方向设置PB10、PB11
#define MPU_SDA_IN() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;}
#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;}
//IO操作函数
#define MPU_IIC_SCL PBout(10) //SCL
#define MPU_IIC_SDA PBout(11) //SDA
#define MPU_READ_SDA PBin(11) //输入SDA
//https://blog.csdn.net/qq_22520215/article/details/72357076
//IO方向设置PB6、PB7
//#define MPU_SDA_IN() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=(u32)8<<28;}
//#define MPU_SDA_OUT() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=(u32)3<<28;}
//IO操作函数
//#define MPU_IIC_SCL PBout(6) //SCL
//#define MPU_IIC_SDA PBout(7) //SDA
//#define MPU_READ_SDA PBin(7) //输入SDA
如果上面IO口操作函数中寄存器指令比较难理解的话，可以参考注释的那个链接，或者我以前的一篇博客：STM32使用MPU6050在TFT_LCD上显示数据。
IIC初始化后，再对MPU6050进行相关配置。MPU6050的配置东西比较多，调用的库文件也有好几个，绕来绕去的，建议参考原理篇的链接资源，这里就不列出来占篇幅了。
然后这里编写了一个函数来调用需要用到的角度数据（至于是pitch还是roll，好像是根据你MPU6050放的位置决定的，我这里用到的是pitch，如果你的平衡倾角是roll，记得平衡角速度也要改变）。具体如下：
/***************************************************************************
函数功能：获取角度，主要是PITCH俯仰角和ROLL横滚角
***************************************************************************/
void Get_Angle()
{
float pitch,roll,yaw; //欧拉角
short aacx,aacy,aacz; //加速度传感器原始数据
short gyrox,gyroy,gyroz; //陀螺仪原始数据
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0) //得到dmp处理后的数据
{
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
Angle_Balance=pitch; //得到平衡倾角
Gyro_Balance=(float)gyroy; //得到平衡角速度
Acceleration_Z=(float)aacz; //得到Z轴加速度计
}
}
运动控制算法实现
直立环和速度环共同作用能实现小车的站立效果（至于转向环，上位机控制方面淘宝店家的资料中没给教程，里面的指令对应的地址值我也很懵逼…）。
直立环PD控制程序实现如下：
/**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回 值：直立控制PWM
**************************************************************************/
int balance(float Angle,float Gyro)
{
float Bias;
int balance;
Bias=Angle-ZHONGZHI; //===求出平衡的角度中值 和机械相关
balance=Balance_Kp*Bias+Gyro*Balance_Kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
return balance;
}
速度环PI控制程序实现如下：
/**************************************************************************
函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity
入口参数：左轮编码器、右轮编码器
返回 值：速度控制PWM
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder,Movement;
static float Encoder_Integral,Target_Velocity;
Target_Velocity=110;
//遥控部分
if(1==Flag_Qian) Movement=-Target_Velocity/Flag_sudu; //===前进标志位置1
else if(1==Flag_Hou) Movement=Target_Velocity/Flag_sudu; //===后退标志位置1
else Movement=0;
Encoder_Least =(Encoder_Left+Encoder_Right)-0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
Encoder *= 0.8; //===一阶低通滤波器
Encoder += Encoder_Least*0.2; //===一阶低通滤波器
Encoder_Integral +=Encoder; //===积分出位移 积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据，控制前进后退
if(Encoder_Integral>10000) Encoder_Integral=10000; //===积分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===积分限幅
Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki; //===速度控制
return Velocity;
}
直立环和速度环最终是在中断服务函数中被调用，做最终处理并输出。这里使用的是外部中断（stm32f103c8t6的四个定时器在电机驱动和编码器模式时已经用完了，为了防止重复使用同一资源带来不确定的影响，这里使用外部中断来对数据进行最终处理），外部中断用到的IO口为PA12，初始化函数如下：
/***************************************************************************************************
**函数功能： 外部中断初始化函数，初始化的IO口为PA12,用作MPU6050的INT管脚时基
**中断线说明：中断线 0-4 每个中断线对应一个中断函数，中断线 5-9 共用中断函数
EXTI9_5_IRQHandler，中断线 10-15 共用中断函数 EXTI15_10_IRQHandler
**中断模式： 中断 EXTI_Mode_Interrupt 和事件 EXTI_Mode_Event两种模式
***************************************************************************************************/
#include "exti.h"
GPIO_InitTypeDef GPIOA_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
void INT_EXTI_Init(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//初始化复用时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//初始化GPIOA时钟
//PA12初始化
GPIOA_InitStructure.GPIO_Pin=GPIO_Pin_12;//PA12
GPIOA_InitStructure.GPIO_Mode=GPIO_Mode_IPU;//上拉输入
GPIOA_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIOA_InitStructure);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource12);//中断线以及中断初始化配置
//EXTI初始化
EXTI_InitStructure.EXTI_Line=EXTI_Line12;//中断线12
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;//中断模式
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Falling;//下降沿触发
EXTI_InitStructure.EXTI_LineCmd=ENABLE;
EXTI_Init(&EXTI_InitStructure);
//NVIC初始化
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn; //使能按键所在的外部中断通道
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02; //抢占优先级2，
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01; //子优先级1
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
NVIC_Init(&NVIC_InitStructure);
}
中断服务函数如下：
int EXTI15_10_IRQHandler(void)
{
if(INT==0)
{
EXTI->PR=1<<12; //清除中断标志位
Flag_Target=!Flag_Target;
if(delay_flag==1)
{
if(++delay_50==10) delay_50=0,delay_flag=0; //给主函数提供50ms的精准延时
}
if(Flag_Target==1) //5ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善卡尔曼滤波和互补滤波的效果
{
Get_Angle(); //===更新姿态
return 0;
}
Encoder_Left=Read_Encoder(3); //读取编码器1前进的值，M法测速，输出为每10ms的脉冲数
Encoder_Right=Read_Encoder(4); //读取编码器2前进的值，M法测速，输出为每10ms的脉冲数
Get_Angle();
Balance_Pwm =balance(Angle_Balance,Gyro_Balance); //===平衡PID控制
Velocity_Pwm=velocity(Encoder_Left,Encoder_Right);
//Turn_Pwm=turn(Encoder_Left,Encoder_Right,Gyro_Turn); //===转向环PID控制
Moto1=Balance_Pwm-Velocity_Pwm;
Moto2=Balance_Pwm-Velocity_Pwm;
Xianfu_Pwm();
Set_Pwm(Moto1,Moto2);
}
return 0;
}
/**************************************************************************
函数功能：限制PWM赋值
入口参数：无
返回 值：无
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=6900; //===PWM满幅是7200 限制在6900
if(Moto1<-Amplitude) Moto1=-Amplitude;
if(Moto1>Amplitude) Moto1=Amplitude;
if(Moto2<-Amplitude) Moto2=-Amplitude;
if(Moto2>Amplitude) Moto2=Amplitude;
}
调试
最后的调试简直让人暴躁，这里是比较早的部分调试记录，后面直立实现过程的调试记录忘记记录了（那时事太多给忘了）…现在尽量回想调试时的问题，毕竟调试的时候问题太多了，而且那时候也不是每天都在弄毕业设计。
****：首次尝试整合所有代码，发现一边电机编码器不起作用，捕获不了脉冲，经查看stm32手册，发现定时器只有高级定时器和通用定时器的通道1和通道2才可以作为编码器模式，因此重新打板，重新分配GPIO口，最终板效果如硬件篇所示。
****：再次整合代码，电机全速转动，编码器没有输出或者输出的数值不对，没有达到预期的效果（要是接近平衡状态，速度作用下小车应该是一个的区间晃动或者向一边倾斜并逐渐加速的效果）。确定相关驱动程序没有错误后，怀疑是赋值给PWM寄存器的时候方向错误或者通道不对，修改后能达到预期效果。
以下就是调参过程中参考的一些有帮助的连接：
PID调参参考1
PID调参参考2
PID调参参考3
PID调参参考4
概括起来就是：调整PID参数的时候先屏蔽速度环，单纯直立PD作用，使小车接近一个平衡状态。但是这个平衡状态容易被外界的干扰如外加力打破，因此单纯的直立环作用并不足以时小车保持一个良好的平衡状态，所以这时候要引进速度环调节。速度环调节是一个正反馈调节，即小车倾角越大，速度越快，小车越容易恢复平衡位状态。在直立环调好后，把速度环加进来，如果你加了速度环但是不管怎么调整参数，施加外力给小车的时候都没能恢复平衡状态，那么就需要检查一下速度环是否是正反馈作用给电机的，在最终PID运算输出PWM值的时候尝试“用减法”，即“Moto1=Balance_Pwm-Velocity_Pwm; ”。
****：捣鼓了几天时间，总算能够让小车平衡起来，并且在施加推力给小车的时候小车能够马上减速并逐步回来原来离开的位置，可惜当时候没有拍视频，打算加上转向环再拍的，结果加上转向环在遥控小车的时候没控制好小车，让它碰了几下墙，小车就傻掉了…我也整个人都傻了。后面检查的时候发现小车底盘没有问题，主控开发板也应该没有问题，估计问题是出在了电机驱动模块或者电路板上，打算更换电机驱动模块，结果网上一搜，这个模块还涨价了，快接近60RMB了（留下了贫穷的泪水…）。还好大体方向走对了，也出来了自己想要的效果，就这样结束吧。。
这是一个以前拍的视频：转B站，能实现直立控制，但是这个版本的平衡小车还没实现速度正反馈，因为相关硬件插拔多几次后，不管怎么调参小车不能平衡了，后来再换了一个小车底盘去排查问题，确实是小车电机出了问题。
总得来说通过这次项目的独立完成，还是能学习到许多东西的。你有想法是好事，但你还可以做得更好，比如去验证它。当初我看到网上有这么多相关参考和优秀的成品，也以为做平衡小车是很容易的，结果实际去完成的时候才发现里面的困难是很多的。调试过程中某一块功能没能达到你的预期，就得从原理、硬件、软件这些方面一一去分析，最终判断问题所在，并验证自己的判断，从而解决问题。
到这里“基于stm32的平衡小车设计”项目就告一段落了，从模块选型到最终的效果实现，已经基本整理完毕，由于时间跨度比较大和个人记录习惯不好，导致有些地方没能清楚展示出来，加上博主水平有限，有些地方可能存在描述上或者理解上的不足，如果各位大神有发现不足的地方，欢迎在评论区留言指出来。