Atmega16+mpu6050平衡小車源碼卡爾曼濾波

ID:260817 · 發表于 2017-12-12 20:18

代碼測試通過，卡爾曼濾波
Anyway2.0_新驅動_客戶例程->default->Anyway_main.hex 此文件為燒寫代碼

單片機源程序如下:

//方能儀器平衡車程序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黑下載附件…………

復制代碼

所有資料51hei提供下載:

Anyway2.0_新驅動_客戶例程.zip (96.57 KB, 下載次數: 35)

ID:30708 · 發表于 2019-10-13 15:12

感謝上傳代碼

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

Atmega16+mpu6050平衡小車源碼 卡爾曼濾波

Atmega16+mpu6050平衡小車源碼卡爾曼濾波