二轮平衡机器人控制器代码

2019-11-09来源: 51hei关键字:二轮平衡机器人  控制器  Mega16

//MCU:Mega16;晶振:8MHz;


//PWM:4KHz;滤波器频率:100Hz;系统频率:100Hz;10ms;


//二轮平衡机器人项目


#include


#include


#include


//#define checkbit(var,bit)     (var&(0x01<<(bit)))     /*定义查询位函数*/


//#define setbit(var,bit)     (var|=(0x01<<(bit)))     /*定义置位函数*/


//#define clrbit(var,bit)     (var&=(~(0x01<<(bit)))) /*定义清零位函数*/


//-------------------------------------------------------


//输出端口初始化


void PORT_initial(void)


{


DDRA=0B00000000;


PINA=0X00;


PORTA=0X00;


DDRB=0B00000000;


PINB=0X00;


PORTB=0X00;


DDRC=0B00010000;


PINC=0X00;


PORTC=0X00;


DDRD=0B11110010;


PIND=0X00;


PORTD=0X00;


}


//-------------------------------------------------------


//定时器1初始化


void T1_initial(void)        


{


TCCR1A|=(1<


TCCR1B|=(1<


}


//-------------------------------------------------------


//定时器2初始化


void T2_initial(void)        //T2:计数至OCR2时产生中断


{


OCR2=0X4E;        //T2:计数20ms(0X9C)10ms(0X4E)时产生中断;


TIMSK|=(1<


TCCR2|=(1<


}


//-------------------------------------------------------


//外部中断初始化


void INT_initial(void)


{


MCUCR|=(1<


GICR|=(1<


}


//-------------------------------------------------------


//串口初始化;


void USART_initial( void )


{        


UBRRH = 0X00;


UBRRL = 51;        //f=8MHz;设置波特率:9600:51;19200:25;


UCSRB = (1<


UCSRC = (1<


UCSRB|=(1<



//-------------------------------------------------------


//串口发送数据;


void USART_Transmit( unsigned char data )


{


while ( !( UCSRA & (1<


UDR = data;        //将数据放入缓冲器,发送数据;



//-------------------------------------------------------


//串口接收数据中断,确定数据输出的状态;


#pragma interrupt_handler USART_Receive_Int:12


static char USART_State;


void USART_Receive_Int(void)


{


USART_State=UDR;//USART_Receive();


}


//-------------------------------------------------------


//计算LH侧轮速:INT0中断;


//-------------------------------------------------------


static int speed_real_LH;


//-------------------------------------------------------


#pragma interrupt_handler SPEEDLHINT_fun:2


void SPEEDLHINT_fun(void)


{


if (0==(PINB&BIT(0)))


{


speed_real_LH-=1;


}


else


{


speed_real_LH+=1;



}


//-------------------------------------------------------


//计算RH侧轮速,:INT1中断;


//同时将轮速信号统一成前进方向了;


//-------------------------------------------------------


static int speed_real_RH;


//-------------------------------------------------------


#pragma interrupt_handler SPEEDRHINT_fun:3


void SPEEDRHINT_fun(void)


{


if (0==(PINB&BIT(1)))


{


speed_real_RH+=1;


}


else


{


speed_real_RH-=1;



}


//-------------------------------------------------------


//ADport采样:10位,采样基准电压Aref


//-------------------------------------------------------


static int AD_data;


//-------------------------------------------------------


int ADport(unsigned char port)


{


ADMUX=port;


ADCSRA|=(1<


while(!(ADCSRA&(BIT(ADIF))));


AD_data=ADCL;


AD_data+=ADCH*256;


AD_data-=512;


return (AD_data);


}


//*


//-------------------------------------------------------


//Kalman滤波,8MHz的处理时间约1.8ms;


//-------------------------------------------------------


static float angle, angle_dot; //外部需要引用的变量


//-------------------------------------------------------


static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;


//注意:dt的取值为kalman滤波器采样时间;


static float P[2][2] = {


{ 1, 0 },


{ 0, 1 }


};


static float Pdot[4] ={0,0,0,0};


static const char C_0 = 1;


static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;


//-------------------------------------------------------


void Kalman_Filter(float angle_m,float gyro_m)        //gyro_m:gyro_measure


{


angle+=(gyro_m-q_bias) * dt;


Pdot[0]=Q_angle - P[0][1] - P[1][0];


Pdot[1]=- P[1][1];


Pdot[2]=- P[1][1];


Pdot[3]=Q_gyro;


P[0][0] += Pdot[0] * dt;


P[0][1] += Pdot[1] * dt;


P[1][0] += Pdot[2] * dt;


P[1][1] += Pdot[3] * dt;


angle_err = angle_m - angle;


PCt_0 = C_0 * P[0][0];


PCt_1 = C_0 * P[1][0];


E = R_angle + C_0 * PCt_0;


K_0 = PCt_0 / E;


K_1 = PCt_1 / E;


t_0 = PCt_0;


t_1 = C_0 * P[0][1];


P[0][0] -= K_0 * t_0;


P[0][1] -= K_0 * t_1;


P[1][0] -= K_1 * t_0;


P[1][1] -= K_1 * t_1;


angle        += K_0 * angle_err;


q_bias        += K_1 * angle_err;


angle_dot = gyro_m-q_bias;


}


//*/


/*


//-------------------------------------------------------


//互补滤波


//-------------------------------------------------------


static float angle,angle_dot; //外部需要引用的变量


//-------------------------------------------------------        


static float bias_cf;


static const float dt=0.01;


//-------------------------------------------------------


void complement_filter(float angle_m_cf,float gyro_m_cf)


{


bias_cf*=0.998;        //陀螺仪零飘低通滤波;500次均值;


bias_cf+=gyro_m_cf*0.002;


angle_dot=gyro_m_cf-bias_cf;


angle=(angle+angle_dot*dt)*0.90+angle_m_cf*0.05;        


//加速度低通滤波;20次均值;按100次每秒计算,低通5Hz;


}


*/        


//-------------------------------------------------------


//AD采样;


//以角度表示;


//加速度计:1.2V=1g=90°;满量程:1.3V~3.7V;


//陀螺仪:0.5V~4.5V=-80°~+80°;满量程5V=200°=256=200°;


//-------------------------------------------------------


static float gyro,acceler;


//-------------------------------------------------------


void AD_calculate(void)


{


acceler=ADport(2)+28;        //角度校正


gyro=ADport(3);        


acceler*=0.004069;        //系数换算:2.5/(1.2*512); // 5/(1.2*1024);5为参考电压5V;1.2V灵敏度对应加速度1g;1024为AD精度


acceler=asin(acceler);        //反正弦求角度


gyro*=0.00341;        //角速度系数:(3.14/180)* 100/512=0.01364;//(3.14/180)*    (200*0.025)/1024*0.025既5/1024*0.025        


//求得角速度 单位 角度/秒


Kalman_Filter(acceler,gyro);        //卡尔曼滤波 带入角度。角速度


//complement_filter(acceler,gyro);


}


//-------------------------------------------------------


//PWM输出


//-------------------------------------------------------


void PWM_output (int PWM_LH,int PWM_RH)


{


if (PWM_LH<0)


{


PORTD|=BIT(6);


PWM_LH*=-1;


}


else


{


PORTD&=~BIT(6);


}


if (PWM_LH>252)


{


PWM_LH=252;


}


if (PWM_RH<0)


{


PORTD|=BIT(7);


PWM_RH*=-1;


}


else


{


PORTD&=~BIT(7);


}


if (PWM_RH>252)


{


PWM_RH=252;


}


OCR1AH=0;


OCR1AL=PWM_LH;        //OC1A输出;


OCR1BH=0;


OCR1BL=PWM_RH;        //OC1B输出;


}


//-------------------------------------------------------


//计算PWM输出值


//车辆直径:76mm; 12*64pulse/rev; 1m=3216pulses;


//-------------------------------------------------------        


//static int speed_diff,speed_diff_all,speed_diff_adjust;


//static float K_speed_P,K_speed_I;


static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot;


static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;


static float position,position_dot;


static float position_dot_filter;


static float PWM;


static int speed_output_LH,speed_output_RH;


static int Turn_Need,Speed_Need;


//-------------------------------------------------------        


void PWM_calculate(void)        


{


if ( 0==(~PINA&BIT(1)) )        //左转


{


Turn_Need=-40;


}


else if ( 0==(~PINB&BIT(2)) ) //右转


{


Turn_Need=40;


}


else        //不转


{


Turn_Need=0;


}


if ( 0==(~PINC&BIT(0)) )        //前进


{


Speed_Need=-2;


}


else if ( 0==(~PINC&BIT(1)) )        //后退 


{


Speed_Need=2;


}


else        //不动


{


Speed_Need=0;


}


K_angle_AD=ADport(4)*0.007;


K_angle_dot_AD=ADport(5)*0.007;


K_position_AD=ADport(6)*0.007;


K_position_dot_AD=ADport(7)*0.007;


position_dot=PWM*0.04;


position_dot_filter*=0.9;        //车轮速度滤波


position_dot_filter+=position_dot*0.1;        


position+=position_dot_filter;


//position+=position_dot;        


position+=Speed_Need;


if (position<-768)        //防止位置误差过大导致的不稳定


{


position=-768;


}


else if  (position>768)


{


position=768;


}


PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD + 


K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD;


speed_output_RH = PWM;// - Turn_Need;


speed_output_LH = - PWM;// - Turn_Need ;


/*


speed_diff=speed_real_RH-speed_real_LH;        //左右轮速差PI控制;


speed_diff_all+=speed_diff;


speed_di

[1] [2]
关键字:二轮平衡机器人  控制器  Mega16 编辑:什么鱼 引用地址:http://news.eeworld.com.cn/mcu/ic479510.html 本网站转载的所有的文章、图片、音频视频文件等资料的版权归版权所有人所有,本站采用的非本站原创文章及图片等内容无法一一联系确认版权者。如果本网所选内容的文章作者及编辑认为其作品不宜公开自由传播,或不应无偿使用,请及时通过电子邮件或电话通知我们,以迅速采取适当措施,避免给双方造成不必要的经济损失。

上一篇:AVR单片机经典使用经验
下一篇:ATMEGA16L实现时间和温度的循环显示程序分享

关注eeworld公众号 快捷获取更多信息
关注eeworld公众号
快捷获取更多信息
关注eeworld服务号 享受更多官方福利
关注eeworld服务号
享受更多官方福利

推荐阅读

Qorvo推出新型电源应用控制器,适用于无刷直流电动工具
移动应用、基础设施与国防应用中核心技术与RF解决方案的领先供应商Qorvo®, Inc.(纳斯达克代码:QRVO)今日宣布,推出市场上性能最强大的新型电源应用控制器(PAC)系列PAC5xxx。该系列的单芯片SOC控制器可实现高效率、高性能和较长的电池寿命,适用于无刷直流(BLDC)电机供电工具。   Qorvo可编程电源管理业务部门总经理Larry Blackledge表示:“Qorvo 率先推出在一块芯片上集成高性能FLASH MCU的集成式可控制、可编程的智能栅极驱动器解决方案。通过PAC5527,OEM能够设计高度可靠的高性能工具,且占用空间极小。” Qorvo的新型PAC5527
发表于 2019-11-05
Qorvo推出新型电源应用控制器,适用于无刷直流电动工具
技术文章—采用降压型控制器产生负电压
引言 负电压被用于为汽车信息娱乐系统中数量越来越多的 LCD 显示屏供电。同样,在工业和铁路环境中,负电压轨可满足仪表和监视应用的需要。在所有的情况下,负电压轨均必须用正电源产生,但是正至负 IC 不像降压型控制器那样容易获得。制造商不太可能拥有经过测试的合格负输出转换器,却很可能已经有了一些经过核准的降压型控制器,例如 LTC3892 双输出控制器。为避免因测试专用负输出转换器招致额外的耗时和成本,可使用 LTC3892 双输出降压型控制器以 Ćuk 拓扑产生负输出电压。 双输出转换器:–12 V
发表于 2019-11-04
技术文章—采用降压型控制器产生负电压
三种现场总线隔离方法解析
共享,通过网络对现场设备和功能块的统一组态,天衣无缝的把不同制造商的网络及设备融为一体,构成统一的 FCS。  现场总线为什么要隔离目前大多数产品对外通讯部分可总结为:MCU+收发器+外部总线,其中大多数常用的 MCU 都集成有 CAN 或 UART 链路层控制器。从 MCU 发出的电平信号一般为 5V 或 3.3V,为达到与总线连接和远传的目的,往往需要在 MCU 与总线间加收发器,它起到电平转换的作用。     采用总线通信方式必然涉及到外部通信走线,CAN 和 458 总线往往需要做数百米的布线。总线越长、经过的环境越复杂越容易出现通信问题。外部环境中复杂多变的电磁场会间接抬高总线的电势,静电、浪涌、短路等会直接作用
发表于 2019-10-29
三种现场总线隔离方法解析
S3C2440的UART通信
1、UART原理简介在介绍2440的UART控制器之前,我们首先来了解一下UART的原理UART:Universal Asynchronous Receiver/Transmitter(通用异步收发送器),用来传输串行数据,发送数据时,CPU将并行数据写入UART,UART按照一定格式在TxD线上串行发出;接收数据时,UART检测到RxD线上的信号,将串行收集放到缓冲区中,CPU即可读取UART获得的这些数据。UART最精简的连线形式只有3根线,TXD用于发送,RXD用于接收,GND用于提供参考电平。UART之间以帧作为数据传输单位,帧由具有完整意义的若干位组成,它包含开始位、数据位、校验位和停止位。发送数据之前,互相通
发表于 2019-10-29
S3C2440的UART通信
平台加温控制器自动化测试系统
一个与火箭的角运动无关的导航坐标系,为加速度计提供可靠的测量基准,也为火箭姿态角的测量提供所需的坐标基准[1]。目前,平台加温控制器采用手动操作、人为监测的方式,数据记录不完善,设备维护不方便。  为了适应火箭密集发射的需要,火箭相关的测试设备需要进一步技术改造,平台加温控制器自动化测试系统(以下简称系统)利用当前测控领域的新技术,借助于现代计算机测控技术,完成对平台内腔的加温预热,结合表头温控实现平台二级温控,确保平台内陀螺和加速度计表头温度稳定。同时实时监测记录平台内腔以及陀螺、加速度计表头的电阻值。确保平台系统内陀螺和加速度计环境稳定。为火箭正常发射提供有力保障,也为平台异常情况分析提供可靠的数据依据。  1 系统设计  本系统
发表于 2019-10-29
平台加温控制器自动化测试系统
小广播
何立民专栏 单片机及嵌入式宝典

北京航空航天大学教授,20余年来致力于单片机与嵌入式系统推广工作。

电子工程世界版权所有 京ICP证060456号 京ICP备10001474号 电信业务审批[2006]字第258号函 京公海网安备110108001534 Copyright © 2005-2019 EEWORLD.com.cn, Inc. All rights reserved