STM32F411+MPU6050控制平衡小車,在獲取姿態(tài)時，如果不加IF判斷，程序就會卡死在DMP...

ID:462642 · 發(fā)表于 2021-5-28 22:38

本帖最后由 hgfhgfddd 于 2021-6-4 19:43 編輯

本人使用的是HAL庫來編程，在移植MPU6050的DMP庫時，遇到了一個問題，希望有大佬指導一下。

在獲取姿態(tài)時，如果不加IF判斷，程序就會卡死在DMP庫中的某個地方。
注意，不要打開文件中的STM32CUBEMX工程，那個是錯的，不能用�。�！
程序部分代碼如下

#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：使用串口直接打印數(shù)據(jù)
#define N 5
#define turnzhi 1.5
//float Turn_Kp = 3;
float
Turn_Kd=turnzhi,//轉向環(huán)KP、KD
Turn_Kp=30;
#define SPEED_Y 600 //俯仰(前后)最大設定速度
#define SPEED_Z 80//偏航(左右)最大設定速度
int Vertical_out,Velocity_out = 0,Turn_out = 0;//直立環(huán)&速度環(huán)&轉向環(huán) 的輸出變量
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);//函數(shù)聲明
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;//未接受到前進后退指令-->速度清零，穩(wěn)在原地
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); //直立環(huán)
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度環(huán)
Turn_out=Turn(gyroz,Turn_Speed);
//轉向環(huán)
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 發(fā)送實際值
// #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);
}
}
}
/*********************
直立環(huán)PD控制器：Kp*Ek+Kd*Ek_D
入口：期望角度、真實角度、真實角速度
出口：直立環(huán)輸出
*********************/
//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; //===接收遙控器數(shù)據(jù)，控制前進后退
// 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;
//}
/*********************
直立環(huán)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;
}
/*********************
速度環(huán)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;//防止速度過大的影響直立環(huán)的正常工作。
//3.對速度偏差積分，積分出位移
Encoder_S+=EnC_Err_Lowout;//【4】
//4.積分限幅
Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
//5.速度環(huán)控制輸出計算
PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;//【5】
if(pitch < (-40) || pitch > (40)) Encoder_S = 0;
return PWM_out;
}
///*********************
//轉向環(huán)：系數(shù)*Z軸角速度
//*********************/
//int Turn(int gyro_Z)
//{
// int PWM_out;
//
// PWM_out=Turn_Kp*gyro_Z;
// return PWM_out;
//}
/*********************
轉向環(huán)：系數(shù)*Z軸角速度+系數(shù)*遙控數(shù)據(jù)
*********************/
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;
}