一、设计思路
????????使用stm32f103zet6作为主控芯片；使用0.96OLED显示当前两个轮子速度及俯仰角度，模块初始化阶段显示各个模块初始化进度；使用mpu6050获取当前位置的俯仰角度；使用L298N通过PWM控制电机速度；使用霍尔编码器测出角速度，进一步计算出实际速度；使用降压模块与12V电源给各个模块和主控芯片供电。使用位置式PID与增量式PID串联控制平衡小车静止直立。
二、模块选取
1.stm32f103zet6
?
图1.stm32f103zet6
2.0.96OLED
?
图2.0.96OLED
3.mpu6050
?
图3.mpu6050
4.L298N
?
图4.L298N
5.霍尔编码器减速电机
?
图5.霍尔编码器减速电机
6.降压模块
?
图6.降压模块
7.12V电源
?
图7.12V电源
三、硬件搭建
图8.硬件搭建1
图9.硬件搭建2
? ? ? ? 如图8、图9所示，mpu6050与0.96oled插在最上方的面包板上，主控芯片固定在面包板下方，接着是降压模块和L298N固定在主控芯片下方，降压模块下12V电源，带有编码盘的直流减速电机对称分布在洞洞板两侧，分别装上轮胎。
四、驱动编写
引脚接口：
TIM2__PA15 && PB3__left ——>PD4,5
TIM4__PD12 && PD13__right ——>PD6,7MPU6050 I2C
PB10:IIC_SCL，PB11:IIC_SDAOLED 4线SPI
D0：PA5（SCL），D1：PA7（SDA），RES ：PB0 ，DC：PB1，CS：PA4
1.OLED
????????
#include "oled.h"
#include "stdlib.h"
#include "oledfont.h"
#include "delay.h"
u8 OLED_GRAM[144][8];
//反显函数
void OLED_ColorTurn(u8 i)
{
if(i==0)
{
OLED_WR_Byte(0xA6,OLED_CMD);//正常显示
}
if(i==1)
{
OLED_WR_Byte(0xA7,OLED_CMD);//反色显示
}
}
//屏幕旋转180度
void OLED_DisplayTurn(u8 i)
{
if(i==0)
{
OLED_WR_Byte(0xC8,OLED_CMD);//正常显示
OLED_WR_Byte(0xA1,OLED_CMD);
}
if(i==1)
{
OLED_WR_Byte(0xC0,OLED_CMD);//反转显示
OLED_WR_Byte(0xA0,OLED_CMD);
}
}
void OLED_WR_Byte(u8 dat,u8 cmd)
{
u8 i;
if(cmd)
OLED_DC_Set();
else
OLED_DC_Clr();
OLED_CS_Clr();
for(i=0;i<8;i++)
{
OLED_SCLK_Clr();
if(dat&0x80)
OLED_SDIN_Set();
else
OLED_SDIN_Clr();
OLED_SCLK_Set();
dat<<=1;
}
OLED_CS_Set();
OLED_DC_Set();
}
//开启OLED显示
void OLED_DisPlay_On(void)
{
OLED_WR_Byte(0x8D,OLED_CMD);//电荷泵使能
OLED_WR_Byte(0x14,OLED_CMD);//开启电荷泵
OLED_WR_Byte(0xAF,OLED_CMD);//点亮屏幕
}
//关闭OLED显示
void OLED_DisPlay_Off(void)
{
OLED_WR_Byte(0x8D,OLED_CMD);//电荷泵使能
OLED_WR_Byte(0x10,OLED_CMD);//关闭电荷泵
OLED_WR_Byte(0xAF,OLED_CMD);//关闭屏幕
}
//更新显存到OLED
void OLED_Refresh(void)
{
u8 i,n;
for(i=0;i<8;i++)
{
OLED_WR_Byte(0xb0+i,OLED_CMD); //设置行起始地址
OLED_WR_Byte(0x00,OLED_CMD); //设置低列起始地址
OLED_WR_Byte(0x10,OLED_CMD); //设置高列起始地址
for(n=0;n<128;n++)
OLED_WR_Byte(OLED_GRAM[n][i],OLED_DATA);
}
}
//清屏函数
void OLED_Clear(void)
{
u8 i,n;
for(i=0;i<8;i++)
{
for(n=0;n<128;n++)
{
OLED_GRAM[n][i]=0;//清除所有数据
}
}
OLED_Refresh();//更新显示
}
//画点
//x:0~127
//y:0~63
void OLED_DrawPoint(u8 x,u8 y)
{
u8 i,m,n;
i=y/8;
m=y%8;
n=1<<m;
OLED_GRAM[x][i]|=n;
}
//清除一个点
//x:0~127
//y:0~63
void OLED_ClearPoint(u8 x,u8 y)
{
u8 i,m,n;
i=y/8;
m=y%8;
n=1<<m;
OLED_GRAM[x][i]=~OLED_GRAM[x][i];
OLED_GRAM[x][i]|=n;
OLED_GRAM[x][i]=~OLED_GRAM[x][i];
}
//画线
//x:0~128
//y:0~64
void OLED_DrawLine(u8 x1,u8 y1,u8 x2,u8 y2)
{
u8 i,k,k1,k2,y0;
if((x1<0)||(x2>128)||(y1<0)||(y2>64)||(x1>x2)||(y1>y2))return;
if(x1==x2) //画竖线
{
for(i=0;i<(y2-y1);i++)
{
OLED_DrawPoint(x1,y1+i);
}
}
else if(y1==y2) //画横线
{
for(i=0;i<(x2-x1);i++)
{
OLED_DrawPoint(x1+i,y1);
}
}
else //画斜线
{
k1=y2-y1;
k2=x2-x1;
k=k1*10/k2;
for(i=0;i<(x2-x1);i++)
{
OLED_DrawPoint(x1+i,y1+i*k/10);
}
}
}
//x,y:圆心坐标
//r:圆的半径
void OLED_DrawCircle(u8 x,u8 y,u8 r)
{
int a, b,num;
a = 0;
b = r;
while(2 * b * b >= r * r)
{
OLED_DrawPoint(x + a, y - b);
OLED_DrawPoint(x - a, y - b);
OLED_DrawPoint(x - a, y + b);
OLED_DrawPoint(x + a, y + b);
OLED_DrawPoint(x + b, y + a);
OLED_DrawPoint(x + b, y - a);
OLED_DrawPoint(x - b, y - a);
OLED_DrawPoint(x - b, y + a);
a++;
num = (a * a + b * b) - r*r;//计算画的点离圆心的距离
if(num > 0)
{
b--;
a--;
}
}
}
//在指定位置显示一个字符,包括部分字符
//x:0~127
//y:0~63
//size:选择字体 12/16/24
//取模方式 逐列式
void OLED_ShowChar(u8 x,u8 y,u8 chr,u8 size1)
{
u8 i,m,temp,size2,chr1;
u8 y0=y;
size2=(size1/8+((size1%8)?1:0))*(size1/2); //得到字体一个字符对应点阵集所占的字节数
chr1=chr-' '; //计算偏移后的值
for(i=0;i<size2;i++)
{
if(size1==12)
{temp=asc2_1206[chr1][i];} //调用1206字体
else if(size1==16)
{temp=asc2_1608[chr1][i];} //调用1608字体
else if(size1==24)
{temp=asc2_2412[chr1][i];} //调用2412字体
else return;
for(m=0;m<8;m++) //写入数据
{
if(temp&0x80)OLED_DrawPoint(x,y);
else OLED_ClearPoint(x,y);
temp<<=1;
y++;
if((y-y0)==size1)
{
y=y0;
x++;
break;
}
}
}
}
//显示字符串
//x,y:起点坐标
//size1:字体大小
//*chr:字符串起始地址
void OLED_ShowString(u8 x,u8 y,u8 *chr,u8 size1)
{
while((*chr>=' ')&&(*chr<='~'))//判断是不是非法字符!
{
OLED_ShowChar(x,y,*chr,size1);
x+=size1/2;
if(x>128-size1) //换行
{
x=0;
y+=2;
}
chr++;
}
}
//m^n
u32 OLED_Pow(u8 m,u8 n)
{
u32 result=1;
while(n--)
{
result*=m;
}
return result;
}
显示2个数字
x,y :起点坐标
len :数字的位数
size:字体大小
void OLED_ShowNum(u8 x,u8 y,u32 num,u8 len,u8 size1)
{
u8 t,temp;
for(t=0;t<len;t++)
{
temp=(num/OLED_Pow(10,len-t-1))%10;
if(temp==0)
{
OLED_ShowChar(x+(size1/2)*t,y,'0',size1);
}
else
{
OLED_ShowChar(x+(size1/2)*t,y,temp+'0',size1);
}
}
}
//显示汉字
//x,y:起点坐标
//num:汉字对应的序号
//取模方式 列行式
void OLED_ShowChinese(u8 x,u8 y,u8 num,u8 size1)
{
u8 i,m,n=0,temp,chr1;
u8 x0=x,y0=y;
u8 size3=size1/8;
while(size3--)
{
chr1=num*size1/8+n;
n++;
for(i=0;i<size1;i++)
{
if(size1==16)
{temp=Hzk1[chr1][i];}//调用16*16字体
else if(size1==24)
{temp=Hzk2[chr1][i];}//调用24*24字体
else if(size1==32)
{temp=Hzk3[chr1][i];}//调用32*32字体
else if(size1==64)
{temp=Hzk4[chr1][i];}//调用64*64字体
else return;
for(m=0;m<8;m++)
{
if(temp&0x01)OLED_DrawPoint(x,y);
else OLED_ClearPoint(x,y);
temp>>=1;
y++;
}
x++;
if((x-x0)==size1)
{x=x0;y0=y0+8;}
y=y0;
}
}
}
//num 显示汉字的个数
//space 每一遍显示的间隔
void OLED_ScrollDisplay(u8 num,u8 space)
{
u8 i,n,t=0,m=0,r;
while(1)
{
if(m==0)
{
OLED_ShowChinese(128,24,t,16); //写入一个汉字保存在OLED_GRAM[][]数组中
t++;
}
if(t==num)
{
for(r=0;r<16*space;r++) //显示间隔
{
for(i=0;i<144;i++)
{
for(n=0;n<8;n++)
{
OLED_GRAM[i-1][n]=OLED_GRAM[i][n];
}
}
OLED_Refresh();
}
t=0;
}
m++;
if(m==16){m=0;}
for(i=0;i<144;i++) //实现左移
{
for(n=0;n<8;n++)
{
OLED_GRAM[i-1][n]=OLED_GRAM[i][n];
}
}
OLED_Refresh();
}
}
//配置写入数据的起始位置
void OLED_WR_BP(u8 x,u8 y)
{
OLED_WR_Byte(0xb0+y,OLED_CMD);//设置行起始地址
OLED_WR_Byte(((x&0xf0)>>4)|0x10,OLED_CMD);
OLED_WR_Byte((x&0x0f),OLED_CMD);
}
//x0,y0：起点坐标
//x1,y1：终点坐标
//BMP[]：要写入的图片数组
void OLED_ShowPicture(u8 x0,u8 y0,u8 x1,u8 y1,u8 BMP[])
{
u32 j=0;
u8 x=0,y=0;
if(y%8==0)y=0;
else y+=1;
for(y=y0;y<y1;y++)
{
OLED_WR_BP(x0,y);
for(x=x0;x<x1;x++)
{
OLED_WR_Byte(BMP[j],OLED_DATA);
j++;
}
}
}
//OLED的初始化
void OLED_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能A端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//速度50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化GPIOD3,6
GPIO_SetBits(GPIOA,GPIO_Pin_5|GPIO_Pin_7|GPIO_Pin_4);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能A端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_8;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//速度50MHz
GPIO_Init(GPIOB, &GPIO_InitStructure); //初始化GPIOD3,6
GPIO_SetBits(GPIOB,GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_8);
OLED_RST_Set();
delay_ms(100);
OLED_RST_Clr();//复位
delay_ms(200);
OLED_RST_Set();
OLED_WR_Byte(0xAE,OLED_CMD);//--turn off oled panel
OLED_WR_Byte(0x00,OLED_CMD);//---set low column address
OLED_WR_Byte(0x10,OLED_CMD);//---set high column address
OLED_WR_Byte(0x40,OLED_CMD);//--set start line address Set Mapping RAM Display Start Line (0x00~0x3F)
OLED_WR_Byte(0x81,OLED_CMD);//--set contrast control register
OLED_WR_Byte(0xCF,OLED_CMD);// Set SEG Output Current Brightness
OLED_WR_Byte(0xA1,OLED_CMD);//--Set SEG/Column Mapping 0xa0左右反置 0xa1正常
OLED_WR_Byte(0xC8,OLED_CMD);//Set COM/Row Scan Direction 0xc0上下反置 0xc8正常
OLED_WR_Byte(0xA6,OLED_CMD);//--set normal display
OLED_WR_Byte(0xA8,OLED_CMD);//--set multiplex ratio(1 to 64)
OLED_WR_Byte(0x3f,OLED_CMD);//--1/64 duty
OLED_WR_Byte(0xD3,OLED_CMD);//-set display offset Shift Mapping RAM Counter (0x00~0x3F)
OLED_WR_Byte(0x00,OLED_CMD);//-not offset
OLED_WR_Byte(0xd5,OLED_CMD);//--set display clock divide ratio/oscillator frequency
OLED_WR_Byte(0x80,OLED_CMD);//--set divide ratio, Set Clock as 100 Frames/Sec
OLED_WR_Byte(0xD9,OLED_CMD);//--set pre-charge period
OLED_WR_Byte(0xF1,OLED_CMD);//Set Pre-Charge as 15 Clocks & Discharge as 1 Clock
OLED_WR_Byte(0xDA,OLED_CMD);//--set com pins hardware configuration
OLED_WR_Byte(0x12,OLED_CMD);
OLED_WR_Byte(0xDB,OLED_CMD);//--set vcomh
OLED_WR_Byte(0x40,OLED_CMD);//Set VCOM Deselect Level
OLED_WR_Byte(0x20,OLED_CMD);//-Set Page Addressing Mode (0x00/0x01/0x02)
OLED_WR_Byte(0x02,OLED_CMD);//
OLED_WR_Byte(0x8D,OLED_CMD);//--set Charge Pump enable/disable
OLED_WR_Byte(0x14,OLED_CMD);//--set(0x10) disable
OLED_WR_Byte(0xA4,OLED_CMD);// Disable Entire Display On (0xa4/0xa5)
OLED_WR_Byte(0xA6,OLED_CMD);// Disable Inverse Display On (0xa6/a7)
OLED_WR_Byte(0xAF,OLED_CMD);
OLED_Clear();
}2.MPU6050
#include "mpu6050.h"
#include "sys.h"
#include "delay.h"
#include "usart.h"
/**********************************************
函数名称：MPU_Init
函数功能：初始化MPU6050
函数参数：无
函数返回值：0,初始化成功 其他,初始化失败
**********************************************/
u8 MPU_Init(void)
{
u8 res;
MPU_IIC_Init(); //初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(200); //设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR) //器件ID正确,即res = MPU_ADDR = 0x68
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(500); //设置采样率为50Hz
}else return 1; //地址设置错误,返回1
return 0; //地址设置正确,返回0
}
/**********************************************
函数名称：MPU_Set_Gyro_Fsr
函数功能：设置MPU6050陀螺仪传感器满量程范围
函数参数：fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
函数返回值：0,设置成功 其他,设置失败
**********************************************/
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3); //设置陀螺仪满量程范围
}
/**********************************************
函数名称：MPU_Set_Accel_Fsr
函数功能：设置MPU6050加速度传感器满量程范围
函数参数：fsr:0,±2g;1,±4g;2,±8g;3,±16g
函数返回值：0,设置成功 其他,设置失败
**********************************************/
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3); //设置加速度传感器满量程范围
}
/**********************************************
函数名称：MPU_Set_LPF
函数功能：设置MPU6050的数字低通滤波器
函数参数：lpf:数字低通滤波频率(Hz)
函数返回值：0,设置成功 其他,设置失败
**********************************************/
u8 MPU_Set_LPF(u16 lpf)
{
u8 data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}
/**********************************************
函数名称：MPU_Set_Rate
函数功能：设置MPU6050的采样率(假定Fs=1KHz)
函数参数：rate:4~1000(Hz) 初始化中rate取50
函数返回值：0,设置成功 其他,设置失败
**********************************************/
u8 MPU_Set_Rate(u16 rate)
{
u8 data;
if(rate>1000)rate=1000;
if(rate<4)rate=4;
data=1000/rate-1;
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
}
/**********************************************
函数名称：MPU_Get_Temperature
函数功能：得到温度传感器值
函数参数：无
函数返回值：温度值(扩大了100倍)
**********************************************/
short MPU_Get_Temperature(void)
{
u8 buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((u16)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;
}
/**********************************************
函数名称：MPU_Get_Gyroscope
函数功能：得到陀螺仪值(原始值)
函数参数：gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
函数返回值：0,读取成功 其他,读取失败
**********************************************/
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((u16)buf[0]<<8)|buf[1];
*gy=((u16)buf[2]<<8)|buf[3];
*gz=((u16)buf[4]<<8)|buf[5];
}
return res;
}
/**********************************************
函数名称：MPU_Get_Accelerometer
函数功能：得到加速度值(原始值)
函数参数：ax,ay,az:加速度传感器x,y,z轴的原始读数(带符号)
函数返回值：0,读取成功 其他,读取失败
**********************************************/
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((u16)buf[0]<<8)|buf[1];
*ay=((u16)buf[2]<<8)|buf[3];
*az=((u16)buf[4]<<8)|buf[5];
}
return res;
}
/**********************************************
函数名称：MPU_Write_Len
函数功能：IIC连续写(写器件地址、寄存器地址、数据)
函数参数：addr:器件地址 reg:寄存器地址
len:写入数据的长度 buf:数据区
函数返回值：0,写入成功 其他,写入失败
**********************************************/
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令(0为写,1为读)
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
for(i=0;i<len;i++)
{
MPU_IIC_Send_Byte(buf[i]); //发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_Stop();
return 0;
}
/**********************************************
函数名称：MPU_Read_Len
函数功能：IIC连续读(写入器件地址后,读寄存器地址、数据)
函数参数：addr:器件地址 reg:要读的寄存器地址
len:要读取的数据长度 buf:读取到的数据存储区
函数返回值：0,读取成功 其他,读取失败
**********************************************/
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|1); //发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
while(len)
{
if(len==1) *buf=MPU_IIC_Read_Byte(0); //读数据,发送nACK
else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK
len--;
buf++;
}
MPU_IIC_Stop(); //产生一个停止条件
return 0;
}
/**********************************************
函数名称：MPU_Write_Byte
函数功能：IIC写一个字节
函数参数：data:写入的数据 reg:要写的寄存器地址
函数返回值：0,写入成功 其他,写入失败
**********************************************/
u8 MPU_Write_Byte(u8 reg,u8 data)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(data); //发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Stop();
return 0;
}
/**********************************************
函数名称：MPU_Read_Byte
函数功能：IIC读一个字节
函数参数：reg:要读的寄存器地址
函数返回值：res:读取到的数据
**********************************************/
u8 MPU_Read_Byte(u8 reg)
{
u8 res;
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0); //发送器件地址+写命令
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1); //发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
res=MPU_IIC_Read_Byte(0); //读取数据,发送nACK
MPU_IIC_Stop(); //产生一个停止条件
return res;
}
#include "mpuiic.h"
#include "delay.h"
/**********************************************
函数名称：MPU_IIC_Delay
函数功能：MPU IIC延时函数，延时2ms
函数参数：无
函数返回值：无
**********************************************/
void MPU_IIC_Delay(void)
{
delay_us(2);
}
/**********************************************
函数名称：MPU_IIC_Init
函数功能：MPU 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 输出高
}
/**********************************************
函数名称：MPU_IIC_Start
函数功能：MPU 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:当SCL线处于高电平时,SDA线突然从高变低,发送起始信号
MPU_IIC_Delay();
MPU_IIC_SCL=0; //钳住I2C总线，准备发送或接收数据
}
/**********************************************
函数名称：MPU_IIC_Stop
函数功能：MPU IIC发送停止信号
函数参数：无
函数返回值：无
**********************************************/
void MPU_IIC_Stop(void)
{
MPU_SDA_OUT(); //SDA线输出
MPU_IIC_SCL=0;
MPU_IIC_SDA=0; //STOP:当SCL线处于高电平时,SDA线突然从低变高,发送停止信号
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_SDA=1; //发送I2C总线结束信号
MPU_IIC_Delay();
}
/**********************************************
函数名称：MPU_IIC_Wait_Ack
函数功能：MPU IIC等待信号到来
函数参数：无
函数返回值：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;
}
/**********************************************
函数名称：MPU_IIC_Ack
函数功能：MPU IIC产生应答信号
函数参数：无
函数返回值：无
**********************************************/
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;
}
/**********************************************
函数名称：MPU_IIC_NAck
函数功能：MPU IIC不产生应答信号
函数参数：无
函数返回值：无
**********************************************/
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;
}
/**********************************************
函数名称：MPU_IIC_Send_Byte
函数功能：MPU IIC发送一个字节
函数参数：txd：要发送的数据
函数返回值：无
注意：IIC发送字节是一个一个位发送的，发送一个字节需要发送八次
**********************************************/
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();
}
}
/**********************************************
函数名称：MPU_IIC_Read_Byte
函数功能：MPU IIC读取一个字节
函数参数：ack: 1,发送ACK 0,发送NACK
函数返回值：接收到的数据
注意：IIC读取字节是一个一个位读取的，读取一个字节需要读取八次
**********************************************/
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;
}
//
//添加的代码部分
//
//本程序只供学习使用，未经作者许可，不得用于其它任何用途
//ALIENTEK精英STM32开发板V3
//MPU6050 DMP 驱动代码
//正点原子@ALIENTEK
//技术论坛:www.openedv.com
//创建日期:2015/1/17
//版本：V1.0
//版权所有，盗版必究。
//Copyright(C) 广州市星翼电子科技有限公司 2009-2019
//All rights reserved
//
//q30格式,long转float时的除数.
#define q30 1073741824.0f
//陀螺仪方向设置
static signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1};
//MPU6050自测试
//返回值:0,正常
// 其他,失败
u8 run_self_test(void)
{
int result;
//char test_packet[4] = {0};
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x3)
{
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel_sens = 0; //设置为基础值
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
return 0;
}else return 1;
}
//陀螺仪方向控制
unsigned short inv_orientation_matrix_to_scalar(
const signed char *mtx)
{
unsigned short scalar;
/*
XYZ 010_001_000 Identity Matrix
XZY 001_010_000
YXZ 010_000_001
YZX 000_010_001
ZXY 001_000_010
ZYX 000_001_010
*/
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
//方向转换
unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b;
if (row[0] > 0)
b = 0;
else if (row[0] < 0)
b = 4;
else if (row[1] > 0)
b = 1;
else if (row[1] < 0)
b = 5;
else if (row[2] > 0)
b = 2;
else if (row[2] < 0)
b = 6;
else
b = 7; // error
return b;
}
//空函数,未用到.
void mget_ms(unsigned long *time)
{
}
//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_init(void)
{
u8 res=0;
MPU_IIC_Init(); //初始化IIC总线
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=run_self_test(); //自检
if(res)return 8;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
}else return 10;
return 0;
}
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}else return 2;
return 0;
}/**
* @brief Get one packet from the FIFO.
* If @e sensors does not contain a particular sensor, disregard the data
* returned to that pointer.
* \n @e sensors can contain a combination of the following flags:
* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
* \n INV_XYZ_GYRO
* \n INV_XYZ_ACCEL
* \n INV_WXYZ_QUAT
* \n If the FIFO has no new data, @e sensors will be zero.
* \n If the FIFO is disabled, @e sensors will be zero and this function will
* return a non-zero error code.
* @param[out] gyro Gyro data in hardware units.
* @param[out] accel Accel data in hardware units.
* @param[out] quat 3-axis quaternion data in hardware units.
* @param[out] timestamp Timestamp in milliseconds.
* @param[out] sensors Mask of sensors read from FIFO.
* @param[out] more Number of remaining packets.
* @return 0 if successful.
*/
int dmp_read_fifo(short *gyro, short *accel, long *quat,
unsigned long *timestamp, short *sensors, unsigned char *more)
{
unsigned char fifo_data[MAX_PACKET_LENGTH];
unsigned char ii = 0;
/* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
* cache this value and save some cycles.
*/
sensors[0] = 0;
/* Get a packet. */
if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))
return -1;
/* Parse DMP packet. */
if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
#ifdef FIFO_CORRUPTION_CHECK
long quat_q14[4], quat_mag_sq;
#endif
quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
((long)fifo_data[2] << 8) | fifo_data[3];
quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
((long)fifo_data[6] << 8) | fifo_data[7];
quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
((long)fifo_data[10] << 8) | fifo_data[11];
quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
((long)fifo_data[14] << 8) | fifo_data[15];
ii += 16;
#ifdef FIFO_CORRUPTION_CHECK
/* We can detect a corrupted FIFO by monitoring the quaternion data and
* ensuring that the magnitude is always normalized to one. This
* shouldn't happen in normal operation, but if an I2C error occurs,
* the FIFO reads might become misaligned.
*
* Let's start by scaling down the quaternion data to avoid long long
* math.
*/
quat_q14[0] = quat[0] >> 16;
quat_q14[1] = quat[1] >> 16;
quat_q14[2] = quat[2] >> 16;
quat_q14[3] = quat[3] >> 16;
quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
(quat_mag_sq > QUAT_MAG_SQ_MAX)) {
/* Quaternion is outside of the acceptable threshold. */
mpu_reset_fifo();
sensors[0] = 0;
return -1;
}
sensors[0] |= INV_WXYZ_QUAT;
#endif
}
if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
ii += 6;
sensors[0] |= INV_XYZ_ACCEL;
}
if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
ii += 6;
sensors[0] |= INV_XYZ_GYRO;
}
/* Gesture data is at the end of the DMP packet. Parse it and call
* the gesture callbacks (if registered).
*/
if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
decode_gesture(fifo_data + ii);
get_ms(timestamp);
return 0;
}
/**
* @brief Register a function to be executed on a tap event.
* The tap direction is represented by one of the following:
* \n TAP_X_UP
* \n TAP_X_DOWN
* \n TAP_Y_UP
* \n TAP_Y_DOWN
* \n TAP_Z_UP
* \n TAP_Z_DOWN
* @param[in] func Callback function.
* @return 0 if successful.
*/
int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
{
dmp.tap_cb = func;
return 0;
}
/**
* @brief Register a function to be executed on a android orientation event.
* @param[in] func Callback function.
* @return 0 if successful.
*/
int dmp_register_android_orient_cb(void (*func)(unsigned char))
{
dmp.android_orient_cb = func;
return 0;
}
/**
* @}
*/
3.霍尔编码器
#include "encoder.h"
#include "stm32f10x_gpio.h"
/****************************************************************
函数功能：把TIM2初始化为编码器接口模式 PA15, PB3
入口参数：无
返回值：无
****************************************************************/
void Encoder_Init_TIM2(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
//使能定时器4的时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
//使能PA, PB端口时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB, ENABLE);
//部分重映射开启AFIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
GPIO_PinRemapConfig(GPIO_PartialRemap1_TIM2, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable , ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化GPIOA
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOB, &GPIO_InitStructure); //初始化GPIOB
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x0; //预分频器
//设定计数器自动重载值
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD;
//选择时钟分频：不分频
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
//TIM向上计数
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12,
TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10;
TIM_ICInit(TIM2, &TIM_ICInitStructure);
TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
//Reset counter
TIM_SetCounter(TIM2,0);
TIM_Cmd(TIM2, ENABLE);
}
/****************************************************************
函数功能：把TIM4初始化为编码器接口模式 PD12, PD13
入口参数：无
返回值：无
****************************************************************/
void Encoder_Init_TIM4(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
//使能定时器4的时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
//使能PD端口时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
//部分重映射开启AFIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_TIM4, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable , ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOD, &GPIO_InitStructure); //初始化GPIOD
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x0; //预分频器
//设定计数器自动重载值
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD;
//选择时钟分频：不分频
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
//TIM向上计数
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,
TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10;
TIM_ICInit(TIM4, &TIM_ICInitStructure);
TIM_ClearFlag(TIM4, TIM_FLAG_Update);//清除TIM的更新标志位
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
TIM_SetCounter(TIM4,0);//Reset counter
TIM_Cmd(TIM4, ENABLE);
}
/*****************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回值：速度值
*****************************************************************/
int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 2: Encoder_TIM= (short)TIM2 -> CNT;
TIM2 -> CNT=0;
break;
case 4: Encoder_TIM= (short)TIM4 -> CNT;
TIM4 -> CNT=0;
break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}
4.电机控制
#include "motor.h"
void TIM3_GPIO_Config(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
//TIM1 Full remapping pins
GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE);
//TIM3 CH1,2重映射PC6,7
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; // 复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
}
void TIM3_Mode_Config(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
/* -----------------------------------------------------------------------
TIMx Channel1 duty cycle = (TIMx_CCR1/ TIMx_ARR+1)* 100% = 50%
----------------------------------------------------------------------- */
//PWM频率f=TIMxCLK/{(TIM_Period+1)*(TIM_Prescaler+1)}=72M/{(999+1)*(71+1)}=1KHZ;
//Time base configuration
TIM_TimeBaseStructure.TIM_Period = 999; //当定时器从0计数到999，即为1000次，为一个定时周期
TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置预分频：不预分频，即为72MHz
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1 ; //设置时钟分频系数：不分频(这里用不到)
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
//PWM1 Mode configuration: Channel1
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //配置为PWM模式1
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //当定时器计数值小于CCR1_Val时为高电平
TIM_OC1Init(TIM3, &TIM_OCInitStructure); //使能通道1
TIM_OC2Init(TIM3, &TIM_OCInitStructure); //使能通道2
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
//TIM3 enable counter
TIM_Cmd(TIM3, ENABLE); //使能定时器3
}
void GPIO_Mode_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_ResetBits(GPIOD, GPIO_Pin_4);
GPIO_ResetBits(GPIOD, GPIO_Pin_5);
GPIO_ResetBits(GPIOD, GPIO_Pin_6);
GPIO_ResetBits(GPIOD, GPIO_Pin_7);
}
void TIM_PWM_Init(void)
{
TIM3_GPIO_Config();
TIM3_Mode_Config();
GPIO_Mode_Init();
}
五、PID算法
1.简介
????????所谓 PID
控制，就是对系统偏差进行比例、积分以及微分的控制。PID 是闭环控制，因此需要有传感器测量我们需要控制的参数，并且反馈到我们的控制计算当中，并且参与控制。PID 由 3 个单元组成，分别是比例（Proportion）单元、积分（Integral） 单元、微分（Differential）单元。通过对这三个单元的处理计算输出给执行器，达到减小偏差最终实现收敛的过程。
2.位置式PID
????????位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
float Position_PID (float Encoder,float Target)
{
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Target- Encoder; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);
Last_Bias=Bias; //保存上一次偏差
// printf("%f\r\n",Pwm);
if (Pwm >= 999) Pwm = 999;
else if (Pwm <= -999) Pwm = -999;
if (Pwm >= 0) return 999 - Pwm;
else return -999 - Pwm;
}
????????入口参数为编码器的位置测量值和位置控制的目标值，返回值为电机控制PWM(现在再看一下上面的控制框图是不是更加容易明白了)。
????????第一行是相关内部变量的定义。
????????第二行是求出位置偏差，由测量值减去目标值。
????????第三行通过累加求出偏差的积分。
????????第四行使用位置式 PID 控制器求出电机 PWM。
????????第五行保存上一次偏差，便于下次调用。
????????最后一行是返回。
3.增量式PID
????????速度闭环控制就是根据单位时间获取的脉冲数（这里使用了 M
法测速）测量电机的速度信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
float Incremental_PI (int Encoder,int Target)
{ static float Bias,Pwm,Last_bias;
Bias=Encoder-Target; //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式 PI 控制器
Last_bias=Bias; //保存上一次偏差
// printf("%f\r\n",Pwm);
return Pwm; //增量输出
}
????????入口参数为编码器的速度测量值和速度控制的目标值，返回值为电机控制 PWM。
????????第一行是相关内部变量的定义。
????????第二行是求出速度偏差，由测量值减去目标值。
????????第三行使用增量 PI 控制器求出电机 PWM。
????????第四行保存上一次偏差，便于下次调用。
????????最后一行是返回。
4.串级PID
????????所谓串级 PID 就是使用两个 PID 控制器进行串联工作，外环控制器的输出作为内环控制器的输入，由内环控制器的输出去控制执行器。
float Position_KP = 0.1;
float Position_KI = 0;
float Position_KD = 5;
float Velocity_KP = 0;
float Velocity_KI = 0;
float Position_PID (float Encoder,float Target) {
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Target- Encoder; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);
Last_Bias=Bias; //保存上一次偏差
// printf("%f\r\n",Pwm);
if (Pwm >= 999) Pwm = 999;
else if (Pwm <= -999) Pwm = -999;
if (Pwm >= 0) return 999 - Pwm;
else return -999 - Pwm;
}
float Incremental_PI (int Encoder,int Target)
{ static float Bias,Pwm,Last_bias;
Bias=Encoder-Target; //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式 PI 控制器
Last_bias=Bias; //保存上一次偏差
// printf("%f\r\n",Pwm);
return Pwm; //增量输出
}六、心得体会
? ? ? ? 在编码器驱动编写的时候遇到TIM2和TIM4通道1、2引脚占用问题，当使用通道3、4时编码器无法正常工作，在将TIM2和TIM4的通道完全重映射后正常工作。我们使用的这个mpu6050对输入供电要求非常严格，必须是3.3V否则就会初始化不成功，这点导致后面调试PID参数的时候非常耗费时间，可以通过购买某原子的mpu6050解决，就是贵一些，推荐这种解决方案。
? ? ? ? 本来以为stm32学的差不多了，常用的都知道了，结果拿到编码器的时候去查阅资料，一直在讲定时器的编码器接口功能，才发现定时器还有专门为编码器准备的一个功能，很震惊，所以在以后的学习中要认真仔细一点。