金牌会员
 
- 积分
- 1252
- 威望
- 703
- 贡献
- 395
- 兑换币
- 16
- 注册时间
- 2011-3-1
- 在线时间
- 77 小时
|
#include<SH88F2051.h>
//#include<math.h>
//#include<hp.c>
//#define port_angle 0
//#define port_angle_dot 1
#define pwm_l PWMD
#define pwm_r TH0
//#define pwm_on PWMCON|=0X80;TR0=1
//#define pwmr_on P3|=0x10
//#define pwmr_off P3&=0xef
#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;
}
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;
}
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;
}
/*
//Kalman滤波，8MHz的处理时间约1.8ms；
//-------------------------------------------------------
static float angle, angle_dot; //外部需要引用的变量
//-------------------------------------------------------
static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.3;
//注意：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) //gyro_m:gyro_measure
{
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;
///angle_dot = gyro_m;
}
*/
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 angle,angle_dot; //外部需要引用的变量
//-------------------------------------------------------
static float bias_cf;
static const float dt=0.01;
//-------------------------------------------------------
void complement_filter(float angle_m_cf,float gyro_m_cf)
{
bias_cf*=0.0001; //陀螺仪零飘低通滤波；500次均值；0.998
bias_cf+=gyro_m_cf*0.009; //0.002
angle_dot=gyro_m_cf-bias_cf;
angle=(angle+angle_dot*dt)*0.95+angle_m_cf*0.04;
//加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；0.90 0.05
}
static float acceler,gyro;
void AD_calculate(void)
{
gyro=0.011557*ADC(1)-3.035+105*3.14/180; //后面105*3.14/180是对角速度零点的修正
acceler=0.003052*ADC(0)-1.4375; //计算角度时，只用了加速度传感器的Z轴就够了
acceler=acceler*1.5+14*3.14/180; //没有使用arcsin（），因为sin在0度附近变化时可以用sin值代替角度值，这样可以减轻单片机的计算负担。
//*1.5是对角度的适当放大，14*3.14/180是对角度零点的修正
complement_filter(acceler,gyro);
}
T1_INI()
{
TMOD|=0X10; //方式1,16位
//TCON1=0X00; 12分频
TH1=0Xf8;
TL1=0XFA;
TR1=1;
ET1=1; // 允许T1中断
}
//-------------------------------------------------------
//PWM输出
//-------------------------------------------------------
/*#define MA P4_0
#define MB P4_1
#define MC P4_2
#define MD P3_7*/
unsigned int PWM_10bit; // 10位 PWM 输出缓冲区
void PWM_output (int PWM_LH)
{
if (PWM_LH<0)
{
FW; //#define FW MA=1;MB=0;MC=1;MD=0
PWM_LH*=-1;
}
else
{
BW; //#define BW MA=0;MB=1;MC=0;MD=1
}
if (PWM_LH>252)
{
PWM_LH=252;
}
PWM_10bit=4*PWM_LH; //因为PWM原为8为pwm计算得来，这里重新定义10位pwm输出，所以，乘以4
//10位pwm获得方法参考网友Cortex-M0的http://bbs.21ic.com/frame.php?fr ... .com/iclist-11.html
}
static int speed_real_RH;
static int speed_real_LH;
//-------------------------------------------------------
//计算PWM输出值
//-------------------------------------------------------
static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot; //放大比例参数；这里是在main（）函数中对它们初始化
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;
//-------------------------------------------------------
void PWM_calculate(void)
{
K_angle_AD=ADC(2)*0.009;
K_angle_dot_AD=ADC(3)*0.009;
K_position_AD=ADC(4)*0.009;
K_position_dot_AD=ADC(5)*0.009;
position_dot=(speed_real_LH+speed_real_RH)*0.5;//position_dot是电机转速
position_dot_filter*=0.85; //车轮速度滤波
position_dot_filter+=position_dot*0.15;
position+=position_dot_filter;//position电机转过的角度，由position_dot累加而来
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是由四个参数累加而来，如果只要求平衡，可以不要position
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.8* 0.558;
K_angle=32* 25.6; //正常来讲这些放大比例参数是要按一定的方法算出来的，这里的平衡是调出来的就无所谓算不算了
K_position_dot=1.09* 55.8; //调平衡时如果想不考虑哪个参数的影响，设为0即可
K_angle_dot= 2*25.6;
for (i=1;i<=500;i++) //延时启动PWM，等待卡尔曼滤波器稳定
{
for (j=1;j<=300;j++);
}
while(1)
{
}
}
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 PWM_10(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;
void T1_isr(void) interrupt 3
{
TH1=0XE4;
TL1=0XFA;
AD_calculate();
PWM_calculate();
i++;
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;
}
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看，没有帐号？注册
x
|
[复制链接]
发表于 2012-2-24 23:56:05
微信
收藏
转播
分享
顶
踩
提升卡
变色卡
抢沙发
楼主
