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

简易旋转倒立摆及控制装置设计报告及总结
摘要
倒立摆系统机理的研究不仅具有重要的理论价值，而且具有重要的现实意义，是控制类中经久不衰的经典题型。

本题中，简易旋转倒立摆，在C8051F040单片机的基础上，使用ZGB42FM直流减速电机，BTN7971B电机驱动，可变电阻（角度传感器），机械摆杆等模块。

通过编写、烧入程序，调控硬件协调工作，使摆杆首先实现一定角度的转动，再完成圆周运动，以及保持竖直向上的倒立状态。

用以满足题目的基本要求,进而深一步探究倒立摆在保持运动姿态方面的发展与应用。

关键字：单片机，倒立摆，摆杆，可变电阻。

引言：本题整体上只由一个电机A 提供动力，电机直接控制旋转臂C 做往复旋
转运动，而通过转轴D 连接在旋臂C 上的摆杆E 是非常灵活的。

旋臂C 转动一定角度时，摆杆E 由于向心力会使摆杆E 继续向上旋转，以达到E 杆转动一个角度的效果。

相似，当C 的转动速度比较快，停下后，E 下端处的速度和向心力都比较大，能够使E 杆完成圆周运动。

为了使摆杆能够倒立，就要求摆杆转动到上半圆周面时，要通过单片机控制电机A 不断的调整使旋转臂C 转动多个角度，尽量的使摆杆E 与竖直面的角度变小，并能够受力平衡，这样就可以保持一段时间的倒立状态。

为达到角度的调整，就要测量出E 杆与竖直面间的角度差，经过单片机的控制，使电机A 做出相应的旋转动作，减小这个角度差。

1、方案设计与讨论： 1.1结构框图
1.2方案论证： 1.21控制器模块
本题，单片机只要接收来自传感器的信号，向电机驱动输入信号处理后计算出的高低电平即可。

方案一：用ATMEL 公司生产的AT89S52单片机，低功耗，高性能CMOS 8位处理器，使用广泛,算法较为简单，但是在执行复杂动作时，处理速度不够高。

方案二:用宏晶公司生产的STC89C52RC 单片机，STC 的单片机性能与ATMEL 的单片机相似，但是价格相对便宜。

缺点是易受潮湿影响，在调用子程序是频繁出错。

方案三:使用C8051F 单片机该芯片与标准的8051芯片兼容，拥有高速指令处理能力，增加了中断源，复位源，内部有两个12位的ADC 子系统，有JTAG 调试和边界扫描，片内集成的SPI 接口，方便系统外设扩展。

单片机
电机驱动
执行电机
摆杆
角度传感器
经过讨论后选择更适合本题要求的方案三，使用C8051F单片机。

1.22电机模块
电机是本题的唯一动力装置，要求能够带动旋转臂C往复旋转一定角度，再使摆杆E发生摆动。

方案一、使用28BYJ-48 5V驱动的步进电机，步进电机可以满足角度旋转的要求，转速较慢，易于控制，但是扭力较小。

方案二、采用ZGB42FM直流减速电机，具有比一般直流电机更大的扭力和力矩，控制也相对简单。

方案三、采用无刷电机，三厢的无刷电机可以做到较为精确的转角。

但是不易控制，输出小，易磨损，成本也相对较高。

在本题中，我们使用长厚木板作为旋转臂C，对电机的输出要求较大。

经比较采用了方案二，能够提供较大转速和扭力的直流减速电机——ZGB42FM直流减速电机。

1.23电机驱动模块
方案一、使用人工焊接的H桥，可以使电机正常工作，但是自主焊接电路板可能会出现硬件问题。

方案二、用BTN7971B驱动模块，相当于一个半桥，也可以满足电机的驱动要求，安装空间也比较小。

在使用焊接的H桥时，MOS管升温很快，硬件部分存在隐患，所一最终我们选用了与H桥功能相同的方案二BTN电机驱动模块。

1.24传感器模块
本题中只需使用到角度传感器
方案一、使用MPU-6050模块，作为角度传感器，其集合程度高，免除了组合陀螺仪与加速器时之轴间差的问题，减少了大量的包装空间，工作效果好。

但程序复杂，成本较高。

方案二、使用可变电阻，耐磨损，寿命长，线性精度高。

可变电阻同时具有与角度传感器相同的功能，成本也相对较低。

所以经过比较，选择方案二，一个可变电阻即可。

1.3设计实现：
硬件的设计与制作
除了以上论证后选用的模块，对于机械方面的部分的设计，将电机套在一块用树脂胶做成的模具上，装上用以固定，用一根厚长条形的木板做旋转臂C，摆杆E也以轴接的方式连接在旋转臂边缘的WDD35D4的转轴上，不影响E的灵活
性。

摆杆E同样是金属质地，为的是保证E的质量，使其能够有较大的惯性和向心力。

在方案的实施过程中，首先使用普通的直流电机进行测试，与作为旋转臂的厚长木条连接后电机转动很慢，未提供较大的扭力和，最终选用了ZGB42FM直流减速电机，解决动力提供不足的问题。

木条与电机转轴的一端不容易接上，最终使用树脂胶将木条与电机转轴固定在一起，以达到运动状态一致。

在测试电机时，尝试使用自己焊接的H桥。

电机短暂工作后，发现MOS管发烫，在用新的MOS管焊接成H桥后测试时依然温度过高，为避免出现硬件问题，选择装上BTN7971B电机驱动模块。

最终将电机固定在一块平面木板上，将木板钉在板凳的四只并且在运转时保持稳定的状态
2.理论分析与计算
2.1电机的选择
我们选择了ZGB42FM直流减速电机，额定电压12V，转速300d/分，该电机本身转速和扭力都比较大，驱动重物也可以轻松运转。

2.2摆杆状态检测
是由空心金属棒制成，与轴承紧密连接，当摆杆被转到九十度后松手，摆杆可在自身重力的作用下做单摆运动直至停下来回摆动超过三个往复运动，最后保持着竖直向下的状态。

2.3驱动
在该题中，我们采用了BTN7971B电机驱动，在接收到单片机输出的高低电平后，驱动会驱使电机做出相应的运转动作。

2.4控制算法
控制算法是单片机控制的重要组成部分，合适的算法能够简单，高效的解决问题。

本题中，由于需要不断的调整旋转臂的转角，使用PID算法比较合适。

PID控制器本身是一种基于对“过去”、“现在”和“未来”信息估计的简单控制算法。

PID 控制系统原理框图
该系统主要由PID 控制器和被控对象组成。

作为一种线性控制器，它根据设定值r(t)和实际输出值y(t)构成控制偏差e(t)，将偏差按比例、积分和微分通过线性组合构成控制量u(t)，对被控对象进行控制。

控制器的输入输出关系可描述为:
()()()()01t
de t u t Kp e t e t dt Td Ti dt ⎡⎤=++⎢⎥⎣⎦
⎰ 式中:e(t)=r(t)-u(t), K 为比例系数T,为积分时间常数、T 为微分时间常数。

使用位置式PID 控制算法。

如果基于用求和代替积分，用差分代替微分来进行离散化，那么，离散化之后的数字量的PID 控制算式为
()()()()()0
1k
D
p i I T T
u k K e k e i e k e k T T =⎧⎫=+
+
--⎡⎤⎨⎬⎣⎦⎩
⎭
∑ 经z 变换后，数字PID 校正器的脉冲传递函数的一般形式为：
()()()()111111D C p I U z T T G z K Z E z T Z T --⎡⎤=
=++-⎢⎥-⎣⎦
3.电路图及原理图见附录1
主程序流程图及PID 子程序流程图见附录2
程序见附录4
u1(t)
u(t)
-++
+
++
r(t) 积分
比例
微分
被控对象
c(t)
+
y(t)
v(t)
4.测量方案及测试结果
测量仪器：电脑，MATLAB 软件。

测量方案：利用MATLAB仿真，通过建模将非线性关系转化成线性关系。

分别
测出旋转臂的角度和摆杆角度的仿真曲线。

测量结果见附录3
5.讨论与总结
5.1简易旋转倒立摆及控制装置的完成情况
题目完成情况未完成原因
基础一。

摆杆从处于自然下垂状态（摆角0°）开始，驱动电
机带动旋转臂作
往复旋转使摆杆摆动，并尽快使摆角达到或超过-60°~ +60°
完成
基础二。

从摆杆处于自然下垂状态开始，尽快增大摆杆的摆动
幅度，直至完成
圆周运动
完成
基础三。

在摆杆处于自然下垂状态下，外力拉起摆杆至接近
165°位置，外力
撤除同时，启动控制旋转臂使摆杆保持倒立状态时间不少于5s；
期间
旋转臂的转动角度不大于90°
完成
发挥一。

从摆杆处于自然下垂状态开始，控制旋转臂作往复旋转运动，尽快使
摆杆摆起倒立，保持倒立状态时间不少于10s 未完成硬件制作粗糙，稳定性
差，程序难以调整出合
适的数据去维持稳定
状态。

发挥二。

在摆杆保持倒立状态下，施加干扰后摆杆能继续保持
倒立或2s 内恢复
倒立状态；
未完成
发挥三。

在摆杆保持倒立状态的前提下，旋转臂作圆周运动，
并尽快使单方向
转过角度达到或超过360°
未完成
5.2心得体会：
比赛完后，我们各组都总结了一下，认为，赛前准备很不充分，要用的元器件多有短缺，有些器件只有一两件。

在调试过程中产生损耗后，还要去网购，浪费时间。

同时，组员间的配合不紧密，工作起来七手八脚，乱糟糟的。

工作态度不严谨，硬件制作粗糙，工具随意乱丢，用起来却找不着。

出现的这些问题就是我们今后改进的方向，在赛前要充分准备，多准备元器件用以备用，工具设备也要准备充足以方便工作。

同时加强训练，让参赛队员严肃，认真地对待比赛，熟悉队友，做到默契分工合作。

6.参考文献：
黄南晨．旋转式倒立摆系统的设计与实现．[硕士学位论文】，中国科学技术大学控制理论与控制工程专业，2002
管凤旭．倒立摆系统的可拓控制策略及实验研究．【硕士学位论文】，哈尔滨工程大学控制理论与控制工程专业，2006
黄辉．基于可拓控制的仿真研究．[硕士学位论文】，南京理工大学控制理论与控制工程专业，2003
王俊．基于倒立摆的三种控制策略的研究．[硕士学位论文】，湖北工业大学电力电子与电力传动专业，2008
李晓丹．模糊PID控制器的设计研究．陬士学位论文】，天津大学控制理论与控制工程专业，2005
王晓明．电动机的单片机控制．北京航空航天大学出版社(第2版)，2007 江晨，王富东．旋转式倒立摆系统的算法研究及仿真．工业控制计算机．2010
附录1：C8051F040单片机
BTN7971B驱动模块
附录 2
NO
YES --
主程序流程图
PID 子程序流程图
开始 程序初始化 读入Kp,Ki,Kd
调用数据采集处理子程序
调用PID 子程序 将PID_flag 位清0
判断PID-flag 是否为
1
计算偏差 E(n)=R(n)-Y(n)
计算比例项 P-term=Kp*E(n)
计算积分项并限幅 I-term=Ki*Ts*sumE
调用电机控制子程序
计算积分项并限幅
D-term=Kd*(E(n)-E(n-2))/(2*Ts)
计算输出控制量C(n)
C(n)=P_term+I_term+D_term
PID 子程序结束
开始PID 子程序
附录3
旋转臂转动角度和摆杆转动角度的仿真曲线
附录4
#include <c8051F040.h> // SFR declarations
#include<math.h>
//-----------------------------------------------------------------------------
// 16-bit SFR Definitions for 'F04x
//-----------------------------------------------------------------------------
sfr16 ADC0 = 0xbe; // ADC0 data
sfr16 RCAP2 = 0xca; // Timer2 capture/reload
sfr16 RCAP3 = 0xca; // Timer3 capture/reload
sfr16 TMR2 = 0xcc; // Timer2
sfr16 TMR3 = 0xcc; // Timer3
sbit PWM = P0^0; // LED='1' means ON
sbit SW1 = P3^7; // SW1='0' means switch pressed sbit P4_0 = P4^0; // LED='1' means ON
sbit P4_1 = P4^1; // 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 and 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信号端
bit 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[-1]
uint16 PrevError ; //Error[-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 PID();
//-----------------------------------------------------------------------------
// Global Variables
//-----------------------------------------------------------------------------
unsigned short Result; // ADC0 decimated value long 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-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_Init (SYSCLK/SAMPLE_RA TE); // Initialize Timer3 to overflow at
// sample rate
ADC0_Init (); // Init ADC
PCA0_Init ();
P4 = 0x00;
SFRPAGE = ADC0_PAGE;
AD0EN = 1; // Enable ADC
Result = 0;
EA = 1; // Enable global interrupts
delay(50);
SFRPAGE = CONFIG_PAGE;
K=75;Ki=70;Kd=2;
en0=en1=en2=0;
Cn=i_term=d_term=0;
SumE=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 SFRPAGE_SA VE = 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_SA VE; // Restore SFR page
}
//-----------------------------------------------------------------------------
// PORT_Init
//-----------------------------------------------------------------------------
//
// Return Value : None
// 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_SA VE = SFRPAGE; // Save Current SFR page
SFRPAGE = CONFIG_PAGE; // set 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_SA VE; // Restore SFR page
}
//-----------------------------------------------------------------------------
// ADC0_Init
//-----------------------------------------------------------------------------
//
// Return Value : None
// Parameters : None
//
// Configure 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_SA VE = SFRPAGE; // Save Current SFR page
SFRPAGE = ADC0_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 pin as ADC mux input
ADC0CF = (SYSCLK/SAR_CLK) << 3; // ADC conversion clock = 2.5MHz, Gain=1 EIE2 |= 0x02; // enable ADC interrupts
SFRPAGE = SFRPAGE_SA VE; // Restore SFR page
}
//-----------------------------------------------------------------------------
// TIMER3_Init
//-----------------------------------------------------------------------------
//
// Return Value : None
// Parameters :
// 1) int counts - calculated Timer overflow rate
// range is postive range 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_SA VE = SFRPAGE; // Save Current SFR page
SFRPAGE = TMR3_PAGE;
TMR3CN = 0x00; // Stop Timer3; Clear TF3;
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_SA VE; // Restore SFR page
}
void PCA0_Init (void)
{
char SFRPAGE_save = SFRPAGE; // Save current SFR 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 counter
CR = 1;
SFRPAGE = SFRPAGE_save;
}
void Timer0_Init(void)
{
char SFRPAGE_SA VE = 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
CKCON = 0x02; // Timer0 uses a 1:48 prescaler
ET0 = 1; // Timer0 interrupt enabled
TCON = 0x10; // Timer0 ON
SFRPAGE = SFRPAGE_SA VE; // 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_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为64HZ i_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; //比例项
Cn=en0*K;
Cn+=i_term+d_term;
en2=en1;
en1=en0;
en0=0;
motor();
}
void motor()
{
SFRPAGE = PCA0_PAGE;
motor_flag=0;
if(Result>1430 && Result<1610)
{
motor_flag=1;
return;
}
Cn>>4;
if(Cn>255)
Cn=255;
if(Cn<-255)
Cn=-255;
if(motor_flag==0)
{
if(Cn>0)
{
// P4_1 = 0;
// PCA0CPH0 = 256 - (256 * 0.3);
// DIR=0; //电机反转
// CCAP0H=255-Cn;
}
if(Cn<0)
{
// P4_1 = 1;
// Cn=-Cn;
// PCA0CPH0 = 256 - (256 * 0.3);
// DIR=1; //电机正转
// CCAP0H=255-Cn;
}
}
}
//-----------------------------------------------------------------------------
// Interrupt Service Routines
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
// Timer0_ISR
//-----------------------------------------------------------------------------
//
// Here we process the Timer0 interrupt and toggle the LED
//
//-----------------------------------------------------------------------------
void Timer0_ISR (void) interrupt 1
{
TH0 = TIMER0_RELOAD_HIGH; // Reinit Timer0 High register TL0 = TIMER0_RELOAD_LOW;
SFRPAGE = CONFIG_PAGE; // Reinit Timer0 Low register
// pwm_val_a++;
// pwm_out_a_moto();
}
//-----------------------------------------------------------------------------
// ADC0_ISR
//-----------------------------------------------------------------------------
//
// Here we take the ADC0 sample, add it to a running total <accumulator>, and
// decrement our local decimation counter <int_dec>. When <int_dec> reaches // zero, we post the decimated result in the global variable <Result>.
//
//-----------------------------------------------------------------------------
void ADC0_ISR (void) interrupt 15
{
// ADC samples
AD0INT = 0; // Clear ADC conversion complete
// indicator
Result = ADC0; // Read ADC value
PID_flag = 1;
flag=1;
//return Result;
}
//-----------------------------------------------------------------------------
// End Of File。