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

1、2013年全国大学生电子设计竞赛简易旋转倒立摆及控制装置（C题）【本科组】2013年9月7日摘 要 本简易旋转倒立摆控制系统采用STC89C52单片机作为主控芯片，BTS7960模块作为电机驱动，用MPU-6050角度传感器检测摆杆位置，单片机用PID算法控制直流减速电机PWM信号精确控制电机的转速和角度，从而实现驱动电机带动旋转臂做往复旋转使摆杆摆动，在规定时间内实现摆角超过60，摆杆从自然下垂状态完成圆周运动和从165起始位置保持倒立，并且使用液晶屏显示摆杆角度和模式。本倒立摆系统用+12V开关电源给减速电机供电，用+5V稳压电源给单片机供电。最后的测试表明，本系统基本达到了基本要求。关键

2、词：单片机，角度传感器，直流减速电机，PID，液晶显示 目 录1系统方案11.1 主控芯片的选择11.2 角度检测模块选择21.3 电机的选择21.4 电机驱动的选择22系统理论分析与计算32.1 控制算法的分析32.2 摆杆角度的计算43电路与程序设计43.1电路的设计4系统总体框图43.1.2各模块电路原理图43.2程序的设计7程序功能描述与设计思路7程序流程图84测试方案与测试结果84.1测试方案84.2 测试条件与仪器94.3 测试结果及分析9测试结果(数据)9测试分析与结论9附录1：电路原理图10附录2：源程序11简易旋转倒立摆及控制装置（C题）【本科组】1系统方案本题要求设计并制作

3、一个简易旋转倒立摆控制系统，能实现摆杆摆过一定角度和做完成圆周运动，保持倒立的基本要求部分。发挥部分要求实现摆杆从自然状态摆起保持倒立并有一定额抗干扰能力，并且在摆杆保持倒立状态下旋臂可以单方向转过360以上。 图1-1旋转倒立摆模型图本系统利用角度传感器检测单摆摆动角度，利用单片机接收信号并控制直流减速电机来控制旋臂转动从而控制摆杆摆动。本系统主要由控制器模块、角度检测模块、电机模块、电机驱动模块、电源模块组成，下面分别论证这几个模块的选择。1.1 主控芯片的选择方案一：采用凌阳公司的SPCE061A单片机作为控制器的方案。该单片机I/O资源丰富，但是价格昂贵。方案二：使用FPGA，CPLD

4、等大规模可编程逻辑控制器件，其时钟频率高，运算速度很快，但不适合于该题目。 方案三：采用ATMEL公司的STC89C52。52单片机价格便宜，应用广泛，其功能足以实现本设计要求。综合考虑，我们选择方案三。1.2 角度检测模块选择方案一：使用电位器作为角度传感器，由于不同角度输出的电阻值不同，通过AD采样电阻两端电压，计算得到角度。 但是，对于一般的电位器，线性度较差，而对于线性度较好的电位器，如22HP-10等，价格较高。方案二：采用OMRON公司的E6B2-CWZ6C编码器，数字信号输出，采集方便，价格昂贵。方案三：采用MPU6050加速度传感器，数字输出（I2C/SPI ）、低功耗、紧凑型

5、电容式微机械加速度计，具有输出精确，体积小，工作可靠，各种标识清晰，扩展性好等特点。综合以上三种方案，选择方案三。1.3 电机的选择方案一：使用伺服电机作为执行元件，运行精确，能高速制动，惯量小，适合闭环控制。但价格昂贵，编程复杂。 方案二：采用57步步进电机控制平板的运动，最小步进角为1.8度，因此能实现转动的精确控制，但步进电机的震动动不利于摆杆的倒立平衡。方案三：采用直流减速电机控制平板，直流减速电机力矩大，转动速度快，反应灵敏，易于及时控制。综合考虑采用方案三。1.4 电机驱动的选择方案一：采用普通三极管作H桥式驱动电路。三极管作为一个典型的直流电机驱动电路。电路得名于“H桥式驱动电路

6、”是因为它的形状酷似字母H。4个三极管组成H的4条垂直腿，而电机就是H中的横杠。H桥式电机驱动电路包括4个三极管和一个电机。要使电机运转，必须导通对角线上的一对三极管。根据不同三极管对的导通情况，电流可能会从左至右或从右至左流过电机，从而控制电机的转向。此H桥电路结构简单，容易实现，但是驱动能力较小三极管容易发热，所以不采用此方案。方案二：采用L298N驱动电路。L298N是SGS公司的产品，内部包含4通道逻辑驱动电路，是一种两相和四相电机的专用驱动器，即内部含二个H桥的高电压大电流双全桥式驱动器，接收标准TTL逻辑电平信号，可驱动46V、2A一下的电机。PWM配合桥是驱动电路L298N，实现

7、直流电机调速，非常简单，调速范围大，但是L298N在驱动大功率电机时发热较严重容易烧坏芯片，所以不采用此方案。方案三：采用BTS7960驱动电路。BTS7960是应用于电机驱动的大电流半桥式高集成芯片，它带有一个P够到的高边MOSFET、一个N沟道的底边MOSGET和一个驱动IC。P沟道高边开关省去了电荷泵的需求，因而减小了EMI. 集成的驱动IC具有逻辑电平输入、电流诊断、斜率调节、死区时间产生和过温、过压、欠压、过流及短路保护的功能。BTS7960通态电阻典型值为16m，驱动电流可达43A。此方案能实现直流电机调速，调速范围大，驱动能力强且不易发热，所以采用BTS7960作为此电路的驱动电

8、路。2系统理论分析与计算2.1 控制算法的分析 倒立摆系统是一个比较复杂的，带有快速、高阶次、多变量、严重非线性绝对不稳定和非最小相位系统的机电系统。PID 控制是由反馈系统偏差的比例(P)、积分(I),微分(D)的线性组合构成的反馈控制律，由于它具有原理简单、直观易懂、易于工程实现、鲁棒性强等一系列优点，多年以来它一直是工业过程控制中应用最广泛的一类控制算法。其输入与输出的关系为： 图2-1a) 比例环节：成比例地反映控制系统的偏差信号e(t)，偏差一旦产生，控制器立即产生控制作用，以减小偏差。b) 积分环节：主要用于消除静差，提高系统的无差度。积分作用的强弱取决于积分时间常数T，T越大，积

9、分作用越弱，反之则越强。c) 微分环节：反映偏差信号的变化趋势，并能在偏差信号变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减少调节时间。根据经验先将控制器的参数放在某一数值上，直接在闭环控制系统中，根据实际现象调节各个参数，调出最优控制。2.2 摆杆角度的计算 角度传感器内部集成了加速度传感器和陀螺仪，可以用两种方法算出摆杆与竖直方向的夹角。用加速度传感器时，通过重力加速度算出其在x轴和y轴方向重力加速度分量，从而计算出摆杆与竖直方向的夹角。用陀螺仪时，通过检测陀螺仪在y轴方向的离心力对周围空气的压力，得出摆杆旋转的角速度，再对角速度进行积分，从而得出摆杆摆过的角

10、度。3电路与程序设计3.1电路的设计系统总体框图单片机对角度传感器检测到的角度进行PID运算，通过控制直流电机占空比从而控制旋臂的转速，进而控制摆臂旋转。系统总体框图如图3-1所示： BTS驱动 直流电机旋臂 摆杆单 片 机角度传感器 液晶屏显示 图3-1 系统总体框图3.1.2各模块电路原理图 本系统主要包括主控制模块、角度传感器模块、电机及其驱动模块、电源模块、液晶显示模块。1、 电机及其驱动模块电路图 图3-2 BTS电机驱动电路图BTS7960B是一款集成度很高的大电流半桥驱动芯片，该芯片内部集成了一个P沟道的高端MOSFET和一个N沟道的低端MOSFET，由于高端采用P沟道MOSFE

11、T，因此避免了充电泵的电磁干扰，提高了EMC能力。驱动电流可直接由输入逻辑电平控制，简化了电路设计，并提高了电路可靠性。持续的漏极电流高达40A，能允许PWM的频率从0Hz到25kHz之间任意变化，满足目标电机的各种驱动需求。通过状态标志位( IS位)，能实现电机状态诊断和电流采样，并具有各种保护功能。可通过外部电阻调节内部MOSFET 开关速率，以获得最优EMI。本系统采用四片BTS7960B做成一个驱动电路以最小的板极空间为高电流的PWM调速电机提供了一种较优的解决方案。2、传感器模块电路图 图3-3 传感器模块电路图MPU-60X0是全球首例9轴运动处理传感器。它集成了3轴MEMS陀螺仪

12、，3轴MEMs加速度计以及一个可扩展的数字运动处理器DMP（DigitalMotionProcessor），可用I2C接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其I2C或SPI接口输出一个9轴的信号（SPI接口仅在MPU-6000可用）。MPU-60X0对陀螺仪和加速度计分别用了三个16位的ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为250，500，1000，2000/秒（dps），加速度计可测范围为2，4，8，16g。本系统采用此角度传感器检测摆杆转过的角度。 3、液晶显示模块电路图 图3-4

13、 液晶显示模块电路图 用1602LCD液晶进行显示。LCD由于显示清晰，内容精确、显示信息较多且使用方便、速度快。结合设计方案要求实时的显示摆杆角度和控制模式。4、单片机控制模块电路图 图3-5 单片机控制模块电路图 STC89C52是STC公司生产的一种低功耗、高性能CMOS8位微控制器，具有 8K 在系统可编程Flash存储器。具有以下标准功能： 8k字节Flash，512字节RAM， 32 位I/O 口线，看门狗定时器，内置4KB EEPROM，MAX810复位电路，3个16 位定时器/计数器，4个外部中断，一个7向量4级中断结构（兼容传统51的5向量2级中断结构），全双工串行口。另外

14、STC89X52 可降至0Hz 静态逻辑操作，支持2种软件可选择节电模式。空闲模式下，CPU 停止工作，允许RAM、定时器/计数器、串口、中断继续工作。掉电保护方式下，RAM内容被保存，振荡器被冻结，单片机一切工作停止，直到下一个中断或硬件复位为止。最高运作频率35MHz，6T/12T可选。本系统采用AT89C52作为主控芯片，就采集到的角度值进行PID运算，控制PWM波的占空比而控制电机转速，从而控制摆杆摆动角度。3.2程序的设计程序功能描述与设计思路 通过单片机对角度传感器检测到的摆杆角度进行PID计算，进而控制输出PWM波占空比，调节电机转速，从而控制旋臂转速从而控制摆杆摆动。通过三个按

15、键设置三个模式，模式一实现摆杆尽快摆过60，模式二实现摆杆迅速摆过一周，模式三实现对摆杆倒立平衡的控制。另外用液晶屏显示摆杆的角度和当前使用的控制模式。程序流程图 开始 初始化 按键1 按键2 按键3 PID算法1 PID算法2 测角度 驱动电机 驱动电机 PID算法3 保持在80 摆一周 驱动电机 结束 结束 保持倒立 是 否 结束 图3-6 程序流程图4测试方案与测试结果4.1测试方案1、 硬件测试 用万用变测量开关电源输出电压为+10.4V，测得稳压电源电压为+5V,电机驱动模块输出电压为+10.2V，硬件电路工作正常。2、硬件软件联调用导线将各模块连接起来，将程序下载进单片机，通电测试

16、，如期实现了基本部分的要求。4.2 测试条件与仪器测试条件：检查多次，仿真电路和硬件电路必须与系统原理图完全相同，并且检查无误，硬件电路保证无虚焊。测试仪器：万用表，量角器。4.3 测试结果及分析测试结果(数据)1、 基本要求（1）部分测试数据： 表4-1 摆角达60以上数据 次数 1 2 3 4 5 时间(s) 1.02 1.09 1.11 1.01 0.87 摆动角度 72 95 110 83 752、 基本要求（2）部分测试数据： 表4-2 完成圆周运动数据 次数 1 2 3 4 5 时间(s) 0.53 0.58 0.63 0.90 0.713、 基本要求（3）部分测试数据： 表4-3

17、 保持摆杆倒立数据 次数 1 2 3 4 5 拉起位置 165 170 170 175 180保持时间(s) 0.64 1.12 1.21 2.05 2.08 旋臂转角 90 135 138 190 187测试分析与结论根据上述测试数据，本系统可以得出以下结论：1、 摆杆从自然下垂状态开始，驱动电机带动旋转臂做往复旋转，能够使摆角迅速超过60。2、 摆杆从自然下垂状态起，尽快增大摆杆摆动幅度，能迅速完成圆周运动。3、摆杆从自然下垂状态下，外力拉起摆杆至180位置，撤除外力同时启动旋转臂使摆杆保持倒立状态能达到两秒左右，期间旋转臂转动角度不大于90。综上所述，本设计达到设计基本要求的大部分，但控

18、制精度和稳定性还需要进一步完善。附录1：电路原理图附录2：源程序#include#include#include#include#include#define uchar unsigned char#define uint unsigned int/ 定义MPU6050内部地址#define SMPLRT_DIV 0x19 /陀螺仪采样率，典型值：x07(125Hz)#define CONFIG 0x1A /低通滤波频率，典型值：x06(5Hz)#define GYRO_CONFIG 0x1B /陀螺仪自检及测量范围，典型值：x18(不自检，deg/s)#define ACCEL_CONFIG

19、 0x1C /加速计自检、测量范围及高通滤波频率，典型值：x01(不自检，G，Hz)#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_

20、YOUT_H 0x45#define GYRO_YOUT_L 0x46#define GYRO_ZOUT_H 0x47#define GYRO_ZOUT_L 0x48#define PWR_MGMT_1 0x6B /电源管理，典型值：x00(正常启用)#define WHO_AM_I 0x75 /IIC地址寄存器(默认数值x68，只读)#define SlaveAddress 0xD0 /IIC写入时的地址字节数据，+1为读取sbit key1=P14;sbit key2=P15;sbit key3=P16;sbit PWM1=P22;sbit PWM2=P23;uchar mode;/模式/

21、uchar z,zz;/制动变量uint m,a;/PWM波占空比double angle;/角度bit dir;/正转与反转标志位typedef struct PID/结构体int SetPoint; /设定目标Desired Value long SumError; /误差累计double Proportion; /比例常数Proportional Const double Derivative; /微分常数Derivative Const int LastError; /Error-1 int PrevError; /Error-2 PID;static PID sPID;static

22、PID *sptr = &sPID;void IncPIDInit(void)/PID参数初始化sptr-SumError = 0;sptr-LastError = 0; /Error-1sptr-PrevError = 0; /Error-2sptr-Proportion = 0.5; /比例常数Proportional Const sptr-Derivative = 5; /微分常数Derivative Const sptr-SetPoint = 0; int IncPIDCalc(int NextPoint) register int iError, iIncpid; /当前误差iErr

23、or = sptr-SetPoint - NextPoint; /增量计算iIncpid = sptr-Proportion * iError /Ek项+ sptr-Derivative * sptr-PrevError; /Ek项/存储误差，用于下次计算sptr-PrevError = sptr-LastError; sptr-LastError = iError; /返回增量值return(iIncpid); void write_MUP6050_single(uchar address,uchar date)start_I2C();write_byte_I2C(SlaveAddress)

24、;/发送设备地址+写信号ack_I2C();write_byte_I2C(address);/内部寄存器地址ack_I2C();write_byte_I2C(date);/内部寄存器数据ack_I2C();stop_I2C();uchar read_MUP6050_single(uchar address)uchar date;start_I2C();write_byte_I2C(SlaveAddress);/发送设备地址+写信号ack_I2C();write_byte_I2C(address);/发送存储单元地址，从开始ack_I2C();start_I2C();write_byte_I2C

25、(SlaveAddress+1);/发送设备地址+读信号ack_I2C();date=read_byte_I2c();/读出寄存器数据s_ack_I2C(1);/发送非应答信号stop_I2C();return date;void init_MPU6050()/MUP6050初始化write_MUP6050_single(PWR_MGMT_1, 0x00);/解除休眠状态 write_MUP6050_single(SMPLRT_DIV, 0x07);/分频产生采样频率采样率=陀螺仪的输出率/(1+SMPLRT_DIV) write_MUP6050_single(CONFIG, 0x06);/滤

26、波器参数设置 write_MUP6050_single(GYRO_CONFIG, 0x18);/陀螺仪XYZ轴不自校验陀螺仪输出范围/s write_MUP6050_single(ACCEL_CONFIG, 0x01);/加速度计XYZ轴不自校验Full Scale Range 2g 高通滤波器的截止设置int GETdate(uchar address)/获取两个字节的原始数据合并int H,L;int date;H=read_MUP6050_single(address);L=read_MUP6050_single(address+1);date=(H*256)+L);switch(add

27、ress) case GYRO_XOUT_H:date+=14;break;case GYRO_YOUT_H:date-=1;break;case GYRO_ZOUT_H:date+=11;break;return date;char get_A_angle()/针向上，利用三轴求出偏转角度/double atan(double,double);即参数和返回值全为double类型char angle;double xx,yy,zz;xx=GETdate(ACCEL_XOUT_H);/获取X轴上的数据yy=GETdate(ACCEL_YOUT_H);zz=GETdate(ACCEL_ZOUT_H

28、);/angle=atan(zz/sqrt(xx*xx+yy*yy)*(double)180/3.14;angle=atan2(yy,sqrt(xx*xx+zz*zz)*(double)180/3.14;/atan返回的是弧度值return angle;/double get_G_angle_speed(uchar GY)/返回角速度/int a;/a=GETdate(GY)/16.4;/角速度，度/秒/if(angle0)/angle=angle;/return a;/void get_G_angle()/用角速度积分/angle+=get_G_angle_speed(GYRO_ZOUT_H

29、)*0.0048;/把每次读出的角速度累乘以时间累加起来/angle=get_A_angle()*0.1+angle*0.9;/void scan()/扫描按键if(key1=0)/模式一delayms(10);if(key1=0) mode=1; write_LCD_com(0x8a); write_LCD_data(mode+0x30);while(!key1);if(key2=0)/模式二delayms(10);if(key2=0)mode=2;write_LCD_com(0x8a);write_LCD_data(mode+0x30);while(!key2);if(key3=0)/模式三delayms(10);if(key3=0)mode=3;write_LCD_com(0x8a);write_LCD_data(mode+0x30);whil