常驻嘉宾
- 积分
- 4068
- 威望
- 2252
- 贡献
- 1296
- 兑换币
- 1483
- 注册时间
- 2012-7-30
- 在线时间
- 260 小时
- 毕业学校
- 克莱德大学
|
- void main(){
- DisableInterrupts;
- /************初始化*******************/
- PIT_init(); //PIT定时中断初始化
- PWM_init(); //电机PWM输出初始化
- Coder_init(); //编码器脉冲读取初始化
- //CCD_init();
- ADC_init(); //加速度计\陀螺仪输入初始化
- // LCD_KEY_init();
- uart_init(UART0,115200); //初始化串口0
- // time_delay_ms(6000);
- /************************************/
- EnableInterrupts; //开总中断
- while(1)
- {
- Kalman_Filter(accelerometer_angle,gyroscope_rate);
- if(angle>10.0)
- printf("angle:%d \n",(u8)angle);
- //uart_putchar(UART0,(char)asin((accelerometer_angle-111.0)*0.009));
- //printf("gyroscope_rate:%d\n",(u16)(gyroscope_rate));
- time_delay_ms(100);
- }
- }
- float angle_zero=164.0; //平衡角度63
- float angle=0;
- void Kalman_Filter(float angle_m,float gyro_m)
- {
- angle_m=(accelerometer_angle-MID)/ACC; //sin值
- if(angle_m>1) //正弦值不能大于1或小于-1
- angle_m=1;
- else if(angle_m<-1)
- angle_m=-1;
- angle_m=180/3.14159*asin(angle_m);
- angle=angle_m;
- }
复制代码 发代码时用[code]标记可以保留缩进。
|
|