- 重置了MPU6050系列文件
- 在sys.c中增加了NVIC函数（所有外部中断函数统一在此定义）
- 整理了本应在sys.h中的各种声明
- 移除了Timer
- 初步完成了PID算法的编写
- 初步完成了外部中断服务函数
之前已完成：【编码器】、【PWM】、【Motor】、【OLED】、【MPU6050】、【EXTI】、【NVIC】
PID
基于之前学习的PID控制算法，编写【直立环】、【速度环】、【转向环】的控制函数
1、完成驱动文件导入操作和编写驱动程序基本代码（参考之前文章）
//将【Control】配置文件放在【Hardware】文件夹里
2、在Control.c编写PID控制函数
-
直立环PD控制
/***************************************直立环PD控制参数1：实际角度Angle参数2：机械中值（期望角度）Mechanical_balance参数3：角速度Gyro直立环输出=Kp1*（实际角度-期望角度+机械中值）+Kd*角度偏差的微分***************************************/int Vertical_Balance(float Angle,float Mechanical_balance,float gyroy){//直立环PWM输出int PWM_Vertical_Balance;PWM_Vertical_Balance = Vertical_Kp * (Angle - Mechanical_balance) + Vertical_Kd * gyroy;return PWM_Vertical_Balance;}
-
速度环PI控制
/***************************************速度环PI控制参数1：左编码器encoder_left参数2：右编码器encoder_right参数3：期望速度Target_Speed速度环输出=Kp2*（实际速度-期望速度）+Ki*速度偏差积分***************************************/int Velocity_Balance(int Encoder_Left,int Encoder_Right,int Target_Speed){//速度环PWM输出static float PWM_Velocity_Balance;//速度偏差static float Encoder_Error,Encoder_Error_Last;//速度偏差积分static float Encoder_Integral;//速度偏差=实际速度-期望速度Encoder_Error_Last = ( Encoder_Left + Encoder_Right ) - Target_Speed;//速度偏差经过【一阶低通滤波器】/*一阶低通滤波器公式：Low_Out = ( 1 - a ) * Encoder_Error + a * Low_Out_Last【参数a】可自行设置，【Low_Out】为一阶低通滤波器本次输出，【Low_Out_Last】为一阶低通滤波器上次输出*///Encoder_Error_Low_Out = ( 1 - a ) * Encoder_Error + a * Encoder_Error_Low_Out_Last;Encoder_Error *= 0.8;Encoder_Error += Encoder_Error_Last*0.2;//用速度偏差进行积分（得到位移）Encoder_Integral +=Encoder_Error;//积分限幅if(Encoder_Integral > 10000){Encoder_Integral = 10000;}if(Encoder_Integral < -10000){Encoder_Integral = -10000;}PWM_Velocity_Balance = Velocity_Kp * Encoder_Error + Velocity_Ki * Encoder_Integral;return PWM_Velocity_Balance;}
-
转向环控制
/***************************************转向环PD控制***************************************/int Turn_Balance(int gyroz){int PWM_Turn_Balance;PWM_Turn_Balance = Turn_Kd * gyroz;return PWM_Turn_Balance;}
3、在Control.h中声明PID控制函数
int Vertical_Balance(float Angle,float Mechanical_balance,float Gyro);int Velocity_Balance(int Encoder_left,int Encoder_right,int Target_Speed);int Turn_Balance(int Gyro_Z);
4、在sys.h中
#includ
e "Control
.h"
#include "Control.h"
IRQHandler
-
MPU6050的INT引脚控制原理：每当MPU6050有数据输出时，引脚INT有相应的电平变化，将其触发外部中断作为控制周期
当MPU6050的读取一次数据，就控制一次，可以很好地保持MPU6050数据的实时性
EX：如果将 MPU6050 的采样频率设置为100HZ，即10ms更新一次数据，那么平衡车的控制周期就是10ms
-
控制函数写在外部中断服务函数中
-
外部中断接在了MPU6050的INT引脚上（PB5）
-
MPU6050每采集一次数据就触发一次外部中断，在外部中断服务程序中执行控制
-
在Control.c编写外部中断服务函数
void EXTI9_5_IRQHandler(void){if(EXTI_GetITStatus(EXTI_Line5) == SET)//中断标志位判断，确定由哪个中断源触发{if(PBin(5)==0)//MPU6050触发中断之后INT（PB5）应为低电平{EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位//采集编码器数据Encoder_Left = -Read_Encoder(2);Encoder_Right = Read_Encoder(3);//采集MPU6050角度信息mpu_dmp_get_data(&pitch,&roll,&yaw); //得到欧拉角（姿态角）的数据MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度数据temp=MPU_Get_Temperature(); //得到温度值//将数据压入闭环控制中，计算得到PWM控制输出值PWM_Vertical_Balance = Vertical_Balance(pitch,Mechanical_balance,gyroy);PWM_Velocity_Balance = Velocity_Balance(Encoder_Left,Encoder_Right,Target_Speed);PWM_Turn_Balance = Turn_Balance(gyroz);//将PWM控制输出值装载到电机上Motor1 = PWM_Vertical_Balance - PWM_Velocity_Balance + PWM_Turn_Balance;//左轮电机最终PWM值Motor2 = PWM_Vertical_Balance - PWM_Velocity_Balance - PWM_Turn_Balance;//右轮电机最终PWM值//PWM限幅Limit(&Motor1,&Motor2);//装载PWM到电机Motor_Load(Motor1,Motor2);}}}
- 遇到的问题
不能进入外部中断，通过调试和检查，发现是sys.h中的【Control.h】文件写错名字，而且未在【MPU6050】系列文件中使能INT引脚（可能是之前改MPU6050采集频率不注意碰到了）
-
需要注意的
-
各个文件的变量存在关系
-
子函数进入主程序的顺序
-
通过
【Motor】+【PWM】+【Encoder】+【MPU6050】+【Control】+【NVIC】+【OLED】可以实现：
MPU6050每10ms采集一次数据，在采集时INT（PB5）引脚置低电平，引起外部中断，在外部中断服务函数中读取MPU6050和两个编码器数值（结合电机转动），并在OLED显示
//暂未将PID计算得出的PWM控制值装载入车轮
-
主程序
//电机装载变量int Motor1,Motor2;float pitch,roll,yaw; //欧拉角(姿态角)short aacx,aacy,aacz; //加速度传感器原始数据short gyrox,gyroy,gyroz; //陀螺仪原始数据float temp; //温度值//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°int Encoder_Left,Encoder_Right;int main(void){delay_init();NVIC_Configuration();OLED_Init();OLED_Clear();MPU_Init();mpu_dmp_init();MPU6050_EXTI_Init();PWM_Init_TIM1(72-1,100-1);Motor_Init();Encoder_TIM2_Init();Encoder_TIM3_Init();OLED_ShowString(0,0,"Pitch:",12);OLED_ShowString(0,1,"Roll :",12);OLED_ShowString(0,2,"Yaw :",12);OLED_ShowString(0,3,"Temp :",12);OLED_ShowString(0,4,"Left :",12);OLED_ShowString(0,5,"Right:",12);while(1){Motor_Load(30,30);/**************显示俯仰角***************/if(pitch<0){OLED_ShowString(48,0,"-",12);OLED_Float(0,56,-pitch,3);}else{OLED_ShowString(48,0,"+",12);OLED_Float(0,56,pitch,3);}/**************显示俯仰角***************//**************显示翻滚角***************/if(roll<0){OLED_ShowString(48,1,"-",12);OLED_Float(1,56,-roll,3);}else{OLED_ShowString(48,1,"+",12);OLED_Float(1,56,roll,3);}/**************显示翻滚角***************//**************显示航向角***************/if(yaw<0){OLED_ShowString(48,2,"-",12);OLED_Float(2,56,-yaw,3);}else{OLED_ShowString(48,2,"+",12);OLED_Float(2,56,yaw,3);}/**************显示航向角***************//**************显示温度*****************/OLED_Float(3,48,temp/100,2);/**************显示温度*****************//***********显示左右编码器值************/OLED_Num2(29,4,Encoder_Left);OLED_Num2(35,5,Encoder_Right);/***********显示左右编码器值************/}}
效果如图：
扫一扫
2181
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
