简易旋转倒立摆及控制装置

1、 简易旋转倒立摆及控制装置设计报告及总结 摘要 倒立摆系统机理的研究不仅具有重要的理论价值，而且具有重要的现实意义，是控制类中经久不衰的经典题型。本题中，简易旋转倒立摆，在C8051F040单片机的基础上，使用ZGB42FM直流减速电机，BTN7971B电机驱动，可变电阻（角度传感器），机械摆杆等模块。通过编写、烧入程序，调控硬件协调工作，使摆杆首先实现一定角度的转动，再完成圆周运动，以及保持竖直向上的倒立状态。用以满足题目的基本要求,进而深一步探究倒立摆在保持运动姿态方面的发展与应用。关键字：单片机，倒立摆，摆杆，可变电阻。引言：本题整体上只由一个电机A提供动力，电机直接控制旋转臂C做往复旋

2、转运动，而通过转轴D连接在旋臂C上的摆杆E是非常灵活的。旋臂C转动一定角度时，摆杆E由于向心力会使摆杆E继续向上旋转，以达到E杆转动一个角度的效果。相似，当C的转动速度比较快，停下后，E下端处的速度和向心力都比较大，能够使E杆完成圆周运动。 为了使摆杆能够倒立，就要求摆杆转动到上半圆周面时，要通过单片机控制电机A不断的调整使旋转臂C转动多个角度，尽量的使摆杆E与竖直面的角度变小，并能够受力平衡，这样就可以保持一段时间的倒立状态。为达到角度的调整，就要测量出E杆与竖直面间的角度差，经过单片机的控制，使电机A做出相应的旋转动作，减小这个角度差。 1、方案设计与讨论： 1.1结构框图 单片机 执行电

3、机 电机驱动 角度传感器 摆杆 1.2方案论证： 1.21控制器模块 本题，单片机只要接收来自传感器的信号，向电机驱动输入信号处理后计算出的高低电平即可。 方案一：用ATMEL公司生产的AT89S52单片机，低功耗，高性能CMOS 8位处理器，使用广泛,算法较为简单，但是在执行复杂动作时，处理速度不够高。 方案二:用宏晶公司生产的STC89C52RC单片机，STC的单片机性能与ATMEL的单片机相似，但是价格相对便宜。缺点是易受潮湿影响，在调用子程序是频繁出错。 方案三:使用C8051F单片机该芯片与标准的8051芯片兼容，拥有高速指令处理能力，增加了中断源，复位源，内部有两个12位的ADC子

4、系统，有JTAG调试和边界扫描，片内集成的SPI接口，方便系统外设扩展。 经过讨论后选择更适合本题要求的方案三，使用C8051F单片机。 1.22电机模块 电机是本题的唯一动力装置，要求能够带动旋转臂C往复旋转一定角度，再使摆杆E发生摆动。 方案一、使用28BYJ-48 5V驱动的步进电机，步进电机可以满足角度旋转的要求，转速较慢，易于控制，但是扭力较小。 方案二、采用ZGB42FM直流减速电机，具有比一般直流电机更大的扭力和力矩，控制也相对简单。 方案三、采用无刷电机，三厢的无刷电机可以做到较为精确的转角。但是不易控制，输出小，易磨损，成本也相对较高。 在本题中，我们使用长厚木板作为旋转臂C

5、，对电机的输出要求较大。经比较采用了方案二，能够提供较大转速和扭力的直流减速电机ZGB42FM直流减速电机。 1.23电机驱动模块 方案一、使用人工焊接的H桥，可以使电机正常工作，但是自主焊接电路板可能会出现硬件问题。 方案二、用BTN7971B驱动模块，相当于一个半桥，也可以满足电机的驱动要求，安装空间也比较小。 在使用焊接的H桥时，MOS管升温很快，硬件部分存在隐患，所一最终我们选用了与H桥功能相同的方案二BTN电机驱动模块。 1.24传感器模块 本题中只需使用到角度传感器 方案一、使用MPU-6050模块，作为角度传感器，其集合程度高，免除了组合陀螺仪与加速器时之轴间差的问题，减少了大量

6、的包装空间，工作效果好。但程序复杂，成本较高。 方案二、使用可变电阻，耐磨损，寿命长，线性精度高。可变电阻同时具有与角度传感器相同的功能，成本也相对较低。 所以经过比较，选择方案二，一个可变电阻即可。1.3设计实现： 硬件的设计与制作 除了以上论证后选用的模块，对于机械方面的部分的设计， 将电机套在一块用树脂胶做成的模具上，装上用以固定，用一根厚长条形的木板做旋转臂C，摆杆E也以轴接的方式连接在旋转臂边缘的WDD35D4的转轴上，不影响E的灵活性。摆杆E同样是金属质地，为的是保证E的质量，使其能够有较大的惯性和向心力。在方案的实施过程中，首先使用普通的直流电机进行测试，与作为旋转臂的厚长木条连

7、接后电机转动很慢，未提供较大的扭力和，最终选用了ZGB42FM直流减速电机，解决动力提供不足的问题。木条与电机转轴的一端不容易接上，最终使用树脂胶将木条与电机转轴固定在一起，以达到运动状态一致。在测试电机时，尝试使用自己焊接的H桥。电机短暂工作后，发现MOS管发烫，在用新的MOS管焊接成H桥后测试时依然温度过高，为避免出现硬件问题，选择装上BTN7971B电机驱动模块。最终将电机固定在一块平面木板上，将木板钉在板凳的四只并且在运转时保持稳定的状态2.理论分析与计算 2.1电机的选择 我们选择了ZGB42FM直流减速电机，额定电压12V，转速300d/分，该电机本身转速和扭力都比较大，驱动重物也

8、可以轻松运转。2.2摆杆状态检测 是由空心金属棒制成，与轴承紧密连接，当摆杆被转到九十度后松手，摆杆可在自身重力的作用下做单摆运动直至停下来回摆动超过三个往复运动，最后保持着竖直向下的状态。2.3驱动在该题中，我们采用了BTN7971B电机驱动，在接收到单片机输出的高低电平后，驱动会驱使电机做出相应的运转动作。2.4控制算法 控制算法是单片机控制的重要组成部分，合适的算法能够简单，高效的解决问题。本题中，由于需要不断的调整旋转臂的转角，使用PID算法比较合适。PID控制器本身是一种基于对“过去”、“现在”和“未来”信息估计的简单控制算法。u1(t) u(t) -+r(t) 积分比例微分被控对象

9、c(t) +y(t) v(t) PID控制系统原理框图该系统主要由PID控制器和被控对象组成。作为一种线性控制器，它根据设定值r(t)和实际输出值y(t)构成控制偏差e(t)，将偏差按比例、积分和微分通过线性组合构成控制量u(t)，对被控对象进行控制。控制器的输入输出关系可描述为: 式中:e(t)=r(t)-u(t), K为比例系数T,为积分时间常数、T为微分时间常数。使用位置式PID控制算法。如果基于用求和代替积分，用差分代替微分来进行离散化，那么，离散化之后的数字量的PID控制算式为经z变换后，数字PID校正器的脉冲传递函数的一般形式为：3.电路图及原理图见附录1 主程序流程图及PID子程

10、序流程图见附录2 程序见附录4 54.测量方案及测试结果 测量仪器：电脑，MATLAB 软件。 测量方案： 利用MATLAB仿真，通过建模将非线性关系转化成线性关系。分别测出旋转臂的角度和摆杆角度的仿真曲线。测量结果见附录35.讨论与总结 5.1 简易旋转倒立摆及控制装置的完成情况 题目完成情况 未完成原因基础一。 摆杆从处于自然下垂状态（摆角 0°）开始，驱动电机带动旋转臂作往复旋转使摆杆摆动，并尽快使摆角达到或超过-60° +60°完成基础二。 从摆杆处于自然下垂状态开始，尽快增大摆杆的摆动幅度，直至完成圆周运动完成基础三。 在摆杆处于自然下垂状态下，外力拉起

11、摆杆至接近 165°位置，外力撤除同时，启动控制旋转臂使摆杆保持倒立状态时间不少于 5s；期间旋转臂的转动角度不大于 90°完成发挥一。 从摆杆处于自然下垂状态开始，控制旋转臂作往复旋转运动，尽快使摆杆摆起倒立，保持倒立状态时间不少于 10s未完成硬件制作粗糙，稳定性差，程序难以调整出合适的数据去维持稳定状态。发挥二。 在摆杆保持倒立状态下，施加干扰后摆杆能继续保持倒立或 2s 内恢复倒立状态；未完成发挥三。 在摆杆保持倒立状态的前提下，旋转臂作圆周运动，并尽快使单方向转过角度达到或超过 360°未完成5.2心得体会： 比赛完后，我们各组都总结了一下，认为，赛前准

12、备很不充分，要用的元器件多有短缺，有些器件只有一两件。在调试过程中产生损耗后，还要去网购，浪费时间。同时，组员间的配合不紧密，工作起来七手八脚，乱糟糟的。工作态度不严谨，硬件制作粗糙，工具随意乱丢，用起来却找不着。 出现的这些问题就是我们今后改进的方向，在赛前要充分准备，多准备元器件用以备用，工具设备也要准备充足以方便工作。同时加强训练，让参赛队员严肃，认真地对待比赛，熟悉队友，做到默契分工合作。6. 参考文献： 黄南晨旋转式倒立摆系统的设计与实现硕士学位论文】，中国科学技术大学控制理论与控制工程专业，2002 管凤旭倒立摆系统的可拓控制策略及实验研究【硕士学位论文】，哈尔滨工程大学控制理论与

13、控制工程专业，2006 黄辉基于可拓控制的仿真研究硕士学位论文】，南京理工大学控制理论与控制工程专业，2003 王俊基于倒立摆的三种控制策略的研究硕士学位论文】，湖北工业大学电力电子与电力传动专业，2008 李晓丹模糊PID控制器的设计研究陬士学位论文】，天津大学控制理论与控制工程专业，2005王晓明电动机的单片机控制北京航空航天大学出版社(第2版)，2007 江晨，王富东旋转式倒立摆系统的算法研究及仿真工业控制计算机2010附录1：C8051F040单片机 BTN7971B驱动模块 开始PID子程序附录2 开始 计算偏差 E(n)=R(n)-Y(n) 程序初始化 计算比例项 P-term=K

14、p*E(n)读入Kp,Ki,Kd 计算积分项并限幅 I-term=Ki*Ts*sumE判断PID-flag 是否为 1 NO YES 计算积分项并限幅 D-term=Kd*(E(n)-E(n-2)/(2*Ts)调用数据采集处理子程序-调用PID子程序 计算输出控制量C(n) C(n)=P_term+I_term+D_term将PID_flag位清0调用电机控制子程序主程序流程图PID子程序结束 PID子程序流程图 附录3旋转臂转动角度和摆杆转动角度的仿真曲线 附录4#include <c8051F040.h> / SFR declarations#include<math.h

15、>/-/ 16-bit SFR Definitions for 'F04x/-sfr16 ADC0 = 0xbe; / ADC0 datasfr16 RCAP2 = 0xca; / Timer2 capture/reloadsfr16 RCAP3 = 0xca; / Timer3 capture/reloadsfr16 TMR2 = 0xcc; / Timer2sfr16 TMR3 = 0xcc; / Timer3sbit PWM = P00; / LED='1' means ONsbit SW1 = P37; / SW1='0' means sw

16、itch pressedsbit P4_0 = P40; / LED='1' means ONsbit P4_1 = P41; / SW1='0' means switch pressed/-/ Global Constants/-#define BAUDRATE 115200 / Baud rate of UART in bps#define SYSCLK 24500000 / System Clock#define SAMPLE_RATE 64 / Sample frequency in Hz#define INT_DEC 256 / Integrate a

17、nd decimate ratio#define SAR_CLK 2500000 / Desired SAR clock speed#define TIMER0_RELOAD_HIGH (65535-40)/256#define TIMER0_RELOAD_LOW (65535-40)%256#define a_moto_go P4_1=0; /左边两个电机向前走#define a_moto_back P4_1=1; /左边两个电机向后转/#define a_moto_Stop P4_0=0,P4_1=0; /左边两个电机停转 #define a_moto_pwm P4_0 /PWM信号端bi

18、t a_moto_stop =1;bit flag=1;int en0,en1,en2, DATA;int SumE,d_term,i_term;long Cn;unsigned char K,Ki,Kd;unsigned char PID_flag,motor_flag;/*typedef struct PID uint16 SetPoint ;/设定目标uint16 Proportion ;/比例常数uint16 Integral ; /积分常数uint16 Derivative ;/微分常数uint16 LastError ; /Error-1uint16 PrevError ; /Er

19、ror-2 PID ;*/-/ Function Prototypes/-void OSCILLATOR_Init (void); void PORT_Init (void);void UART1_Init (void);void ADC0_Init (void);void TIMER3_Init (int counts);void ADC0_ISR (void);void Wait_MS (unsigned int ms);void PCA0_Init (void);void Timer0_Init (void);void pwm_out_a_moto();void motor();void

20、 PID();/-/ Global Variables/-unsigned short Result; / ADC0 decimated valuelong delay_count = 0;unsigned char pwm_val_a =0;/变量定义unsigned char push_val_a =0;/ 左电机占空比N/10/-/ main() Routine/- void run(void) / push_val_a=3; /速度调节变量 0-9。9最小，0最大 P4_1=0; /左电机往前走 void backrun(void) / push_val_a=3; /速度调节变量 0-

21、9。9最小，0最大 P4_1=1 ; /左电机往前走/*/延时函数 void delay(unsigned int k) unsigned int x,y; for(x=0;x<k;x+) for(y=0;y<2000;y+);void main (void) WDTCN = 0xde; / Disable watchdog timer WDTCN = 0xad; OSCILLATOR_Init (); / Initialize oscillator PORT_Init (); / Initialize crossbar and GPIO Timer0_Init(); TIMER3

22、_Init (SYSCLK/SAMPLE_RATE); / Initialize Timer3 to overflow at / sample rate ADC0_Init (); / Init ADC PCA0_Init (); P4 = 0x00; SFRPAGE = ADC0_PAGE; AD0EN = 1; / Enable ADCResult = 0; EA = 1; / Enable global interruptsdelay(50);SFRPAGE = CONFIG_PAGE; K=75;Ki=70;Kd=2;en0=en1=en2=0;Cn=i_term=d_term=0;S

23、umE=0;motor_flag=0; while (1) run();delay(300);backrun();delay(320);/*PID();*/ /-/ Initialization Subroutines/-/-/ OSCILLATOR_Init/-/ Return Value : None/ Parameters : None/ This function initializes the system clock to use the internal oscillator/ at 24.5 MHz./-void OSCILLATOR_Init (void) char SFRP

24、AGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = CONFIG_PAGE; / Set SFR page OSCICN = 0x83; / Set internal oscillator to run / at its maximum frequency CLKSEL = 0x00; / Select the internal osc. as / the SYSCLK source SFRPAGE = SFRPAGE_SAVE; / Restore SFR page/-/ PORT_Init/-/ Return Value : None

25、/ Parameters : None/ This function configures the crossbar and GPIO ports./ P0.4 digital push-pull UART TX/ P0.5 digital open-drain UART RX/ P1.6 digital push-pull LED/ AIN0.1 analog Analog input /-void PORT_Init (void) char SFRPAGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = CONFIG_PAGE; / se

26、t SFR page XBR0 = 0x08; /XBR0 = 0x00; XBR1 = 0x00; XBR2 = 0x40; / Enable crossbar and weak pull-up / Enable UART1 P0MDOUT |= 0x01; / Set CEX0 (P0.0) to push-pull P4MDOUT |= 0x03; P7MDOUT |= 0xff; SFRPAGE = SFRPAGE_SAVE; / Restore SFR page/-/ ADC0_Init/-/ Return Value : None/ Parameters : None/ Confi

27、gure ADC0 to use Timer3 overflows as conversion source, to/ generate an interrupt on conversion complete, and to use right-justified/ output mode. Enables ADC end of conversion interrupt. Leaves ADC disabled./-void ADC0_Init (void) char SFRPAGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = ADC0_

28、PAGE; ADC0CN = 0x04; / ADC0 disabled; normal tracking / mode; ADC0 conversions are initiated / on overflow of Timer3; ADC0 data is / right-justified REF0CN = 0x03; / Enable on-chip VREF, / and VREF output buffer AMX0CF = 0x00; / AIN inputs are single-ended (default) AMX0SL = 0x01; / Select AIN0.1 pi

29、n as ADC mux input ADC0CF = (SYSCLK/SAR_CLK) << 3; / ADC conversion clock = 2.5MHz, Gain=1 EIE2 |= 0x02; / enable ADC interrupts SFRPAGE = SFRPAGE_SAVE; / Restore SFR page/-/ TIMER3_Init/-/ Return Value : None/ Parameters :/ 1) int counts - calculated Timer overflow rate/ range is postive rang

30、e of integer: 0 to 32767/ Configure Timer3 to auto-reload at interval specified by <counts> (no/ interrupt generated) using SYSCLK as its time base./-void TIMER3_Init (int counts) char SFRPAGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = TMR3_PAGE; TMR3CN = 0x00; / Stop Timer3; Clear TF3;

31、 TMR3CF = 0x08; / use SYSCLK as timebase RCAP3 = -counts; / Init reload values TMR3 = RCAP3; / Set to reload immediately EIE2 &= 0x01; / Disable Timer3 interrupts TR3 = 1; / start Timer3 SFRPAGE = SFRPAGE_SAVE; / Restore SFR pagevoid PCA0_Init (void) char SFRPAGE_save = SFRPAGE; / Save current S

32、FR Page SFRPAGE = PCA0_PAGE; / configure PCA time base; overflow interrupt disabled PCA0CN = 0x00; / Stop counter; clear all flags PCA0MD = 0x04; / Use SYSCLK as time base PCA0CPM0 = 0x42; / Module 0 = 8-bit PWM mode / Configure initial PWM duty cycle = 50% PCA0CPH0 = 256 - (256 * 0.3); / Start PCA

33、counter CR = 1; SFRPAGE = SFRPAGE_save;void Timer0_Init(void) char SFRPAGE_SAVE = SFRPAGE; / Save Current SFR page SFRPAGE = TIMER01_PAGE; / Set SFR page TH0 = TIMER0_RELOAD_HIGH; / Init Timer0 High register TL0 = TIMER0_RELOAD_LOW ; / Init Timer0 Low register TMOD = 0x01; / Timer0 in 16-bit mode CK

34、CON = 0x02; / Timer0 uses a 1:48 prescaler ET0 = 1; / Timer0 interrupt enabled TCON = 0x10; / Timer0 ON SFRPAGE = SFRPAGE_SAVE; / Restore SFR page/*/* PWM调制电机转速 */*/* 左电机调速 */*调节push_val_a的值改变电机转速,占空比 */void pwm_out_a_moto(void) if(a_moto_stop) if(pwm_val_a<=push_val_a) a_moto_pwm=1; else a_moto_

35、pwm=0; if(pwm_val_a>=10) pwm_val_a=0; else a_moto_pwm=0; void PID() i_term=d_term=0; en0=1990-Result; SumE=SumE+en0; /计算误差项之和 if(SumE>10000) /检测sumE是否太大 SumE=10000;if(SumE<-10000) /检测sumE是否太小 SumE=-10000;i_term=SumE>>6;/计算sumE/Tf Tf为64HZi_term*=Ki; /计算积分项 d_term=en0-en2;if(d_term>60) /检测是否太大 d_term=60;if(d_term<-60) /检测是否太小 d_term=-60;d_term*=Kd;d_term*=32;/检测微分项if(d_term>2000) d_term=2000;if(d_term<-2000) d_term=-2000; /比例