本文详细描述了如何基于先前的PID算法，对小车的直立环、速度环和转向环进行控制，同时引入蓝牙功能来改变小车的期望速度和转向，确保运动平衡与限幅。
-
任务：基于之前PID算法编写小车的可运动可平衡控制的功能代码
Control
基于之前完成的PID控制算法，修改部分编写【直立环】、【速度环】、【转向环】的控制函数
1、在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控制（小改）
——增加小车前进、后退功能需要对【期望速度Target_Speed】进行修改
//在保持直立的前提下
/***************************************速度环PI控制参数1：左编码器encoder_left参数2：右编码器encoder_right参数3：期望速度Target_Speed速度环输出=Kp2*（实际速度-期望速度）+Ki*速度偏差积分***************************************/int Velocity_Balance(int Encoder_Left,int Encoder_Right,int Target_Speed){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;Encoder_Integral=Encoder_Integral - Target_Speed; //===接收遥控器数据，控制前进后退//积分限幅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;//===电机关闭后清除积分if(pitch<-40||pitch>40){Encoder_Integral=0;}return PWM_Velocity_Balance;}
转向环控制（大改）
——解除对小车转向的约束，并新增其对小车的转向控制关系
//重要！！
/**************************************************************************转向PD控制入口参数：参数1：Z轴角速度参数2：遥控数据Turn_Kd——转向的【约束】Turn_Kp——转向的【遥控】**************************************************************************/int Turn_Balance(int gyroz,int RC){int PWM_Turn_Balance;//转向约束if(RC==0){//若无左右转向指令，则开启转向约束Turn_Kd=TURN_KD;}else{//若左右转向指令接收到，则去掉转向约束Turn_Kd=0;}PWM_Turn_Balance = Turn_Kd * gyroz + Turn_Kp * RC;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 gyroz,int RC);
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每采集一次数据就触发一次外部中断，在外部中断服务程序中执行控制
-
基于【MPU6050的INT引脚控制原理】新增蓝牙对小车【速度环】、【转向环】的控制
//在数据压入闭环控制前改变【Target_Speed】、【Turn_Speed】
1、在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(); //得到温度值--------------------蓝牙控制//USART3接收数据控制小车改变期望速度【前后】、【左右】//前后控制if((Fore==0)&&(Back==0))Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地if(Fore==1)Target_Speed--; //前进1标志位拉高-->需要前进if(Back==1)Target_Speed++; //后退1标志位拉高-->需要后退//左右控制if((Left==0)&&(Right==0))Turn_Speed=0;//未接受到左转右转指令-->约束转向if(Left==1)Turn_Speed-=30; //左转1标志位拉高-->需要左转if(Right==1)Turn_Speed+=30; //右转1标志位拉高-->需要右转//限幅函数//防止【速度环】和【转向环】对小车控制过度Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//前后限幅Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//左右限幅//将数据压入闭环控制中，计算得到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,Turn_Speed);//将PWM控制输出值装载到电机上Motor1 = PWM_Vertical_Balance - PWM_Velocity_Balance - PWM_Turn_Balance;Motor2 = PWM_Vertical_Balance - PWM_Velocity_Balance + PWM_Turn_Balance;//PWM限幅Limit(&Motor1,&Motor2);//倾角异常关闭电机Turn_Off(pitch);//装载PWM到电机Motor_Load(Motor1,Motor2);}}}
2、在外部中断服务函数
EXTI9_5_IRQHandler中增加限幅功能
//宏定义#define SPEED_Y 100 //俯仰(前后)最大设定速度#define SPEED_Z 80//偏航(左右)最大设定速度//限幅函数//防止【速度环】和【转向环】对小车控制过度Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//前后限幅Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//左右限幅
结合之前写的USART3串口蓝牙发送数据就可以使小车完成对应方向的可运动可平衡功能
例如：用蓝牙向小车发送二进制数据【0x5A】or十进制数据【90】，小车就会向前运动并在限幅内保持运动平衡
扫一扫
820
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
