基于STM32单片机的双轮小车近静态动平衡控制统

、主程序#include"stm32f10x.h"#include"bsp_SysTick.h"#include"bsp_usart1.h"#include"bsp_mpu6050.h"#include"bsp_key.h"#include"bsp_pwm.h"#include"bsp_motor.h"#include"bsp_12864.h"//#include"math.h"voidDelay(__IOuint32_tnCount);Void Kalman_Filter(floatAccel,floatGyro); voidPWM_Calcu(void);voidAngle_Calcu(void);//角度参数floatGyro_y;//Y轴陀螺仪数据暂存floatAngle_gy; //由角速度计算得的倾斜角度floatAccel_x; //X轴加速度暂存floatAngle_ax;//由加速度计算的倾斜角度floatAngle;//小车最终的倾斜角度uint8_tvalue; //角度正负极性标记//PWM参数intPWM_R;//前进的PWM参数intPWM_L;//后退的PWM参数floatPWM;//综合PWM参数//PID参数staticfloatKp=205;//185staticfloatKd=22; //10//******卡尔曼参数************ floatQ_angle=0.001;floatQ_gyro=0.003;floatR_angle=0.5;floatdt=0.01; //dt为kalman滤波器采样时间;charC_0=1;floatQ_bias,Angle_err;floatPCt_0,PCt_1,E;floatK_0,K_1,t_0,t_1;floatPdot[4]={0,0,0,0};floatPP[2][2]={{1,0},{0,1}};intmain(void){ SysTick_Init(); USART1_Config(); Init_MPU6050();; TIM3_PWM_Init(); LCD_12864_Init(); while(1) { LCD_WriteStr(1,1,"陈国文"); LCD_WriteStr(2,1,"ACCEL:");LCD_WriteStr(3,1,"GYRO:"); LCD_WriteNum4(2,4,Angle) ; LCD_WriteNum4(3,4,Gyro_y) ; }}voidPWM_Calcu(void) { if(Angle<-30||Angle>30) { TIM3->CCR3=0;TIM3->CCR4=0; return; } PWM=(int)(Kp*(Angle-1)+Kd*Gyro_y); PWM_R=PWM; PWM_L=PWM*11/9; PWM_Motor(PWM_R,PWM_L);}voidAngle_Calcu(void) { //加速度 //范围为2g时，换算关系：16384LSB/g //角度较小时，x=sinx得到角度（弧度）,deg=rad*180/3.14 //因为x>=sinx,故乘以1.3适当放大// Accel_x=GetData(ACCEL_XOUT_H); //读取X轴加速度// Angle_ax=(Accel_x-570)/16384;//去除零点偏移,计算得到角度（弧度）// Angle_ax=Angle_ax*1.2*180/3.14;//弧度转换为度 Accel_x=GetData(ACCEL_XOUT_H)-1375; //读取X轴加速度426180 if(Accel_x>32767) { Accel_x=Accel_x-65536; } //Angle_ax=(Accel_x-570)/16384;//去除零点偏移,计算得到角度（弧度） //Angle_ax=Angle_ax*1.2*180/3.14;//弧度转换为度 Angle_ax=Accel_x*0.004196468;//角速度 //范围为2000deg/s时，换算关系：16.4LSB/(deg/s) //Gyro_y=GetData(GYRO_YOUT_H); //静止时角速度Y轴输出为-30左右 //Gyro_y=-(Gyro_y+12)/16.4;//去除零点偏移，计算角速度值,负号为方向处理 //Angle_gy=Angle_gy+Gyro_y*0.01;//角速度积分得到倾斜角度. Gyro_y=GetData(GYRO_YOUT_H)-1375; //静止时角速度Y轴输出为-30左右8 if(Gyro_y>32767) { Gyro_y=Gyro_y-65536; }//Gyro_y=Gyro_y*0.01098633; //Gyro_y=-Gyro_y/16.4;//去除零点偏移，计算角速度值,负号为方向处理 Gyro_y=-Gyro_y*0.06097561; // Angle_gy=Angle_gy+Gyro_y*0.005;// Kalman_Filter(Angle_ax,Gyro_y); /*//互补滤波 //补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与//陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度 //0.5为放大倍数，可调节补偿度；0.01为系统周期10ms Angle=Angle+(((Angle_ax-Angle)*0.5+Gyro_y)*0.01);*/ }voidDelay(__IOuint32_tnCount){ for(;nCount!=0;nCount--);}2.子程序2.1bsp_mpu6050.h#include"stm32f10x.h"#include"bsp_mpu6050.h"chartest=0; //IIC用到voidI2C_GPIO_Config(void) //IO口初始化{GPIO_InitTypeDefGPIO_InitStructure;RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);GPIO_InitStructure.GPIO_Pin=GPIO_Pin_6;GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_OD;GPIO_Init(GPIOB,&GPIO_InitStructure);GPIO_InitStructure.GPIO_Pin=GPIO_Pin_7;GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_OD;GPIO_Init(GPIOB,&GPIO_InitStructure);}voidI2C_delay(void){u8i=30;//这里可以优化速度，经测试最低到5还能写入while(i){i--;}}voiddelay5ms(void){inti=5000;while(i){i--;}}boolI2C_Start(void)//I2C起始判断{ SDA_H; SCL_H; I2C_delay(); if(!SDA_read)returnFALSE; SDA_L; I2C_delay(); if(SDA_read)returnFALSE; SDA_L; I2C_delay(); returnTRUE;}voidI2C_Stop(void) { SCL_L; I2C_delay(); SDA_L; I2C_delay(); SCL_H; I2C_delay(); SDA_H; I2C_delay();}voidI2C_Ack(void) //I2C发送应答信号{ SCL_L; I2C_delay(); SDA_L; I2C_delay(); SCL_H; I2C_delay(); SCL_L; I2C_delay();}voidI2C_NoAck(void) //I2C发送应答信号{ SCL_L; I2C_delay(); SDA_H; I2C_delay(); SCL_H; I2C_delay(); SCL_L; I2C_delay();}boolI2C_WaitAck(void) //I2C接收应答信号 返回为:=1有ACK,=0无ACK{ SCL_L; I2C_delay(); SDA_H; I2C_delay(); SCL_H; I2C_delay(); if(SDA_read) {SCL_L; I2C_delay();returnFALSE;} SCL_L; I2C_delay(); returnTRUE;}voidI2C_SendByte(u8SendByte)//数据从高位到低位//{u8i=8;while(i--){SCL_L;I2C_delay();if(SendByte&0x80)SDA_H;elseSDA_L; SendByte<<=1;I2C_delay(); SCL_H;I2C_delay();}SCL_L;}unsignedcharI2C_RadeByte(void)//数据从高位到低位//{u8i=8;u8ReceiveByte=0;SDA_H; while(i--){ReceiveByte<<=1;SCL_L;I2C_delay(); SCL_H;I2C_delay(); if(SDA_read){ReceiveByte|=0x01;}}SCL_L;returnReceiveByte;}boolSingle_Write(unsignedcharSlaveAddress,unsignedcharREG_Address,unsignedcharREG_data) //void{ if(!I2C_Start())returnFALSE;I2C_SendByte(SlaveAddress);if(!I2C_WaitAck()){I2C_Stop();returnFALSE;}I2C_SendByte(REG_Address);I2C_WaitAck(); I2C_SendByte(REG_data);I2C_WaitAck();I2C_Stop();delay5ms();returnTRUE;}unsignedcharSingle_Read(unsignedcharSlaveAddress,unsignedcharREG_Address){unsign