简易旋转倒立摆及控制装置(C))_简易旋转倒立摆

2020-02-27 其他范文下载本文

简易旋转倒立摆及控制装置(C))由刀豆文库小编整理，希望给你工作、学习、生活带来方便，猜你可能喜欢“简易旋转倒立摆”。

2013年全国大学生电子设计竞赛

简易旋转倒立摆及控制装置（2013年9月7日

C题）

摘要

本设计以16位的MC9S12XS128单片机为系统控制核心，MMA7361加速传感器，ENC-03角速度传感器，组成了一个较为完善的倒立摆控制系统。MMA7361可以根据运动和方向改变输出信号的电压值，单片机A/D转换器读取其输出信号，可以检测其运动和方向；而ENC-03是一种应用科氏力原理的角速度传感器，输出模拟电压信号与角速度成正比，单片机通过此电压信号来检测摆杆角速度。通过软件编程控制使倒立摆稳定达到题目要求状态。

关键词：倒立摆，单片机，直流电机，加速传感器和角速度传感器

This design with 16 MC9S12XS128 single-chip microcomputer as the system control core, MMA7361 acceleration sensor, ENC03 is an application of coriolis force principle of the angular velocity sensor, the output analog voltage signal is proportional to the angular velocity, the microcontroller through the voltage signal to detect swinging rod angular velocity.Through software programming to control the inverted pendulum stable state meet the subject requirements.Key words: inverted pendulum,single-chip microcomputer, MMA7361,ENC-03

1系统方案.........................................................3 1.1单片机的选择.................................................3 1.2传感器的选择.................................................3 1.3电机的选择...................................................3 2系统理论分析与计算...............................................3 3电路与程序设计...................................................4 3.1系统框图...................................................4 3.2系统硬件电路...............................................4 3.2.1单片机最小系统.......................................4 3.2.2陀螺仪...............................................5 3.3软件程序设计...............................................6 3.3.1程序功能描述.........................................6 3.3.2主程序流程图.........................................6 4 测试方案与结果..................................................7 4.1软件硬件联调..............................................7 4.2测试结果..................................................7 5 参考文献........................................................8

附录.......................................................................................................................9

1系统方案

1.1单片机的选择

方案1：采用51单片机作为控制器，A/D转换器读取传感器输出电压信号，并转换成角度信号来控制电机。51单片机价格便宜，应用广泛，简单实用，市场需求大；但接口少，使用时需扩展，运算处理速度不高，功耗高，且在本系统中还要加A/D采集。

方案2：采用16位的MC9S12XS128单片机作为系统控制器，通过将角度传感器输出的模拟电压信号转换成控制信号，来控制电机使摆杆按要求运动。MC9S12XS128是一款针对汽车电子的高性能16位单片机，速度快，功能强，成本低，功耗低。总线速度高达40MHz,可配置8位，10位或12位ADC,3µs的转换时间，8通道PWM，易于实现电机控制。

经过综合考虑，我们选择方案二MC9S12XS128单片机。

1.2 传感器的选择

方案一：用水平传感器检测摆杆转角。MMA7361加速度传感器是检测物件运动和方向的传感器，它根据物件运动和方向改变输出信号的电压值。将采集到的电压信号转换成角度信号，就可控制电机以控制摆杆。

方案二：用电阻式角度传感器检测摆杆角度。电阻式角度传感器模拟信号输出，所以用它测角度变化不明显，灵敏度不高，电压干扰大。

经过讨论，我们决定使用现有的芯片MMA7361，采取方案一。

1.3 电机的选择

方案一：采用舵机。控制电路板接受来自信号线的控制信号，控制电机转动，电机带动一系列齿轮组，减速后传动至输出舵盘。舵机的输出轴和位置反馈电位计是相连的，舵盘转动的同时，带动位置反馈电位计，电位计将输出一个电压信号到控制电路板，进行反馈，然后控制电路板根据所在位置决定电机的转动方向和速度，从而达到目标停止。控制信号是一个周期在20ms左右，宽为1-2ms.角度控制脉冲更改太快，舵机反应不过来，不易精确控制。

方案二：采用直流电机。其中直流电机使用方便，价格便宜，平滑方便，可实现频繁的无级快速起动、制动和反转；调整范围广、过载能力强，直流电机用于调速控制时比较完善。

综合以上两种方案，我们选择方案二。

2系统理论分析

倒立摆是一个复杂的快速，非线性，多变量，强耦合，自然不稳定非最小相位系统。是重心在上支点在下控制问题的抽象。在对倒立摆的控制研究中，往往因为找不到合适的研究对象，而不能对研究成果做进一步的深入研究。

按照题目要求，摆杆在运动过程中涉及到平衡位置的确定和摆杆的角度，摆动的幅度等问题，因此，我们在摆杆上固定加速度计和陀螺仪，二则结合使用，实时的根据摆杆的运动和方向，输出模拟电压信号，经过单片机的分析控制，通过控制电机是摆杆状态达到要求。

3电路与程序设计

3.1 系统框图如3-1所示：

开始初始化传感器检测MC9S12XS128电机驱动直流电机摆杆摆动结束图3-1 系统框图

3.2系统硬件电路

3.2.1 单片机最小系统：MC9S12XS128是一款针对汽车电子市场的高性能16位单片机。总线速度高达40MHz,可配置8位，10位或12位ADC,3µs的转换时间，4通道16位计数器，8通道PWM，易于实现电机控制。具有速度快，功能强，成本低，功耗低的特点。单片机最小系统如图3-2所示。

图3-2 单片机最小系统

3.2.2 陀螺仪ENC-03电路：ENC-03是一种应用科氏力原理的角速度传感器，输出模拟电压信号与角速度成正比。能够测量包含旋转的各种运动，供电电压为2.7-5.25V，角速度为零时输出1.35V。具有特别小的体积和重量，响应快，驱动电压和功耗低的特点。电路图如图3-3所示。

图3-3 ENC-03电路

3.3：软件程序设计

3.3.1 程序功能描述

根据系统控制过程及要求，编写程序代码。软件实现的功能如下： 1，摆杆从自然下垂开始，电机驱动旋转臂往复旋转使摆杆摆动并尽快超过—60~+60度。

2，摆杆从自然下垂开始，尽量增大摆杆摆动幅度，完成圆周运动。

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

3.3.2主程序流程图：

开始初始化写命令字传感器检测并分析单片机控制电机驱动摆杆摆动结束

图 3-4 主程序流程图

4测试方案与测试结果

4.1 硬件软件联调

调试测试波形如图4-1所示

图4-1 调试波形

4.2 调试结果

1.将摆杆处于自由下垂状态，接通电源，拨动拨码开关1号键，驱动电机往复旋转使摆杆摆动，要求摆角尽快达到或超过-60°~ +60°。

结果：开始运行时摆杆摆动幅度较小，经过仔细研究，改进装置和修改程序，是摆杆摆角较快达到了要求，完成基本要求一。

2.将摆杆处于自由下垂状态，接通电源，拨动拨码开关2号键，使摆杆尽快增大摆角，完成圆周运动。

结果：开始测试时，旋转臂旋转较快，摆杆摆动幅度不能满足要求，无法完成圆周运动。经过长时间反复思考摆杆摆动过程，并修改程序最终终于稳定完成圆周运动，实现基本要求二。

5：参考文献李朝青.单片机原理及接口技术.北京：北京航空航天大学

出版社，2006 李朝青.单片机原理及接口技术（简明修订版）.北京：北

京航空航天大学出版社，2003 3 钱逸秋.单片机原理与应用.北京：北京电子工业出版社，2002 4 黄智伟.全国大学生电子设计竞赛制作实训.北京：北京航空航天大学出版社，2007.2

附录：源程序

#include

/* common defines and macros */ #include “derivative.h”

/* derivative-specific definitions */ #include #include #include “OLED.h” #define uint unsigned int #define uchar unsigned char //蜂鸣器K5口宏定义

//#define Speaker_Out DDRK_DDRK5 //#define Speaker

PORTK_PK5

/*******************************变量定义*********************************/

float OutData[4]={0};

//串口示波器输出数据数组 //拨码开关标志位

uchar DIP_flag1,DIP_flag2,DIP_flag3,DIP_flag4,DIP_flag5,DIP_flag6,DIP_flag7,DIP_flag8;unsigned int PIT_CNT = 0;unsigned int count0=0;unsigned int count1=0;unsigned int count2=0;unsigned int count3=0;unsigned int flag=0;unsigned int flag1;//方向标志 //直立控制全局变量

float ENC_03,MMA_X,MMA_Z,GM_J_Y,GM_J_Z;float ENC[3],GM_Z[3],GM_Y[3];float g_ENC_03,g_GM_J;

//互补滤波后的角度，角速度值

volatile float GM_J_Delta = 46.2;//GM_J_Zero-=GM_J_Delta;越大越前倾 volatile float GM_Z_Zero;volatile float GM_Y_Zero;volatile float ENC_03_Zero;volatile float PID_P =21.0;volatile float PID_D =9.61;// 0.56;//5.72;float AngleControl_Left = 0;

float fLeft, fRight;int speed=40;;int time=50;

void DIP_Switch(void);void Sudu(int max);void Start_1(void);

void Start_2(void);void DIP(void);

/****************************设置总路线频率*********************************/ void Set_Bus_64M(void){

CLKSEL=0X00;

PLLCTL_PLLON=0;

SYNR =0xc0 | 0x07;

REFDV=0x80 | 0x01;

POSTDIV=0x00;

_asm(nop);

PLLCTL_PLLON=1;

while(!CRGFLG_LOCK);

CLKSEL_PLLSEL =1;

}

void Set_Bus_80M(void)

//超频到80M 不一定适用{

CLKSEL_PLLSEL=0;

PLLCTL_PLLON=0;

SYNR =0xc0 | 0x09;

REFDV=0x80 | 0x01;

POSTDIV=0x00;

_asm(nop);

PLLCTL_PLLON=1;

while(!CRGFLG_LOCK);

CLKSEL_PLLSEL =1;

}

//64M~ms级精确延时

void Dly_ms(unsigned int ms)

{

uint ii,jj;if(ms

{

for(jj=0;jj

{

_asm NOP;_asm NOP;_asm NOP;_asm NOP;

} }

}

//64M~us级精确延时

void Dly_us(unsigned int us){

uint cnt0;

for(cnt0=0;cnt0

{

_asm NOP;_asm NOP;_asm NOP;_asm NOP;_asm NOP;

_asm NOP;_asm NOP;_asm NOP;

} }

/****************************初始化Uart*********************************/ void UART_Init(void){

SCI0CR1=0x00;//8位数据位，1位停止位

SCI0CR2=0x2c;//开中断,RX enable,Tx enable

SCI0BDH=0x01;

SCI0BDL=0xA0;}

////////////////////////////////////////////////////////////////////////// void uart_putchar(unsigned char c){

while(!SCI0SR1_TDRE);

//keep waiting when not empty

SCI0DRL=c;} ////////////////////////////////////////////////////////////////////////// void uart_putstr(char ch[]){

unsigned char ptr=0;

while(ch[ptr])

{

uart_putchar((unsigned char)ch[ptr++]);

} } ////////////////////////////串口示波器的CRC校验/////////////////////////// unsigned short CRC_CHECK(unsigned char *Buf, unsigned char CRC_CNT)

{

unsigned short CRC_Temp;

unsigned char i,j;

CRC_Temp = 0xffff;

for(i=0;i

{

CRC_Temp ^= Buf[i];

for(j=0;j

{

if(CRC_Temp & 0x01)

CRC_Temp =(CRC_Temp >>1)^ 0xa001;

else

CRC_Temp = CRC_Temp >> 1;

}

return(CRC_Temp);} ////////////////////////////串口示波器输出函数///////////////////////////// void OutPut_Data(float OutData[4])

//配合串口示波器的输出协议，float 字长设为16位 {

//可以输出4路信息，每路数据长度是16位

int temp[4] = {0};

//每次输出字节总数10个，最后两个字节为校验

unsigned int temp1[4] = {0};

unsigned char databuf[10] = {0};

unsigned char i;

unsigned short CRC16 = 0;

for(i=0;i

{

temp[i] =(int)OutData[i];

temp1[i] =(unsigned int)temp[i];

}

for(i=0;i

{

databuf[i*2]

=(unsigned char)(temp1[i]%256);

databuf[i*2+1] =(unsigned char)(temp1[i]/256);

}

CRC16 = CRC_CHECK(databuf,8);

databuf[8] = CRC16%256;

databuf[9] = CRC16/256;

for(i=0;i

uart_putchar(databuf[i]);}

/****************************初始化ATD*********************************/ void ADC_Init(void){

ATD0STAT0= 0xB0;

//转换序列完成，外部触发复写，结果寄存器复写标志位写1清零

ATD0CTL0 = 0x07;

//翻转通道为通道0

ATD0CTL1 = 0x20;

//禁止外部触发,10位精度,放电,3210:ch

ATD0CTL2 = 0x40;

//禁止外部触发, 中断禁止

ATD0CTL3 = 0xB0;//0x81;

//右对齐无符号,每次转换1个序列长度, No FIFO, Freeze模式下继续转

ATD0CTL4 = 0x27;

//采样时间为6个AD时钟周期,ATDClock=[BusClock*0.5]/[PRS+1]=4M

ATD0CTL5 = 0x30;

//特殊通道禁止,连续转换 ,单通道扫描模式

ATD0DIEN = 0x00;

//禁止数字输入 }

/////////////////////////////ATD数据采集////////////////////////////////// float ReadATD(unsigned char channel){

float ad=0;

while(!ATD0STAT0_SCF);

switch(channel)

{

default:

case 0: ad= ATD0DR0;break;

case 1: ad= ATD0DR1;break;

case 2: ad= ATD0DR2;break;

case 3: ad= ATD0DR3;break;

case 4: ad= ATD0DR4;break;

case 5: ad= ATD0DR5;break;

case 6: ad= ATD0DR6;break;

case 7: ad= ATD0DR7;break;

case 8: ad= ATD0DR8;break;

}

return ad;

}

/************************PIT 1ms定时中断*******************************/ void PIT_Init(void){

PITCFLMT_PITE=0;//定时中断通道0关

PITCE_PCE0=1;

//定时器通道0使能

PITMTLD0=64-1;

//8位定时器初值设定,8分频，在64MHzBusClock下，为8MHz。

即0.125us

PITLD0=1000-1;

//16位定时器初值设定。定时周期=(PITMTLD0+1)*(PITLD0+1)/fBUS = 1ms

PITFLT_PFLT0=1;

PITINTE_PINTE0=1;//定时器中断通道0中断使能

PITCFLMT_PITE=1;//定时器通道0使能

}

/***************************初始化PWM**********************************/ void PWM_Init(void){

PWME = 0x00;

//禁止PWM

PWMPRCLK = 0x22;

//CLOCK A,B预分频4，即采用总线时钟64M/4=16M

PWMSCLA = 0x04;

//CLOCK SA = CLOCKA/2/PWMSCLA = 2M

PWMSCLB = 0x04;

//CLOCK SB = CLOCKB/2/PWMSCLB = 2M

PWMCLK

= 0xff;

//各通道均选用SA,SB时钟

PWMCAE = 0x00;

//波形均为左对齐

PWMCTL = 0xf0;

//级联01，23，45，67通道，高位有效

PWMPER01 = 200;

//PWM周期=通道时钟周期*PWMPERx = 50us ,即PWM频率为20KHz

PWMPER23 = 200;

PWMDTY01 = 0;

PWMDTY23 = 0;

PWMPOL = 0xff;//通道极性为正，先出高电平，再出低电平

PWME = 0xff;

//使能PWM }

///////////////////////////两路电机占空比定义///////////////////////////// void Moto(int pwm){

if(pwm>=0)

{

PWME_PWME3 = 1;

PWME_PWME1 = 0;

DDRP_DDRP1 = 1;

PTP_PTP1 = 0;

PWMDTY23 = pwm;

}

if(pwm

{

PWME_PWME3 = 0;

PWME_PWME1 = 1;

DDRP_DDRP3 = 1;

PTP_PTP3 = 0;

PWMDTY01 =-pwm;

} } /***********************************角度采集******************************/

//20次均值滤波

void Average_Filter(void)

//AD采集均值滤波 {

ENC_03 =(ReadATD(0)+ReadATD(0)+ReadATD(0)+ReadATD(0)+ReadATD(0)+ReadATD(0)+ReadATD(0)+ReadATD(0)+ReadATD(0)+ReadATD(0))/10;

GM_J_Z =(ReadATD(1)+ReadATD(1)+ReadATD(1)+ReadATD(1)+ReadATD(1)+ReadATD(1)+ReadATD(1)+ReadATD(1)+ReadATD(1)+ReadATD(1))/10;//sum3 / 20;

GM_J_Y =(ReadATD(2)+ReadATD(2)+ReadATD(2)+ReadATD(2)+ReadATD(2)+ReadATD(2)+ReadATD(2)+ReadATD(2)+ReadATD(2)+ReadATD(2))/10;}

void Filter(void){

unsigned char i;

for(i=0;i

Average_Filter();

ENC[i] = ENC_03;

GM_Z[i] =GM_J_Z;

GM_Y[i] =GM_J_Y;

}

if((ENC[1]-(ENC[0]+ENC[2])/2)>1){

ENC_03 =(ENC[0]+ENC[2])/2;

}

else

ENC_03 = ENC[1];

if((GM_Z[1]-(GM_Z[0]+GM_Z[2])/2)>1){

GM_J_Z =(GM_Z[0]+GM_Z[2])/2;

}

else

GM_J_Z = GM_Z[1];

if((GM_Y[1]-(GM_Y[0]+GM_Y[2])/2)>1){

GM_J_Y =(GM_Y[0]+GM_Y[2])/2;

}

else

GM_J_Y = GM_Y[1];}

//陀螺仪零点标定

void ENC_Zero_Capture(void){

unsigned int i=0;

unsigned int m=0;

float ENC_03_zero = 0;

float GM_Y_zero = 0;

float GM_Z_zero = 0;

for(i=0;i

{

ENC_03_zero += ReadATD(0);

}

ENC_03_Zero = ENC_03_zero /20000;

for(i=0;i

{

GM_Y_zero += ReadATD(2);

}

GM_Y_Zero = GM_Y_zero /20000;

for(i=0;i

{

GM_Z_zero += ReadATD(2);

}

GM_Z_Zero = GM_Z_zero /20000;

//GM_J_Zero-=GM_J_Delta;} /**********************************启动60度范围**************************/ void Sudu(int max){

if(count1==1)

{

count1=0;

if(speed

speed+=10;

else speed=max;

} }

void Start_1(void){

Sudu(80);if(count0

{

fLeft=speed;

Moto(fLeft);

} else if(count0>210 && count0

{

fLeft=0;

Moto(fLeft);

}

else if(count0>215 && count0

{

fLeft=-speed;

Moto(fLeft);

} else if(count0>415 && count0

{

fLeft=0;

Moto(fLeft);

}

else if(count0>420)

{

count0=0;

}

/**************************************360**********************************/

度范17

围

void Start_2(void){

if(GM_J_Y>=360)

{

Sudu(100);

if(count0

{

fLeft=speed;

Moto(fLeft);

}

else if(count0>210 && count0

{

fLeft=0;

Moto(fLeft);

}

else if(count0>215 && count0

{

fLeft=-speed;

Moto(fLeft);

}

else if(count0>415 && count0

{

fLeft=0;

Moto(fLeft);

}

else if(count0>420)

{

count0=0;

}

else

{

fLeft=150;

Moto(fLeft);

} }

/***********************************角度控制********************************/ void angle_control(void){

//if(GM_J_Y>=300)

AngleControl_Left =(int)((GM_Z_ZeroENC_03_Zero)*PID_D);//+Dead_DTY);

//else AngleControl_Left =(int)((GM_J_Y-GM_Y_Zero)*PID_P+(ENC_03-ENC_03_Zero)*PID_D);

fLeft = AngleControl_Left;

if(fLeft>=200)

{

fLeft=200;

}

if(fLeft

{

fLeft=-200;

}

Moto((int)fLeft);}

/**************************************拨码开关*******************************/ //拨码开关去抖动 void DIP(void){

if(PORTB!= 0x00)Dly_ms(20);PORTB=PORTB;}

void DIP_Switch(void){

DIP();

//开关1 控制电机是否输出

if(PORTB_PB0 == 1){DIP_flag1 = 1;Dis_Char(6,0,'Y');}

if(PORTB_PB0 == 0){DIP_flag1 = 0;Dis_Char(6,0,'N');}

if(PORTB_PB1 == 1){DIP_flag2 = 1;Dis_Char(6,2,'1');}

if(PORTB_PB1 == 0){DIP_flag2 = 0;Dis_Char(6,2,'0');}

if(PORTB_PB2 == 1){DIP_flag3 = 1;Dis_Char(6,4,'2');}

if(PORTB_PB2 == 0){DIP_flag3 = 0;Dis_Char(6,4,'0');}

if(PORTB_PB3 == 1){DIP_flag4 = 1;Dis_Char(6,6,'3');}

if(PORTB_PB3 == 0){DIP_flag4 = 0;Dis_Char(6,6,'0');}

if(PORTB_PB4 == 1){DIP_flag5 = 1;Dis_Char(6,9,'4');}

if(PORTB_PB4 == 0){DIP_flag5 = 0;Dis_Char(6,9,'0');}

if(PORTB_PB5 == 1){DIP_flag6 = 1;Dis_Char(6,11,'5');}

if(PORTB_PB5 == 0){DIP_flag6 = 0;Dis_Char(6,11,'0');}

if(PORTB_PB6 == 1){DIP_flag7 = 1;Dis_Char(6,13,'6');}

if(PORTB_PB6 == 0){DIP_flag7 = 0;Dis_Char(6,13,'0');}

//开关7 控制补光灯是否点亮

if(PORTB_PB7 == 1){DIP_flag8 = 0;Dis_Char(6,15,'7');}

if(PORTB_PB7 == 0){DIP_flag8 = 1;Dis_Char(6,15,'0');}

}

/*************************各模块统一初始化*****************************/ void Device_Init(void){

Set_Bus_64M();ADC_Init();

UART_Init();

PIT_Init();

OLED_Init();

PWM_Init();

DDRB=0X00;

DDRA=0XFF;

}

/*************************************主函数********************************/ void main(void){

Device_Init();

PIT_CNT = 0;

ENC_Zero_Capture();

Dly_ms(10);

OLED_Fill(0xff);//黑屏

Dly_ms(1000);

OLED_Fill(0x00);//亮屏

Dly_ms(1000);

EnableInterrupts;

for(;;)

{

//StartModule();

OLED_P8x16Str(20,0,“Dream Team”);

OLED_P8x16Str(0,4,“The mode is:”);

//OLED_P6x8Str(16,6,“chiusir@yahoo.cn”);

DIP_Switch();

//OutData[0] = ENC_03;//ENC_Data;

//OutData[1] = GM_J_Y;//fLeft;//MMA_X;//QingJiao;

//OutData[2] = GM_J_Z;//SPEED_SET;//fRight;//MMA_Z;

//OutData[3] = 0;//g_fCarSpeed;// GM_J;

//OutPut_Data(OutData);

//nRF905_OutPut_Data(OutData);

} }

#pragma CODE_SEG __NEAR_SEG NON_BANKED void interrupt 66 PIT0_ISR(void){

DisableInterrupts;

PIT_CNT++;

count0++;

count2++;

if(PIT_CNT==100)

{

PIT_CNT=0;

count1++;

count3++;

}

if(count2==1)

{

Filter();}

else if(count2==2){

if(DIP_flag1==1 && DIP_flag2==1)Start_1();

}

else if(count2==3)

{

if(DIP_flag1==1 && DIP_flag3==1)Start_2();

}

else if(count2==4)

{

if(DIP_flag1==1 && DIP_flag4==1)angle_control();

}

else if(count2>=5)

{

if(DIP_flag1==0)

{

speed=40;

fLeft=0;

Moto(fLeft);

}

count2=0;

}

PITTF_PTF0= 1;;

EnableInterrupts;}

《简易旋转倒立摆及控制装置(C)).docx》

将本文的Word文档下载，方便收藏和打印

推荐度：

点击下载文档

相关专题简易旋转倒立摆简易装置简易旋转倒立摆简易装置

[其他范文]相关推荐

[其他范文]热门文章