【求助】关于平衡小车用TIM6定时10ms中断读取6050数据计算pwm,为什么使用中断读数是死的0/90，不用中断读数完全正确。麻烦前辈帮看看问题

电子音符 · 发表于 2015-4-18 15:11:14

void Timerx_Init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); //时钟使能

TIM_TimeBaseStructure.TIM_Period = arr;
TIM_TimeBaseStructure.TIM_Prescaler =psc;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStructure);

TIM_ITConfig( TIM6,TIM_IT_Update,ENABLE);//使能定时器6更新触发中断
    TIM_Cmd(TIM6, ENABLE);
  //NVIC配置
NVIC_InitStructure.NVIC_IRQChannel =TIM6_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}

//定时器6中断服务程序
void TIM6_IRQHandler(void)
{
if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)
{

            Motor_Control();
   LED1=!LED1;
}
  TIM_ClearITPendingBit(TIM6, TIM_IT_Update );
}

******************************

float Q_angle=0.001;
float Q_gyro=0.003;
float R_angle=0.5;
float dt=0.01; //*********************
float Angle=0,Gyro_x=0;         //小车滤波后倾斜角度/角速度
//******卡尔曼参数************

float Q_angle=0.001;
float Q_gyro=0.003;
float R_angle=0.5;
float dt=0.01;                   //dt为kalman滤波器中断采样时间;
char   C_0 = 1;
float Q_bias=0, Angle_err=0;
float PCt_0=0, PCt_1=0, E=0;
float K_0=0, K_1=0, t_0=0, t_1=0;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

//*********************************************************
// 卡尔曼滤波
//*********************************************************

//Kalman滤波，20MHz的处理时间约0.77ms；

void Kalman_Filter(float Gyro,float Accel)
{
Angle+=(Gyro - Q_bias) * dt; //先验估计

dot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

dot[1]=- PP[1][1];
dot[2]=- PP[1][1];
dot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
P[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

Angle_err = Accel - Angle;

Ct_0 = C_0 * PP[0][0];
Ct_1 = C_0 * PP[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 * PP[0][1];

PP[0][0] -= K_0 * t_0;   //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;

Angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
Gyro_x   = Gyro - Q_bias; //输出值(后验估计)的微分=角速度

// PWM_duty = (PD+PX) * Angle + (DD) * Gyro_x+ position*(0.0186+positionadd) + EncoderAver_filter * (1.77+speedadd);
   PWM_duty = (PD+PX) *Angle + (DD) * Gyro_x;
   printf("角速度为：%4.2f,\n", Gyro_x);
   printf("角度为：%4.2f\n", Angle);
   printf("PWM=%3d,\n", (int)PWM_duty);
}