STM32F411+MPU6050控制平衡小车,在获取姿态时，如果不加IF判断，程序就会卡死在DMP... -

#include "control.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "main.h"
#include "stdio.h"
#include "motor.h"
#include "protocol.h"
#define PID_ASSISTANT_EN 1 // 1:使用PID调试助手显示波形，0：使用串口直接打印数据
#define N 5
#define turnzhi 1.5
//float Turn_Kp = 3;
float
Turn_Kd=turnzhi,//转向环KP、KD
Turn_Kp=30;
#define SPEED_Y 600 //俯仰(前后)最大设定速度
#define SPEED_Z 80//偏航(左右)最大设定速度
int Vertical_out,Velocity_out = 0,Turn_out = 0;//直立环&速度环&转向环的输出变量
int i,j;
int b = 5;
short Accel[3];
short Gyro[3];
float Temp;
char value_buff[N];
char k = 0;
// short filter()
// {
// int count;
// int sum = 0;
// value_buff[k++] = gyroz;
//
// if(k == N)
// k = 0;
// for(count = 0;count < N;count ++)
// sum += value_buff[count];
// return (short)(sum/N);
// }
//
int Vertical(float Med,float Angle,float gyro_Y);//函数声明
int Velocity(float Target_Speed,int encoder_left,int encoder_right);
int Turn(int gyro_Z,int RC);
void angle_even(float pitch);
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
unsigned long a = 20;
int PWM_out;
if(GPIO_Pin == GPIO_PIN_5)
{
if(mpu_dmp_get_data(&pitch,&roll,&yaw) == 0)
{
mpu_get_gyro_reg(Accel,&a);
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
//Temp = MPU_Get_Temperature();
}
if(j >= 10)
{
j = 0;
Encoder_Left = Read_Speed(3);
Encoder_Right = - Read_Speed(4);
}
j++;
/*前后*/
if((Fore==0)&&(Back==0))Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地
if(Fore==1)Target_Speed-=5;//前进1标志位拉高-->需要前进
if(Back==1)Target_Speed+=5;//
Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅
/*左右*/
if((Left==0)&&(Right==0))Turn_Speed=0;
if(Left==1)Turn_Speed+=1; //左转
if(Right==1)Turn_Speed-=1; //右转
Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//限幅( (20*100) * 100 )
/*转向约束*/
if((Left==0)&&(Right==0))Turn_Kd=turnzhi;//若无左右转向指令，则开启转向约束
else if((Left==1)||(Right==1))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束
Vertical_out=Vertical(Med_Angle,pitch,gyroy); //直立环
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
Turn_out=Turn(gyroz,Turn_Speed);
//转向环
PWM_out=Vertical_out-Vertical_Kp*Velocity_out;//最终输出
MOTO1=PWM_out-Turn_out;//左电机
MOTO2=PWM_out+Turn_out;//右电机
Limit(&MOTO1,&MOTO2); //PWM限幅
angle_even(pitch);
Load(MOTO1,MOTO2); //加载到电机上。
i ++;
if(i >= 30)
{
i = 0;
HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_13);
// #if defined(PID_ASSISTANT_EN)
// set_computer_value(SEND_PERIOD_CMD, CURVES_CH1, &b, 1); // 给通道 1 目标值值
// #endif
//
// a= (int)pitch;
// #if defined(PID_ASSISTANT_EN)
// set_computer_value(SEND_FACT_CMD, CURVES_CH1, &a, 1); // 给通道 1 发送实际值
// #endif
printf("Pitch:%8.2f",pitch);
printf(" YAW:%8.2f",yaw);
printf(" Gyro:%8d Z:%8d",gyroy,gyroz);
printf(" EL:%d, ER:%d",Encoder_Left,Encoder_Right);
printf(" PWM:%d\r\n",MOTO1);
//printf("Temp: %2.2f\r\n",Temp);
}
}
}
/*********************
直立环PD控制器：Kp*Ek+Kd*Ek_D
入口：期望角度、真实角度、真实角速度
出口：直立环输出
*********************/
//int Vertical(float Med,float Angle,float gyro1_Y)
//{
// int PWM_out;
//
// PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro1_Y-0);
// return PWM_out;
//}
void angle_even(float pitch)
{
if(pitch > (40) || pitch < (-40) )
{
MOTO1 = 0;
MOTO2 = 0;
PB12(H);PB13(H);
PB14(H);PB15(H);
}
}
//int PI_Balance(int encoder_left,int encoder_right)
//{
// static float Velocity,Encoder_Least,Encoder,Movement;
// static float Encoder_Integral;
//
// Movement = 0;
//
// //=============速度PI控制器=======================//
// 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; //===速度控制
// if(pitch < -40 || pitch > 40) Encoder_Integral=0; //===电机关闭后清除积分
// return Velocity;
//}
/*********************
直立环PD控制器：Kp*Ek+Kd*Ek_D
Med:机械中值
Angle:真实角度
gyro_Y:真实角度的加速度
*********************/
int Vertical(float Med,float Angle,float gyro_Y)
{
int PWM_out;
PWM_out=Vertical_Kp*(Angle - Med)+Vertical_Kd*(gyro_Y-0);//【1】
return PWM_out;
}
/*********************
速度环PI：Kp*Ek+Ki*Ek_S
*********************/
int Velocity(float Target_Speed,int encoder_left,int encoder_right)
{
static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;//【2】
float a=0.7;//【3】
//1.计算速度偏差
Encoder_Err=(encoder_left+encoder_right)-Target_Speed;//舍去误差
//2.对速度偏差进行低通滤波
//low_out=(1-a)*Ek+a*low_out_last;
EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last;//使得波形更加平滑，滤除高频干扰，防止速度突变。
EnC_Err_Lowout_last=EnC_Err_Lowout;//防止速度过大的影响直立环的正常工作。
//3.对速度偏差积分，积分出位移
Encoder_S+=EnC_Err_Lowout;//【4】
//4.积分限幅
Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
//5.速度环控制输出计算
PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;//【5】
if(pitch < (-40) || pitch > (40)) Encoder_S = 0;
return PWM_out;
}
///*********************
//转向环：系数*Z轴角速度
//*********************/
//int Turn(int gyro_Z)
//{
// int PWM_out;
//
// PWM_out=Turn_Kp*gyro_Z;
// return PWM_out;
//}
/*********************
转向环：系数*Z轴角速度+系数*遥控数据
*********************/
int Turn(int gyro_Z,int RC)
{
int PWM_out;
//这不是一个严格的PD控制器，Kd针对的是转向的约束，但Kp针对的是遥控的转向。
PWM_out=Turn_Kd*gyro_Z + Turn_Kp*RC;
return PWM_out;
}

复制代码