本篇是軟件調試篇，接上一篇硬件篇：基於stm32的兩輪自平衡小車3（硬件篇），本篇內容是對硬件部分的軟件實現，具體模塊詳見目錄。
目錄
- 定時器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_CNT<TIMx_CCR1時通道1爲有效電平，否則爲無效電平；
在向下計數時，一旦TIMx_CNT>TIMx_CCR1時通道1爲無效電平(OC1REF=0)，否
則爲有效電平(OC1REF=1)
PWM模式2：向上計數時，一旦TIMx_CNT<TIMx_CCR1時通道1爲無效電平，否則爲有效電平；
在向下計數時，一旦TIMx_CNT>TIMx_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的平衡小車設計”項目就告一段落了，從模塊選型到最終的效果實現，已經基本整理完畢，由於時間跨度比較大和個人記錄習慣不好，導致有些地方沒能清楚展示出來，加上博主水平有限，有些地方可能存在描述上或者理解上的不足，如果各位大神有發現不足的地方，歡迎在評論區留言指出來。