|
|
楼主 |
发表于 2015-7-20 09:15:24
|
显示全部楼层
- /* 定义PID类型 */
- typedef struct PID_Type PID_Type;
- struct PID_Type
- {
- float SetPoint;
- unsigned char SetPointChange; //设置目标发生变化
- int CtrlValue;//0~1000
- float kP;
- float kI;
- float kD;
- };
下面的计算工程的计算频率时100Hz
- MPU6050ReadAcc(Accel);
- MPU6050ReadGyro(Gyro);
- //对加速度进行平滑滤波
- acc_filter(Accel,Mag);
- //将获得的原始数据转化为度/s
- AnglePot=Gyro[0]*GYRO_SCALE;
- //根据滤波后的加速度计算角度
- Angle=atan2(Accel[1],Accel[2])*57.295780;
- //进行卡尔曼滤波
- kalman_filter(Angle,AnglePot,&f_Angle,&f_AnglePot);
- //对角度进行平滑滤波
- //angle_filter(&PID1,Angle,&AfterAccel);
- //返回PID的控制量，并设置占空比
- SET_DUTY(speed_filter(pid2(&PID1,f_Angle,f_AnglePot)));
-
- //显示波形
- Mag[0]=PID1.SetPoint;
- Mag[1]=Angle;
- Mag[2]=Duty;
- //角度、角速度放到PID2里面去
- PID2.kP=Angle;
- PID2.kI=AnglePot;
- PID2.kD=f_Angle;
- //显示相差的大小
- PID3.kD=fabs(PID1.SetPoint-Angle);
- //波形数据从串口发送出去->蓝牙->空气->蓝牙->串口->串口转USB->电脑接收并处理
- NingmingSend_Data(Gyro,Accel,Mag);
- Task_Delay[1]=10;
计算相关的函数
- #include "include.h"
- //卡尔曼滤波
- static float angle, angle_dot;
- const float Q_angle = 0.002, Q_gyro = 0.002, R_angle = 0.5, dt = 0.01;
- static float P[2][2]={
- { 1, 0 },
- { 0, 1 }
- };
- static float Pdot[4] = {0, 0, 0, 0};
- const u8 C_0 = 1;
- static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
- //平滑滤波
- short ax_buf[FILTER_COUNT]={0};
- short ay_buf[FILTER_COUNT]={0};
- short az_buf[FILTER_COUNT]={0};
- short Angle_buf[FILTER_COUNT]={0};
- s16 speed_buf[FILTER_COUNT]={0};
- /*************************************************
- 名称：void acc_filter(void)
- 功能：加速度计数据滤波
- 输入参数：据滤波后的数据
- 输出参数：无
- 返回值： 无
- **************************************************/
- void acc_filter(short *Accel,short *Mag)
- {
- u8 i;
- s32 ax_sum = 0, ay_sum = 0, az_sum = 0;
- for(i = 1 ; i < FILTER_COUNT; i++)
- {
- ax_buf[i - 1] = ax_buf[i];
- ay_buf[i - 1] = ay_buf[i];
- az_buf[i - 1] = az_buf[i];
- }
- ax_buf[FILTER_COUNT - 1] = Accel[0];
- ay_buf[FILTER_COUNT - 1] = Accel[1];
- az_buf[FILTER_COUNT - 1] = Accel[2];
- for(i = 0 ; i < FILTER_COUNT; i++)
- {
- ax_sum += ax_buf[i];
- ay_sum += ay_buf[i];
- az_sum += az_buf[i];
- }
- Mag[0] = (s16)(ax_sum / FILTER_COUNT);
- Mag[1] = (s16)(ay_sum / FILTER_COUNT);
- Mag[2] = (s16)(az_sum / FILTER_COUNT);
- }
- //角度的平滑滤波
- void angle_filter(short Angle,short *AfterAngle)
- {
- u8 i;
- s32 angle_sum = 0;
-
- for(i = 1 ; i < FILTER_COUNT; i++)
- {
- Angle_buf[i - 1] = Angle_buf[i];
- }
-
- Angle_buf[FILTER_COUNT - 1] = Angle;
-
- for(i = 0 ; i < FILTER_COUNT; i++)
- {
- angle_sum += Angle_buf[i];
- }
-
- (*AfterAngle)=(s16)(angle_sum / FILTER_COUNT);
- }
- /*************************************************
- 名称：void kalman_filter(float angle_m, float gyro_m, float *angle_f, float *angle_dot_f)
- 功能：陀螺仪数据与加速度计数据通过滤波算法融合
- 输入参数：
- float angle_m 加速度计计算的角度
- float gyro_m 陀螺仪角速度
- float *angle_f 融合后的角度
- float *angle_dot_f 融合后的角速度
- 输出参数：滤波后的角度及角速度
- 返回值：无
- **************************************************/
- void kalman_filter(float angle_m, float gyro_m, float *angle_f, float *angle_dot_f)
- {
- angle += (gyro_m - q_bias) * dt;
-
- Pdot[0] =Q_angle - P[0][1] - P[1][0];
- Pdot[1] = -P[1][1];
- Pdot[2] = -P[1][1];
- Pdot[3] = Q_gyro;
-
- P[0][0] += Pdot[0] * dt;
- P[0][1] += Pdot[1] * dt;
- P[1][0] += Pdot[2] * dt;
- P[1][1] += Pdot[3] * dt;
-
- angle_err = angle_m - angle;
-
- PCt_0=C_0 * P[0][0];
- PCt_1=C_0 * P[1][0];
-
- E = R_angle + C_0 * PCt_0;
-
- K_0 = PCt_0 / E;
- K_1 = PCt_1 / E;
-
- t_0 = PCt_0;
- t_1 = C_0 * P[0][1];
- P[0][0] -= K_0 * t_0;
- P[0][1] -= K_0 * t_1;
- P[1][0] -= K_1 * t_0;
- P[1][1] -= K_1 * t_1;
-
- angle += K_0 * angle_err;
- q_bias += K_1 * angle_err;
- angle_dot = gyro_m - q_bias;
- *angle_f = angle;
- *angle_dot_f = angle_dot;
- }
- /*************************************************
- 名称：void speed_filter(void)
- 功能：速度滤波
- 输入参数：无
- 输出参数：无
- 返回值： 无
- **************************************************/
- int speed_filter(int speed)
- {
- u8 i;
- s32 speed_sum = 0;
- for(i = 1 ; i < FILTER_COUNT; i++)
- {
- speed_buf[i - 1] = speed_buf[i];
- }
- speed_buf[FILTER_COUNT - 1] = speed;
- for(i = 0 ; i < FILTER_COUNT; i++)
- {
- speed_sum += speed_buf[i];
- }
-
- return (s16)(speed_sum / FILTER_COUNT);
- }
双向PID是很好的，我没有试过，但是曾经想试！你做个，到时发个视频上来！ |
|
[复制链接]
发表于 2015-7-16 12:45:13
多谢回复~下午试了下，可以绘制出传感器的曲线。不知道可不可以在上位机上直接修改PID参数进行调节，上位机上的帮助信息有一部分没有看明白~
