设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › AVR单片机 › 二轮平衡机器人控制器代码
返回列表 发新帖
查看: 5735|回复: 8
收起左侧

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

[复制链接]
ID:80436 发表于 2015-5-22 01:06 | 显示全部楼层 |阅读模式



  3. //MCU：Mega16；晶振：8MHz；

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

  7. //二轮平衡机器人项目

  9. #include <iom16v.h>

  11. #include <macros.h>

  13. #include <math.h>

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

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

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

  21. //-------------------------------------------------------

  23. //输出端口初始化

  25. void PORT_initial(void)

  27. {

  29. DDRA=0B00000000;

  31. PINA=0X00;

  33. PORTA=0X00;

  35. DDRB=0B00000000;

  37. PINB=0X00;

  39. PORTB=0X00;

  41. DDRC=0B00010000;

  43. PINC=0X00;

  45. PORTC=0X00;

  47. DDRD=0B11110010;

  49. PIND=0X00;

  51. PORTD=0X00;

  53. }

  55. //-------------------------------------------------------

  57. //定时器1初始化

  59. void T1_initial(void)       

  61. {

  63. TCCR1A|=(1<<COM1A1)|(1<<WGM10)|(1<<COM1B1);        //T1：8位快速PWM模式、匹配时清0，TOP时置位

  65. TCCR1B|=(1<<WGM12)|(1<<CS11);        //PWM：8分频：8M/8/256=4KHz;

  67. }

  69. //-------------------------------------------------------

  71. //定时器2初始化

  73. void T2_initial(void)        //T2：计数至OCR2时产生中断

  75. {

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

  79. TIMSK|=(1<<OCIE2);

  81. TCCR2|=(1<<WGM21)|(1<<CS22)|(1<<CS21)|(1<<CS20);        //CTC模式，1024分频

  83. }

  85. //-------------------------------------------------------

  87. //外部中断初始化

  89. void INT_initial(void)

  91. {

  93. MCUCR|=(1<<ISC01)|(1<<ISC00)|(1<<ISC11)|(1<<ISC10);        //INT0、INT1上升沿有效

  95. GICR|=(1<<INT0)|(1<<INT1);        //外部中断使能

  97. }

  99. //-------------------------------------------------------

  101. //串口初始化；

  103. void USART_initial( void )

  105. {       

  107. UBRRH = 0X00;

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

  111. UCSRB = (1<<RXEN)|(1<<TXEN);        //接收器与发送器使能；

  113. UCSRC = (1<<URSEL)|(1<<UCSZ0)|(1<<UCSZ1);        //设置帧格式: 8 个数据位, 1 个停止位；

  115. UCSRB|=(1<<RXCIE);        //USART接收中断使能

  117. }

  119. //-------------------------------------------------------

  121. //串口发送数据；

  123. void USART_Transmit( unsigned char data )

  125. {

  127. while ( !( UCSRA & (1<<UDRE)));        //等待发送缓冲器为空；

  129. UDR = data;        //将数据放入缓冲器，发送数据；

  131. }

  133. //-------------------------------------------------------

  135. //串口接收数据中断，确定数据输出的状态；

  137. #pragma interrupt_handler USART_Receive_Int:12

  139. static char USART_State;

  141. void USART_Receive_Int(void)

  143. {

  145. USART_State=UDR;//USART_Receive();

  147. }

  149. //-------------------------------------------------------

  151. //计算LH侧轮速：INT0中断；

  153. //-------------------------------------------------------

  155. static int speed_real_LH;

  157. //-------------------------------------------------------

  159. #pragma interrupt_handler SPEEDLHINT_fun:2

  161. void SPEEDLHINT_fun(void)

  163. {

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

  167. {

  169. speed_real_LH-=1;

  171. }

  173. else

  175. {

  177. speed_real_LH+=1;

  179. }

  181. }

  183. //-------------------------------------------------------

  185. //计算RH侧轮速，：INT1中断；

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

  189. //-------------------------------------------------------

  191. static int speed_real_RH;

  193. //-------------------------------------------------------

  195. #pragma interrupt_handler SPEEDRHINT_fun:3

  197. void SPEEDRHINT_fun(void)

  199. {

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

  203. {

  205. speed_real_RH+=1;

  207. }

  209. else

  211. {

  213. speed_real_RH-=1;

  215. }

  217. }

  219. //-------------------------------------------------------

  221. //ADport采样：10位，采样基准电压Aref

  223. //-------------------------------------------------------

  225. static int AD_data;

  227. //-------------------------------------------------------

  229. int ADport(unsigned char port)

  231. {

  233. ADMUX=port;

  235. ADCSRA|=(1<<ADEN)|(1<<ADSC)|(1<<ADPS1)|(1<<ADPS0);        //采样频率为8分频；

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

  239. AD_data=ADCL;

  241. AD_data+=ADCH*256;

  243. AD_data-=512;

  245. return (AD_data);

  247. }

  249. //*

  251. //-------------------------------------------------------

  253. //Kalman滤波，8MHz的处理时间约1.8ms；

  255. //-------------------------------------------------------

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

  259. //-------------------------------------------------------

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

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

  265. static float P[2][2] = {

  267. { 1, 0 },

  269. { 0, 1 }

  271. };

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

  275. static const char C_0 = 1;

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

  279. //-------------------------------------------------------

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

  283. {

  285. angle+=(gyro_m-q_bias) * dt;

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

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

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

  293. Pdot[3]=Q_gyro;

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

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

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

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

  303. angle_err = angle_m - angle;

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

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

  309. E = R_angle + C_0 * PCt_0;

  311. K_0 = PCt_0 / E;

  313. K_1 = PCt_1 / E;

  315. t_0 = PCt_0;

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

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

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

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

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

  327. angle        += K_0 * angle_err;

  329. q_bias        += K_1 * angle_err;

  331. angle_dot = gyro_m-q_bias;

  333. }

  335. //*/

  337. /*

  339. //-------------------------------------------------------

  341. //互补滤波

  343. //-------------------------------------------------------

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

  347. //-------------------------------------------------------       

  349. static float bias_cf;

  351. static const float dt=0.01;

  353. //-------------------------------------------------------

  355. void complement_filter(float angle_m_cf,float gyro_m_cf)

  357. {

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

  361. bias_cf+=gyro_m_cf*0.002;

  363. angle_dot=gyro_m_cf-bias_cf;

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

  367. //加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；

  369. }

  371. */       

  373. //-------------------------------------------------------

  375. //AD采样；

  377. //以角度表示；

  379. //加速度计：1.2V=1g=90°；满量程：1.3V~3.7V；

  381. //陀螺仪：0.5V~4.5V=-80°~+80°；满量程5V=200°=256=200°；

  383. //-------------------------------------------------------

  385. static float gyro,acceler;

  387. //-------------------------------------------------------

  389. void AD_calculate(void)

  391. {

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

  395. gyro=ADport(3);       

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

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

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

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

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

  407. //complement_filter(acceler,gyro);

  409. }

  411. //-------------------------------------------------------

  413. //PWM输出

  415. //-------------------------------------------------------

  417. void PWM_output (int PWM_LH,int PWM_RH)

  419. {

  421. if (PWM_LH<0)

  423. {

  425. PORTD|=BIT(6);

  427. PWM_LH*=-1;

  429. }

  431. else

  433. {

  435. PORTD&=~BIT(6);

  437. }

  439. if (PWM_LH>252)

  441. {

  443. PWM_LH=252;

  445. }

  447. if (PWM_RH<0)

  449. {

  451. PORTD|=BIT(7);

  453. PWM_RH*=-1;

  455. }

  457. else

  459. {

  461. PORTD&=~BIT(7);

  463. }

  465. if (PWM_RH>252)

  467. {

  469. PWM_RH=252;

  471. }

  473. OCR1AH=0;

  475. OCR1AL=PWM_LH;        //OC1A输出；

  477. OCR1BH=0;

  479. OCR1BL=PWM_RH;        //OC1B输出；

  481. }

  483. //-------------------------------------------------------

  485. //计算PWM输出值

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

  489. //-------------------------------------------------------       

  491. //static int speed_diff,speed_diff_all,speed_diff_adjust;

  493. //static float K_speed_P,K_speed_I;

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

  497. static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;

  499. static float position,position_dot;

  501. static float position_dot_filter;

  503. static float PWM;

  505. static int speed_output_LH,speed_output_RH;

  507. static int Turn_Need,Speed_Need;

  509. //-------------------------------------------------------       

  511. void PWM_calculate(void)       

  513. {

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

  517. {

  519. Turn_Need=-40;

  521. }

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

  525. {

  527. Turn_Need=40;

  529. }

  531. else        //不转

  533. {

  535. Turn_Need=0;

  537. }

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

  541. {

  543. Speed_Need=-2;

  545. }

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

  549. {

  551. Speed_Need=2;

  553. }

  555. else        //不动

  557. {

  559. Speed_Need=0;

  561. }

  563. K_angle_AD=ADport(4)*0.007;

  565. K_angle_dot_AD=ADport(5)*0.007;

  567. K_position_AD=ADport(6)*0.007;

  569. K_position_dot_AD=ADport(7)*0.007;

  571. position_dot=PWM*0.04;

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

  575. position_dot_filter+=position_dot*0.1;       

  577. position+=position_dot_filter;

  579. //position+=position_dot;       

  581. position+=Speed_Need;

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

  585. {

  587. position=-768;

  589. }

  591. else if  (position>768)

  593. {

  595. position=768;

  597. }

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

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

  603. speed_output_RH = PWM;// - Turn_Need;

  605. speed_output_LH = - PWM;// - Turn_Need ;

  607. /*

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

  611. speed_diff_all+=speed_diff;

  613. speed_diff_adjust=(K_speed_P*speed_diff+K_speed_I*speed_diff_all)/2;

  615. */

  617. PWM_output (speed_output_LH,speed_output_RH);       

  619. }

  621. //-------------------------------------------------------

  623. //定时器2中断处理

  625. //-------------------------------------------------------

  627. static unsigned char temp;

  629. //-------------------------------------------------------

  631. #pragma interrupt_handler T2INT_fun:4

  633. void T2INT_fun(void)       

  635. {

  637. AD_calculate();

  639. PWM_calculate();

  641. if(temp>=4)        //10ms即中断；每秒计算：100/4=25次；

  643. {       

  645. if (USART_State==0X30)        //ASCII码：0X30代表字符'0'

  647. {

  649. USART_Transmit(angle*57.3+128);

  651. USART_Transmit(angle_dot*57.3+128);

  653. USART_Transmit(128);       

  655. }

  657. else if(USART_State==0X31)        //ASCII码：0X30代表字符'1'

  659. {

  661. USART_Transmit(speed_output_LH+128);

  663. USART_Transmit(speed_output_RH+128);       

  665. USART_Transmit(128);

  667. }

  669. else if(USART_State==0X32)        //ASCII码：0X30代表字符'2'

  671. {

  673. USART_Transmit(speed_real_LH+128);

  675. USART_Transmit(speed_real_RH+128);

  677. USART_Transmit(128);

  679. }

  681. else if(USART_State==0X33)        //ASCII码：0X30代表字符'3'

  683. {

  685. USART_Transmit(K_angle+128);

  687. USART_Transmit(K_angle_dot+128);

  689. USART_Transmit(K_position_dot+128);

  691. }       

  693. temp=0;       

  695. }

  697. speed_real_LH=0;

  699. speed_real_RH=0;       

  701. temp+=1;       

  703. }

  705. //-------------------------------------------------------

  707. int i,j;

  709. //-------------------------------------------------------

  711. void main(void)

  713. {

  715. PORT_initial();

  717. T2_initial();

  719. INT_initial();

  721. USART_initial ();

  723. SEI();

  725. K_position=0.8 * 0.209;        //换算系数：(256/10) * (2*pi/(64*12))=0.20944；//256/10:电压换算至PWM，256对应10V；

  727. K_angle=34 * 25.6;        //换算系数：256/10 =25.6；

  729. K_position_dot=1.09 * 20.9;        //换算系数：(256/10) * (25*2*pi/(64*12))=20.944；

  731. K_angle_dot=2.2 * 25.6;        //换算系数：256/10 =25.6；

  733. for (i=1;i<=500;i++)        //延时启动PWM，等待卡尔曼滤波器稳定

  735. {

  737. for (j=1;j<=300;j++);;

  739. }

  741. T1_initial();

  743. while(1)

  745. {

  747. ;

  749. }

  751. }
复制代码





回复

使用道具 举报

ID:82588 发表于 2015-6-10 10:06 | 显示全部楼层
先收藏再说,谢谢分享
回复

使用道具 举报

ID:82588 发表于 2015-6-14 11:25 | 显示全部楼层
先收藏备用,谢谢分享
回复

使用道具 举报

ID:65957 发表于 2017-4-13 13:11 | 显示全部楼层
好复杂，先备着。谢谢楼主
回复

使用道具 举报

ID:205843 发表于 2017-7-11 03:47 来自手机 | 显示全部楼层
谢谢分享，很好的学习资料
回复

使用道具 举报

ID:48413 发表于 2017-8-16 10:33 | 显示全部楼层
学习一下谢谢！！！！！！！！！！！！！！！！！！！
回复

使用道具 举报

ID:962686 发表于 2022-3-23 23:12 | 显示全部楼层
感觉有点复杂啊，有点头痛
回复

使用道具 举报

ID:1083996 发表于 2023-9-7 11:42 | 显示全部楼层
有没有防止电动车摩托车自行车这种，左右歪倒的东西，比如陀螺仪啥的；
网上貌似很多了，脚踢也不会倾倒
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表