Atmega16+mpu6050平衡小车源码卡尔曼滤波 -

//方能仪器平衡车程序V2.0***************************
//Landforcer 方能仪器版权所有
/*使用到的单片机模块有：
1、一路定时器中断用于给定卡尔曼滤波周期，大约10ms
2、七路AD，其中ADC0用于检测转向电位器输出（备用扩展，小车有接口），
ADC1用于检测左轮电机的电流(备用)，
ADC2连接外部滑动变阻器RK0，用于调整小车的平衡角度，也就是调零，ADC3连接
RK1用于调节角度的比例系数，ADC4连接RK2用于调节角度的微分系数，ADC5连接
RK3用于调节位置的比例系数，ADC6连接RK4用于调节位置的微分系数，ADC7用于检测
右轮电机的电流(备用)
3、两路外部中断，用于检测编码器的输出脉冲数，进而测得车速
4、两路PWM输出，PWM驱动直流伺服电机
5、一般IO口，通过发送高低电平控制电机的正反转，
6、iic接口用于读取MPU6050六轴传感器的数据
使用到的外部硬件模块有：MPU6050六轴传感器
电机驱动模块，车轮检测编码器模块和主控板*/
//*************************AnyWay调试说明**********************//
/*RK0为小车的平衡角度调节，也就是零点调节，
RK1为角度比例系数，顺时针变小，逆时针变大，系数越大车子越有力，但是系数太大会导致抖动造成自激，建议大小为2.0V
RK2为角度微分系数，顺时针变小，逆时针变大，系数越大车子反应越灵敏，能一定程度上消除RK1导致的自激，但是不能太大，建议为3.6V
RK3为位置微分系数，顺时针变小，逆7时针变大，系数越大车子越稳定，但是太大会导致自激，建议为0.6V
RK4为位置比例系数，顺时针变小，逆时针变大，系数越大车子回到原点的速度越快，但是太大也会导致自激，建议为1.0V
*/
#include<avr/io.h>
#include<avr/interrupt.h>
//#include<avr/signal.h>
#include "Anyway_uart.h"
#include "Anyway_Kalman.h"
#include "Anyway_mup6050.h"
#include"Anyway_twi.h"
#include <math.h>
#define uchar unsigned char
#define uint unsigned int
/////////////////变量定义区
extern float angle, angle_dot;
volatile unsigned int AD_data;
volatile float PWM;
volatile unsigned char flager,reger;
volatile int speed_output_RH,speed_output_LH,NUMBER;
volatile int speed_real_LH,speed_real_RH;
volatile float gyro,acc,adc_v,angle_t;
volatile int Kp_angle,Kd_angle,Kd_position;
volatile float Kp_position;
volatile float position,position_dot;
volatile float position_dot_filter;
volatile int Turn_Need,Speed_Need;
unsigned char cdis[14]={0,}; //读缓存区
unsigned int aaa,ggg;
#define xtal 16 //以MHz为单位，不同的系统时钟要修改。
void delay_1ms(void)
{
unsigned int i;
for(i=0;i<(unsigned int)(xtal*143-2);i++);
}
void delay_nms(unsigned int num)
{
unsigned int i;
for(i=0;i<num;i++)
delay_1ms();
}
///////////////////中断初始化
void T1_initial(void)
{
TCCR1A|=(1<<COM1A1)|(1<<WGM10)|(1<<COM1B1); //T1：8位快速PWM模式、匹配时清0，TOP时置位
TCCR1B|=(1<<WGM12)|(1<<CS11); //PWM：8分频：16M/8/256=8KHz;
}
//-------------------------------------------------------
//定时器2初始化
void T2_initial(void) //T2：计数至OCR2时产生中断
{
OCR2=0x30;//0X5E; //T2：10ms时产生中断;
TIMSK|=(1<<OCIE2);
TCCR2|=(1<<WGM21)|(1<<CS22)|(1<<CS21)|(1<<CS20); //CTC模式，1024分频
}
//-------------------------------------------------------
//外部中断初始化
void INT_initial(void)
{
MCUCR|=(1<<ISC01)|(1<<ISC00)|(1<<ISC11)|(1<<ISC10); //INT0、INT1上升沿有效
GICR|=(1<<INT0)|(1<<INT1); //外部中断使能
}
///////////////////////ad转换
int mega16_ad(uchar Ch1)
{
uint addata;
ADMUX = 0x00 | Ch1;//这个地方注意设置，外部基准源
ADCSRA|=(1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);;
while(!(ADCSRA & (1 << ADIF)));
addata=ADCL;
addata=addata+ADCH*256;
// AD_data-=512;
ADCSRA &= ~(1 << ADIF);
ADCSRA &= ~(1 << ADEN);
return addata;
}
void AD_calculate(void)
{aaa=cdis[2]*256+cdis[3]+32768;
//acc=(mega16_ad(0)-mega16_ad(2))*3.286/800;
acc=(32768-aaa*1.0)/16384;//+(512-mega16_ad(2))*0.004;;
if(acc>1) ///这个地方必须限幅，不然会出问题
acc=1;
else if(acc<-1)
acc=-1;
acc=57.3*acc;
//gyro=-(mega16_ad(1)-380)*4.9;
ggg=cdis[10]*256+cdis[11]+32768;
gyro=(32768-ggg*1.0)/131;
angle_t=mega16_ad(7)/5.3;
//Kalman_Filter(acc,gyro);
angle=0.95*(angle)+0.05*acc;
angle_dot=gyro;
}
////////////////////////////////PWM发送
void PWM_output (int PWM_LH,int PWM_RH)
{
if (PWM_LH<0)
{
PORTB|=1<<7;
PWM_LH*=-1;
}
else
{
PORTB&=~(1<<7);
}
if (PWM_LH>252)
{
PWM_LH=252;
}
if (PWM_RH<0)
{
PORTB|=1<<6;
PWM_RH*=-1;
}
else
{
PORTB&=~(1<<6);
}
if (PWM_RH>252)
{
PWM_RH=252;
}
OCR1AH=0;
OCR1AL=PWM_LH; //OC1A输出；
OCR1BH=0;
OCR1BL=PWM_RH; //OC1B输出；
}
///////////////////////////////////PWM 计算
void PWM_calculate(void)
{
// PORTB|=(1<<6);
/* if (0x10==(PINB&1<<4) ) //前进
{
Speed_Need=-4;
}
else if (0x20==(PINB&1<<5) ) //后退
{
Speed_Need=4;
}
else //不动
{
Speed_Need=0;
}
*/
//position_dot=(speed_real_LH-speed_real_RH)*0.5; ///B
// position_dot=(-speed_real_LH-speed_real_RH)*0.5;///D
// position_dot=(speed_real_LH+speed_real_RH)*0.5; ///A
position_dot=(-speed_real_LH+speed_real_RH)*0.5; ///C
position_dot_filter*=0.85; //车轮速度滤波
position_dot_filter+=position_dot*0.15;
position+=position_dot_filter;
position+=Speed_Need;
position+=Turn_Need;
if (position<-768) //防止位置误差过大导致的不稳定
{
position=-768;
}
else if (position>768)
{
position=768;
}
PWM =-Kp_angle*angle-Kd_angle*angle_dot/*+Kp_position*position*/+Kd_position*(position_dot_filter-Speed_Need);
speed_output_RH = PWM+Turn_Need;
speed_output_LH = PWM-Turn_Need;
PWM_output (speed_output_LH,speed_output_RH);
}
////////////////////////////////////主程序
int main()
{//uchar a;
DDRB=0B11000000;
// PORTB=0;
DDRB &= ~(1<<0 | 1<<1|1<<2|1<<3|1<<4|1<<5);
PORTB |= 1<<0 | 1<<1|1<<2|1<<3|1<<4|1<<5;
DDRA=0;
PORTA=0;
DDRD=0B11110010;//这个地方不能加PIN，不然会出问题
DDRC |=((1<<PC0)|(1<<PC1));
PORTC|=((1<<PC0)|(1<<PC1));
TWBR=0x0a; //I2C 初始化，400kbps
TWSR|=(1<<TWPS0);
T1_initial();
T2_initial();
USART_Init();
// PIND=0X00;
// PORTD=0X00;
// PORTD|=~(1<<6);
// SFIOR|=1<<PUD;
INT_initial();
sei();
MPU_Init();
Kp_angle=mega16_ad(3)/10.0;
Kd_angle=mega16_ad(4)/250.0;
Kp_position=mega16_ad(5)/500.0;
Kd_position=mega16_ad(6)/5.0;
angle_t=mega16_ad(7)/5.3;
while(1)
{
if(flager==1)
{MPU_6050_Read(cdis);
AD_calculate();
/*flager=0;
AD_calculate();
PWM_calculate();
MPU_6050_Read(cdis);
speed_real_LH=0;//速度计数清零
speed_real_RH=0;
}*/
/*
Putc(0xfd);
Putc(0);
Putc(angle*2+120);
*/
Putc(0xff);
Putc(angle+128); //红
Putc(angle_t+10);//绿
Putc(acc+128); //蓝
}
}
return 0;
}
ISR(TIMER2_COMP_vect)//定时器2中断服务函数
{
flager=1;
NUMBER++;
//MPU_6050_Read(cdis);
//AD_calculate();
}
ISR(INT0_vect)
{
if (0==(PINB&1<<0))
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码