卡尔曼滤波可直接用

依恋lry · 发表于 2012-2-24 23:48:48

#include<SH88F2051.h>

#define pwm_l PWMD
#define pwm_r TH0
#define MA P4_0
#define MB P4_1
#define MC P4_2
#define MD P3_7
#define FW MA=1;MB=0;MC=1;MD=0
#define BW MA=0;MB=1;MC=0;MD=1

void INT_INI()
{
EX1=1;
EX0=1;    //开中断
IT0=1;    //边沿触发
IT1=1;
PX0L = 1;    // 设置 INT0为高级中断
PX1L = 1;
}

void USART_INI()
{
SCON=0X50;

T2MOD=0X80;    //不对sysclk再分频，51那是直接用ocsclk计数故徐分频
T2CON=0X30;    //收发都以t2计数
RCAP2H=0XFF;
RCAP2L=0XE5; //计算得来，非查表
TR2=1;    //开T2
}

void PWM_INI()
{
PWMCON=0XB1;    //八分频，pwm引脚输出
PWMP=0Xff;       //计数上限
IPL1 |= 0x20;    // 设置 PPWML为高级中断
IEN1  |= 0x20;    // PWM 允许中断

/*
TMOD|=2;    //t0自重载方式
TCON1|=4;    //不分频
TH0=0Xf8;    //e0
TL0=0Xf8;
//ET0=1;    //T0中断允许
PT0L=1;    // 设置 T0定时器为高级中断 */
}

void USART_T( unsigned char dat )
{
SBUF=dat;
while(!TI);
TI=0;

}

void
ADC_INI()
{
ADT=0; //ad计算时间

ADCH=0XFF;    //选择管脚复用功能为ad
}

void PORT_INI()
{
P1M0 = 0x08;
  P1M1 = 0;
  P3M0 = 0;       // 设置 MA,MB,MC,MD,C1为推挽输出, C2为准双向
  P3M1 = 0xC0;
  P4M0 = 0;
  P4M1 = 0x07;
}

static float angle, angle_dot; //外部需要引用的变量

//Kalman滤波
//-------------------------------------------------------

//-------------------------------------------------------
static  float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.5;
//注意：dt的取值为kalman滤波器采样时间;
static float Pk[2][2] = { {1, 0 }, {0, 1 }};

static float Pdot[4] ={0,0,0,0};

static const char C_0 = 1;

static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m)
{
angle+=(gyro_m-q_bias) * dt;

Pdot[0]=Q_angle - Pk[0][1] - Pk[1][0];
Pdot[1]=- Pk[1][1];
Pdot[2]=- Pk[1][1];
Pdot[3]=Q_gyro;

Pk[0][0] += Pdot[0] * dt;
Pk[0][1] += Pdot[1] * dt;
Pk[1][0] += Pdot[2] * dt;
Pk[1][1] += Pdot[3] * dt;

angle_err = angle_m - angle;

PCt_0 = C_0 * Pk[0][0];
PCt_1 = C_0 * Pk[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 * Pk[0][1];

Pk[0][0] -= K_0 * t_0;
Pk[0][1] -= K_0 * t_1;
Pk[1][0] -= K_1 * t_0;
Pk[1][1] -= K_1 * t_1;

angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;
}



ADC(unsigned char port)
{
float x;

ADCON|=port*2; //选端口
ADCON|=0X80; //开是转化
x++;
x--;
ADCON|=0X01;
while(ADCON&1); //等待完成

x=ADDH*4;
x+=ADDL;

ADCON=0;

return x;
}

static float acceler=0,gyro=0;unsigned char flag=1 ;
void AD_calculate(void)
{

  gyro=0.011557*ADC(1)-3.0396+114*3.14/180;
if(gyro*57.3>43)
gyro=43/57.3;

//acceler =ADC(0);

acceler=0.003052*ADC(0)-1.4375; //系数换算：2.5/(1.2*512);

acceler=acceler*1.5+14*3.14/180;       // acceler=asin(acceler)*1.5+5*3.14/180;


//angle=acceler; angle_dot=gyro;


Kalman_Filter(acceler,gyro);

}

void T1_INI()
{
TMOD|=0X10; //方式1,16位
//TCON1=0X00; 12分频
TH1=0Xf8;
TL1=0XFA;
TR1=1;
ET1=1;    //  允许T1中断
}

//-------------------------------------------------------
//PWM输出
//-------------------------------------------------------
unsigned int PWM_10bit; // 10位 PWM 输出缓冲区
void PWM_output (int PWM_LH)
{
if (PWM_LH<0)
{
FW;
PWM_LH*=-1;
}
else
{
BW;
}

if (PWM_LH>252)
{
PWM_LH=252;
}

PWM_10bit=4*PWM_LH;

}

static int speed_real_RH;
static int speed_real_LH;

//-------------------------------------------------------
//计算PWM输出值
//车辆直径：76mm; 12*64pulse/rev; 1m=3216pulses;
//-------------------------------------------------------
//static int speed_diff,speed_diff_all,speed_diff_adjust;
//static float K_speed_P,K_speed_I;
static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot;
static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;
static float position,position_dot;
static float position_dot_filter;
static float PWM;
//static int speed_output_LH,speed_output_RH;
static unsigned char Turn_Need,Speed_Need;

//-------------------------------------------------------
void PWM_calculate(void)
{

     K_angle_AD=ADC(2)*0.009;//2;
K_angle_dot_AD=ADC(6)*0.009;//3;
K_position_AD=ADC(4)*0.009;//5;
K_position_dot_AD=ADC(5)*0.009;//3.5;

position_dot=(speed_real_LH+speed_real_RH)*0.5;
// position_dot=-PWM*0.0027;

position_dot_filter*=0.85; //车轮速度滤波
position_dot_filter+=position_dot*0.15;

position+=position_dot_filter;
//position+=position_dot;
position+=Speed_Need;

if (position<-768) //防止位置误差过大导致的不稳定
{
position=-768;
}
else if  (position>768)
{
position=768;
}

PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD
   +K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD;

PWM=-PWM;
// speed_output_RH = PWM ;
// speed_output_LH = PWM  ;

//PWM=angle;

/* if (PWM>252)
{
PWM=252;
}
else if(PWM<-252)
{
PWM=-252;
}  */

PWM_output (PWM);
}



void main()    /////////////////////////////////////////////////////////////////
{
int t=0;
unsigned char i=0,j;

CLKCON=0x2c;  //内部16.6M，两分频后8.3Mhz

USART_INI();    //初始化串口
ADC_INI();    //初始化ad
T1_INI();    //初始化T1
PWM_INI(); //PWM初始化
INT_INI(); //外部中断初始化
PORT_INI(); //端口初始化
EA=1;    //开中断

K_position= 0* 0.558; //0.5 0.8换算系数：(256/10) * (2*pi/(64 50*12 6))=0.20944；//256/10:电压换算至PWM，256对应10V；
K_angle=819; //32换算系数：256/10 =25.6；
K_position_dot=61; //1.09换算系数：(256/10) * (100*2*pi/(64*12))=20.944；
K_angle_dot= 51; //2换算系数：256/10 =25.6；


for (i=1;i<=500;i++) //延时启动PWM，等待卡尔曼滤波器稳定
{
for (j=1;j<=300;j++);

}

while(1)
{

/* Dword var32;
var32 = sdiv32_8(-987654321,-123);
uidiv(1,2);
sdiv32_8(-987654321,-123);
USART_T(angle*57.3+128);
if(P1&0X08)
{
Speed_Need=0.1;
P1|=0x04;
}
else
{Speed_Need=0;
  P1&=0xfb;    USART_T(speed_real_LH);
  }    */
}
}

void INT_L(void) interrupt 0  using 1
{
if(MA==1)
{
  speed_real_LH++;
}
else
{speed_real_LH--;
}

}

void INT_R(void) interrupt 2  using 1
{
if(MA==1)
{
  speed_real_RH++;
}
else
speed_real_RH--;
}


void PWMp(void) interrupt 12    using 1
{ static unsigned int PWM_10bit_bak;    // 10位 PWM 输出缓冲发送区
  static unsigned char PWM_Counter=0;    // 10位 PWM 输出发送个数计数器
  if(PWM_Counter == 0)
{ PWM_10bit_bak = PWM_10bit & 0x3ff;    // 10位 PWM 发送开始，装入10位 PWM 初值至输出缓冲发送区

}
  if(PWM_10bit_bak >= 26)
{ PWM_10bit_bak -= 26;    // 10位 PWM输出缓冲发送区发送数据>=26, 数据减26暂存
   PWMD = 0xff;    // 10位 PWM 发送一次数据0xff
}
  else if(PWM_10bit_bak == 0)    // 10位 PWM输出缓冲发送区发送数据=0
{ PWMD = 0x00;    // 10位 PWM 发送一次数据0x00
}
  else
{ PWM_10bit_bak = 0;    // 10位 PWM输出缓冲发送区发送数据<26, 数据乘10，发送之
   PWMD = (unsigned char)(PWM_10bit_bak * 10);
}
  if(++PWM_Counter > 40)
{ PWM_Counter = 0;    // 10位 PWM 输出发送个数计数值大于40, 清0
}
  PWMCON &= 0XFD;                // 清除 PWM 周期计数器溢出标志 PWMIF

}

unsigned char i=0;int j=0 ;
void T1_isr(void) interrupt 3
{

TH1=0XE4;
TL1=0XFA;

AD_calculate();
PWM_calculate();


i++; j++;
if(j==500&&flag==1)
{
j=0;flag=0;dt=0.01;  //这里是为了提高滤波速度，dt越大速度越快但误差大，dt小速度慢误差小
   //所以这里前五秒dt为0.5，五秒过后dt变为正常的0.01

}

if(i==6)
{
i=0;

USART_T(angle*57.3);
USART_T(angle_dot*57.3);
      USART_T(255);

}
speed_real_RH =0;
speed_real_LH    =0;
}