|
|
前言
今天聊一聊代码，只有直立功能的代码。
代码总体思路
给定一个目标值，单片机通过IIC和mpu6050通信，得知数据后，根据角度环计算出一个PWM值给电机驱动器，从而控制单机转动。电机转动，编码器就会输出脉冲，单片机通过编码器模式检测脉冲值，并把该值代入速度环计算出一个角度值送给角度环，角度环在输出PWM给电机，以此循环。其实就是上节的框图，再贴一遍。
程序
程序部分为了减少篇幅，之前讲解的编码器模式及pwm等代码都不在讲解，原理都一样，只是引脚不同，我只把重点代码说一下。如果想看这部分代码的同学，后面我整理之后会放在公众号。
时序
通过外部中断检测mpu6050INT引脚，检测到信号之后就进行一次pid运算。以此来严格控制PID计算周期。可能有些人会有疑问，为什么不放到while中循环检测？因为放在while循环的话，这个循环是可以被中断打断的。小车平衡对实时性要求高，如果在while循环里，姿态矫正时，程序被其他模块中断，小车就立不起来了。因此外部中断的优先级要配置成最高。
代码如下：
/************************外部中断，通过mpu6050的INT引脚，严格控制PID计算周期 PB5引脚**************************/void MPU6050_EXTI_Init(void){ EXTI_InitTypeDef EXTI_InitStruct; GPIO_InitTypeDef GPIO_InitStruct; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);//开启时钟 GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IPU; //上拉输入 GPIO_InitStruct.GPIO_Pin=GPIO_Pin_5; //PB5 GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz; GPIO_Init(GPIOB,&GPIO_InitStruct); GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);//配置中断线 EXTI_InitStruct.EXTI_Line=EXTI_Line5;//EXTI中断/事件线选择，可选 EXTI0 至 EXTI19 EXTI_InitStruct.EXTI_LineCmd=ENABLE;//使能 EXTI_InitStruct.EXTI_Mode=EXTI_Mode_Interrupt;//EXTI 模式选择 EXTI_InitStruct.EXTI_Trigger=EXTI_Trigger_Falling;//EXTI 边沿触发事件 EXTI_Init(&EXTI_InitStruct);//初始化中断}//优先级配置void NVIC_Config(void){ NVIC_InitTypeDef NVIC_InitStruct; NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//4级抢占,4级响应。 //外部中断 NVIC_InitStruct.NVIC_IRQChannel=EXTI9_5_IRQn; NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE; NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=0; NVIC_InitStruct.NVIC_IRQChannelSubPriority=0; NVIC_Init(&NVIC_InitStruct);}
直立环
原理上节都讲过了，必要的地方我在程序中已经注释了。
/**************************************************************************函数功能：直立PD控制入口参数：角度、角速度返回 值：直立控制PWM作 者：平衡小车之家**************************************************************************/int balance(float Angle,float Gyro){ float Bias; int balance; Bias=Angle-ZHONGZHI; //===求出平衡的角度中值 和机械相关 balance=Balance_Kp*Bias+Gyro*Balance_Kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数 return balance;}
上面角度环的公式是不是和上节中推导的一样。
Angle - Med_Angle：
Angle 为真实角度Med_Angle为机械中值角度，也就是当小车直立时mpu6050所检测到的角度，该角度并不一定为0Angle - Med_Angle为真实角度和机械中值角度的偏差
测量机械中值的方法（手动）：
将oled和mpu6050都连接好并调试通过之后，将采样的Pitch值显示在oled上。之后让小车轮子固定不动，用手沿电机旋转方向先逆时针（顺时针）慢慢推动小车上方，让小车直立，当小车从直立状态向逆时针方向倾斜时，看看屏幕上显示的角度是多少，之后同样的方法测顺时针，看看角度是多少，之后两值相加/2，得出的值就是机械中值。
速度环
/**************************************************************************函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity，比如，改成60就比较慢了入口参数：左轮编码器、右轮编码器返回 值：速度控制PWM作 者：平衡小车之家**************************************************************************/int velocity(int encoder_left,int encoder_right){ static float Velocity,Encoder_Least,Encoder,Movement; static float Encoder_Integral,Target_Velocity; //=============速度PI控制器=======================// Encoder_Least =(encoder_left+encoder_right)-0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零） Encoder *= 0.7; //===一阶低通滤波器 Encoder += Encoder_Least*0.3; //===一阶低通滤波器 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; //===速度控制 return Velocity;}
速度环加入低通滤波，目的为了减少速度环的作用，毕竟直立才是最重要的。而且速度环相对于直立环来说是个干扰，因此不能让速度环作用太大。
转向环
/**************************************************************************函数功能：转向控制 修改转向速度，请修改Turn_Amplitude即可入口参数：左轮编码器、右轮编码器、Z轴陀螺仪返回 值：转向控制PWM作 者：平衡小车之家**************************************************************************/int turn(int encoder_left,int encoder_right,float gyro)//转向控制{ static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count; float Kd=1.3; //=============转向PD控制器=======================// Turn=-gyro*Kd; //===结合Z轴陀螺仪进行PD控制 return Turn;}
转向环目的为了保持直线行走。假设小车发生偏移，gyro就不为0，转向环就会有输出。
最终控制代码
int EXTI9_5_IRQHandler(void){ int PWM_out; if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定 { if(PBin(5)==0)//二级判定 { EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位 //进入中断首先读取数据----编码器数据和角度数据 Encoder_Left=-Read_Encoder(2); //===读取编码器的值 Encoder_Right=Read_Encoder(4); //===读取编码器的值 mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度 MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺仪 MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度 Balance_Pwm =balance(Pitch,gyroy); //===角度环 Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); //===速度环PID控制 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点 Turn_Pwm =turn(Encoder_Left,Encoder_Right,gyroz); //===转向环PID控制 PWM_out=Balance_Pwm - Balance_Kp*Velocity_Pwm;//最终输出 Moto1=PWM_out+Turn_Pwm; //===计算左轮电机最终PWM Moto2=PWM_out-Turn_Pwm; //===计算右轮电机最终PWM Xianfu_Pwm(); //===PWM限幅 Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器 } } return 0; }
注意
PWM_out=Balance_Pwm - Balance_Kp*Velocity_Pwm;//最终输出
由于是串行PID，因此速度环的输出就是直立环的输入，如上面的框图一样。因此将速度环代入到直立环当中，就可以求出最终输出。也就是将Velocity_Pwm代入到角度环形参Angle中即可得出最终输出公式。
mpu6050、oled代码移植
这两个硬件相关的原理就不讲了，网上有很多视频讲解，这里简单说下移植步骤吧。
第一步：找到相关的.c 和 .h文件
将这几个文件复制到你的HAREWARE文件夹中，可以分别单独建个文件夹，方便查找。如下图：
之后再keil中添加C文件及设置头文件查找路径：
之后进行编译，就能调用函数了。主函数初始化：
OLED_Init(); OLED_Clear(); MPU_Init(); mpu_dmp_init();
while中一直刷新平衡角：
while(1) { OLED_Float(0,10,Pitch,3); }
读取角度的函数放在外部中断中：
mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度 MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺仪 MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
总结
原理明白之后，在看代码是不是很简单，先在天上飘一会。。。
一路过来，其实平衡小车也不难嘛。再飘一会。。。
。。。。下不来了。
|
|
