balence-car.rar_倒立摆

#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)