#include<mega16.h>
//#include "iom16v.h"
#include<delay.h>
#include<stdio.h>
#include<lcd.h>
#include<math.h>
#include<string.h>
#include "I2C.h"
#include "Balance_Kalman.h"
//#include "HT_Kalman.h"
#include "Full_Kalman.h"
#include "TC1.h"
#asm
.equ __lcd_port=0x12//定义显示用PD口
#endasm
//#define uchar unsigned char
#define uint unsigned int
#define xtal 8000000
#define baud 9600
#define A_X_offset 118
#define G_Y_offset -2
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
//AD转换器寄存器的宏定义
#define REFS0 6
#define ADPS2 2
#define ADPS1 1
#define ADEN 7
#define ADSC 6
#define ADIF 4
//extern uchar MPU6050_outdata[N];
char str[7]="a"; //对str进行初始化
float fabs(float x);
void Dely_ms(uint xms)
{
uint i,j;
for(i=0;i<xms;i++)
{
for(j=0;j<1140;j++);
}
}
void int2str(int k2)
{
uint temp;
if(k2<0)
{
k2=-k2;
str[0]='-';
}
else str[0]=' ';
str[1]=k2/10000+0x30;
temp=k2%10000;
str[2]=temp/1000+0x30;
temp=k2%1000;
str[3]=temp/100+0x30;
temp=k2%100;
str[4]=temp/10+0x30;
str[5]=k2%10+0x30;
}
void AD_init()
{
PORTA=0;
DDRA=0; //ADC通道设置为高阻态
//ADMUX|=(1<<REFS0);//参考电压AVcc，结果右对齐，选择通道0
ADCSRA|=(1<<ADEN)|(1<<ADPS2)|(1<<ADPS1);//使能AD转换，ADC时钟64分频
}
uint AD_getdata(uchar k)
{
uint ADdata;
ADMUX=(1<<REFS0)|k;//参考电压AVcc，结果右对齐，选择通道k
ADCSRA|=(1<<ADSC); //开始AD转换
while(!(ADCSRA&(1<<ADIF)));
ADCSRA|=(1<<ADIF);
ADdata=ADCL+ADCH*256;
return ADdata;
}
void set_portc(uchar i)
{
PORTC|=(1<<i);
}
void rest_portc(uchar i)
{
PORTC&=~(1<<i);
}
void set_portb(uchar i)
{
PORTB|=(1<<i);
}
void rest_portb(uchar i)
{
PORTB&=~(1<<i);
}
void main()
{
char str_ax[6]="as";
uchar j=0,flag_ir_stop=0;
float position=0,motor_control,angle_mm,gyro_mm,speed=0,speed_fillter=0,speed_fillter_r=0,count_speed=0;
int k,kp_angle=107,kd_gyro=12,kp_speed=55,ki_angle=3;
float angle_zero=0,advance=0,turn=0,motor_control_r,motor_control_l,angle_error=0;
uint addata;
//UBRRL=xtal/(16*(baud-1));
//UBRRH=0;
//UCSRB=0x18;
//float matrix1[3][3]={0},matrix2[3][3]={0};
//Matrix_Mul333(3,3,3,matrix1,matrix2);
I2C_init();
//DDRD=0xff;
//PORTA=0;
Init_MPU6050();
read_MPU6050(ACCEL_XOUT_H);
k=((int)MPU6050_outdata[2*j]<<8)+MPU6050_outdata[2*j+1];
angle_mm=asin((float)(k-A_X_offset)/16384);//单位rad
AD_init();
INT2_init();
INT1_init();
timer2_init();
lcd_init(16);
lcd_clear();
//DDRD=0xff;
//PORTD=0xaf;
//调试
DDRB&=~(1<<3);//PB3设置为输入
PORTB|=0x08;//B3上拉电阻启用
DDRB|=(1<<1);//pb1设置为输出
while(PINB&0x08)
{
addata=AD_getdata(5); //启动ADc0数据
int2str(addata);
strcpy(str_ax,str);
lcd_gotoxy(0,0);
lcd_puts(str_ax);
kp_angle=addata;//kp参数调节
//OCR1A=addata; //左轮电机PWM控制
addata=AD_getdata(1); //启动ADc1数据
int2str(addata);
strcpy(str_ax,str);
lcd_gotoxy(9,0);//第1行9列显示
lcd_puts(str_ax);
kd_gyro=addata; //kd参数调节
//OCR1B=addata; //右轮电机PWM控制
addata=AD_getdata(2); //启动ADc2数据
int2str(addata);
strcpy(str_ax,str);
lcd_gotoxy(0,1);//第二行9列显示
lcd_puts(str_ax);
//ki_position=addata;
ki_angle=addata;
//motor_L_dead=addata;
//OCR1B=addata; //右轮电机PWM控制
//addata=AD_getdata(3); //启动ADc3数据
int2str(key1-125);
strcpy(str_ax,str);
lcd_gotoxy(9,1);//第2行9列显示 kp_speed
lcd_puts(str_ax);
kp_speed=key1-125;
}
//DDRA=0xff; //测周期
//PORTA=0;
while(!(PINB&0x08));
Dely_ms(5000);
timer1_init();
timer0_init();
INT0_init();
INT1_init();
//DDRB|=0x04;
//PORTB|=0x04;
DDRC|=0xf0;//PC口高四位输出状态
while(1)
{
if(flag_ir==1)
{
flag_ir=0;
switch(key)
{
case 232:angle_zero-=0.2;break;//按键CH-
case 248:angle_zero+=0.2;break;//按键CH+
case 206:advance+=4;break;//按键5
case 210:advance-=4;break;//按键8
case 208:turn+=200;break;//按键7
case 212:turn-=200;break;//按键9
case 228:flag_ir_stop=1;break;
case 240:flag_ir_stop=0;break;
case 216:advance=0;turn=0;break;//按键CH
default :break;
}
}
if(na>=8) //8ms
{
na=0;
//lcd_clear();
rest_portc(7);
read_MPU6050(ACCEL_XOUT_H);
//putchar(1<<7);
//for(j=0;j<N/2;j++)
//{
j=0;
k=((int)MPU6050_outdata[2*j]<<8)+MPU6050_outdata[2*j+1];
angle_mm=asin((float)(k-A_X_offset)/16384)*57.3;//求角度
int2str(angle_mm*10);
strcpy(str_ax,str);
// lcd_gotoxy(0,0);
//lcd_puts(str_ax);
if(k<0)
{
k=-k;
k=k+32768;
}
//putchar(k/256);
//putchar(k%256);
j=5;
k=-(((int)MPU6050_outdata[2*j]<<8)+MPU6050_outdata[2*j+1])-4;//取过反
gyro_mm=(float)k/32.8;
int2str(key2);
strcpy(str_ax,str);
//lcd_gotoxy(0,1);
//lcd_puts(str_ax);
if(k<0)
{
k=-k;
k=k+32768;
}
//putchar(k/256);
//putchar(k%256);
//开始卡尔曼
rest_portc(6);
Kalman(angle_mm,gyro_mm);
set_portc(6);
if(fabs(angle)>27|flag_ir_stop==1) //判断是否倾倒
flag_stop=0;
else
flag_stop=1;
if(fabs(angle)<0.3) //判断是否过零点
rest_portb(1);
else
set_portb(1);
int2str(angle*10);
int2str(key1);//na=0;
strcpy(str_ax,str);
//lcd_gotoxy(9,0);
//lcd_puts(str_ax);
//int2str(gyro);
//strcpy(str_ax,str);
//lcd_gotoxy(9,1);
//lcd_puts(str_ax);
/*
k=angle*100;
if(k<0)
{
k=-k;
k=k+32768;
}
//putchar(k/256);
//putchar(k%256);
k=gyro_mm;
if(k<0)
{
k=-k;
k=k+32768;
} */
// putchar(k/256);
//putchar(k%256);
if(++nt1>5) //80ms测速一次
{
speed_fillter=TCNT0;
speed_fillter_r=speed_r;
speed_r=0;
nt1=0;
TCNT0=0;
if(flag_direct_l==1)
speed_fillter=-speed_fillter;
if(flag_direct_r==1)
speed_fillter_r=-speed_fillter_r;
speed_fillter=(speed_fillter+speed_fillter_r)
