Hướng phát triển của đề tài - Luận văn thạc sĩ Kỹ- 123docz.net

− Cải thiện thuật toán điều khiển cho hệ có thể swing –up ở mọi vị trí.

− Kết hợp bộ điều khiển thuật toán di truyền và thuật toán Trượt(Genetic

Sliding Mode).

− Tối ưu các hệ số của bộ Trượt dùng lý thuyết LMI.

TÀI LIỆU THAM KHẢO:

[1] M.W.Spong, D.J.Block,”The pendubot: a mechatronic system for control

research and education,” Pro.34th IEEE Conference Decision and Control, pp.555-556, 1995.

[2] Daniel Jerome Block,”Mechanical design and control ofb Pendubot ”for the

degree of Master of Science in General Engineering in the Graduate College of the University of Illinois at Urbana-Champaign, 1996

[3] Xiao Qing Ma,”Fuzzy Control for an under-actuated RoboticManipirlator:

Pendubot”,For the Degree of Master of Appiied Science at Concordia University Montreal, Quebec, Canada August 2001.

[4] Michael Kwapisz,”Control of an Inverted Pendulum(Pendubot)”,

Department of Automatic Control Lund Institute of Technology, January 2005.

[5] Djamila Zehar and Khier Benmahammed, ”Optimal sliding mode control of

the pendubot”, nternational Research Journal of Computer Science and Information Systems (IRJCSIS) Vol. 2(3) pp. 45-51, April, 2013.

[6] M.Zhang, and T.J.Tarn,”Hybrid control of the pendubot,” IEEE/ASMI

Transaction Mechatronics, Vol.7, No.1, pp.79-86, 2002 Systems Technology, vol.10, No.1, pp.14-20, 2002.

[7] I.Fantoni, R.Lozano,and M.W.Spong, “Energy based control of the

pendubot,”IEEE Transaction on Automatic Control, Vol.45, No.4, pp.725- 729, 2000.

[8] Dương Hoài Nghĩa. Điều khiển hệ thống đa biến, NXB Đại học quốc gia

TPHCM 2007.

[9] Nguyễn Thị Phương Hà. Lý thuyết điều khiển hiện đại. Nhà xuất bản Đại

học Quốc gia TP. HCM, 2009.

PHỤ LỤC A M ạch điện tử:

Hình 6.1 Sơ đồ module Basement board

Hình 6.2 Sơ đồ khối module UART

PB6_Encoder1_A

PD5_TXD

R1 1 3 5 7 9 11 13 15 17 19 21 23 25 27 29 31 33 35 37 39 41 43 45 47 49 2

4 6 8 10 12 14 16 18 20 22 24 26 28 30 32 34 36 38 40 42 44 46 48 50 B11

PB5_Encoder2_B

PA9_DIR PA8_PWM

PB4_Encoder2_A

PB7_Encoder1_B

PD6_RXD 5v

EN1

STM32F407

L1 1 3 5 7 9 11 13 15 17 19 21 23 25 27 29 31 33 35 37 39 41 43 45 47 49 2

4 6 8 10 12 14 16 18 20 22 24 26 28 30 32 34 36 38 40 42 44 46 48 50

B22 B1_USER

C4 10UF

PD6_RXD

U2 MAX3232 13

11 10

3 4

6 12

1615 R1IN

R2IN

T1IN T2IN

C+

C1- C2+

C2-

V+

V- R1OUT

R2OUT

T1OUT

T2OUT VCCGND C1 10UF

C10 10UF

UART

C7 10UF UART1 1

1 6 2 7 3 8 4 9 5

PD5_TXD

Hình 6.3 Sơ đồ khối module Encoder

EN B1_USER

B0_USER C5

104 5v

C6 104 B

PWM_OUT 1 2

DIR_OUT 1 2

EN1

PWM

C9 104 U1

74HC245 2

3 4 5 6 7 8 9

19 1

18 17 16 15 14 13 12 11

2010

A0 A1 A2 A3 A4 A5 A6 A7

OE DIR

B0 B1 B2 B3 B4 B5 B6 B7

VCCGND

B0_USER EN

5v PWM

ENCODER

PB4_Encoder2_A

ENCODER1 1 2 3 4 5 6

DIR+

DIR -

B1 C2 104

PB6_Encoder1_A

C11 104 A

R5 1K

DIR + B1

A1 PB7_Encoder1_B

C8 104 J4

ENCODER2 1 2 3 4 5 6 PA9_DIR

C3 104 PB5_Encoder2_B B

SW1

1 4

2 3

C12 104 PA8_PWM

1k 5v

Chương trình điều khiển:

Hàm main:

***************************PHAM HOAI VAN*************

#include "stm32f4xx.h"

#include <stdio.h>

#include <stdlib.h> /* This ert_main.c example uses printf/fflush */

#include "LQR1.h" /* Model's header file */

#include "rtwtypes.h" /* MathWorks types */

//======================================

#define truyen_4byte_8bit() DMA_Cmd(DMA1_Stream6, ENABLE) uint8_t SVRXBUF[14];

uint8_t SVTXBUF[]= {

0x00, 0x03, 0x01, 0x00, 0x00, 0xE5, 0x01, 0x00, 0xEB, 0x17, };

//======================================

extern int check;

extern int check1;

extern int check3;

__IO int32_t encVal_new;

__IO int32_t encVal_new22;

__IO int32_t encVal_new23;

__IO int32_t encVal_new24;

__IO int32_t encVal_old;

__IO int32_t encVal_new1;

__IO int32_t encVal_new2;

__IO int32_t encVal_new3;

__IO int32_t encVal_new4;

__IO int32_t encVal_old1;

__IO real_T new_pos_arm;

__IO real_T old_pos;

__IO real_T new_pos_pen;

__IO real_T old_pos_pen;

__IO real_T veclo_arm;

__IO real_T veclo_arm1;

__IO real_T veclo_arm2;

__IO real_T veclo_arm3;

__IO real_T veclo_pen;

__IO real_T veclo_pen1;

__IO real_T veclo_pen2;

__IO real_T veclo_pen3;

__IO real_T U=0;

__IO real_T U_dk=0;

void encoder_reset(void);

void adc_convert(void);

void position_arm(void);

void int_VAO_RA(void);

void encoder_init(void);

void position_pen(void);

void TIM2_10MS(void);

void TIM_PWM_Configuration(void);

void truyen_dulieu_8bit(int a);

void UART(unsigned int BaudRate);

void SendUSART(USART_TypeDef* USARTx, uint16_t ch);

int GetUSART(USART_TypeDef* USARTx);

/////////////////////////////////////////////////////////

GPIO_InitTypeDef gpioInit;

NVIC_InitTypeDef NVIC_InitStructure;

TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;

TIM_OCInitTypeDef TIM_OCInitStructure;

USART_InitTypeDef USART_InitStructure;

static boolean_T OverrunFlag = 0;

void rt_OneStep(void) {

/* Disable interrupts here */

/* Check for overrun */

if (OverrunFlag++) { rtmSetErrorStatus(LQR1_M, "Overrun");

return;

}

/* Save FPU context here (if necessary) */

/* Re-enable timer or interrupt here */

/* Set model inputs here */

/* Step the model */

LQR1_step();

/* Get model outputs here */

/* Indicate task complete */

OverrunFlag--;

}

void InitDMAUSART2() {

//DMA_InitTypeDef DMA_InitStructure;

//NVIC_InitTypeDef NVIC_InitStructure;

DMA_InitTypeDef DMA_InitStructure_RX;

DMA_InitTypeDef DMA_InitStructure_TX;

NVIC_InitTypeDef NVIC_InitStructure_RX;

NVIC_InitTypeDef NVIC_InitStructure_TX;

//DMA RX DMA1_STREAM_0

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);

DMA_DeInit (DMA1_Stream5); //For USART2 Rx

DMA_DeInit (DMA1_Stream6); //For USART2 Tx

while (DMA_GetCmdStatus (DMA1_Stream5) != DISABLE)

{}; //Doi DMA for RX khoi tao xong while (DMA_GetCmdStatus (DMA1_Stream6) != DISABLE)

{}; //Doi DMA for TX khoi tao xong

DMA_StructInit(&DMA_InitStructure_TX);

DMA_InitStructure_TX.DMA_PeripheralBaseAddr = (uint32_t) & (USART2-

>DR);

DMA_InitStructure_TX.DMA_Channel = DMA_Channel_4; //Table 43 DMA_InitStructure_TX.DMA_DIR = DMA_DIR_MemoryToPeripheral;

DMA_InitStructure_TX.DMA_MemoryInc = DMA_MemoryInc_Enable;

DMA_InitStructure_TX.DMA_PeripheralInc = DMA_MemoryInc_Enable;

DMA_InitStructure_TX.DMA_PeripheralDataSize = DMA_MemoryDataSize_Byte;

DMA_InitStructure_TX.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; /// DMA_MemoryDataSize_Byte;

DMA_InitStructure_TX.DMA_Mode = DMA_Mode_Normal;//

DMA_Mode_Circular; //DMA_Mode_Normal;

DMA_InitStructure_TX.DMA_Priority = DMA_Priority_Low;

DMA_InitStructure_TX.DMA_FIFOMode = DMA_FIFOMode_Disable;//DMA_FIFOMode_Enable;

DMA_InitStructure_TX.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;

DMA_InitStructure_TX.DMA_MemoryBurst = DMA_MemoryBurst_Single;

DMA_InitStructure_TX.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;

DMA_InitStructure_TX.DMA_Memory0BaseAddr = (uint32_t)&SVTXBUF;

///

DMA_InitStructure_TX.DMA_BufferSize = 10; ////so byte can truyen

DMA_Init (DMA1_Stream6, &DMA_InitStructure_TX);

/* Enable USART DMA */

USART_DMACmd(USART2, USART_DMAReq_Tx, ENABLE);

// configure Interrupt

NVIC_InitStructure_TX.NVIC_IRQChannel = DMA1_Stream6_IRQn ;

NVIC_InitStructure_TX.NVIC_IRQChannelPreemptionPriority = 1;

NVIC_InitStructure_TX.NVIC_IRQChannelSubPriority = 1;

NVIC_InitStructure_TX.NVIC_IRQChannelCmd = ENABLE;

NVIC_Init(&NVIC_InitStructure_TX);

/* Enable DMA interrupts */

DMA_ITConfig(DMA1_Stream6, DMA_IT_TC, ENABLE);

DMA_StructInit(&DMA_InitStructure_RX);

DMA_InitStructure_RX.DMA_PeripheralBaseAddr = (uint32_t) & (USART2-

>DR);

DMA_InitStructure_RX.DMA_Channel = DMA_Channel_4;

DMA_InitStructure_RX.DMA_DIR = DMA_DIR_PeripheralToMemory;

DMA_InitStructure_RX.DMA_MemoryInc = DMA_MemoryInc_Enable;

DMA_InitStructure_RX.DMA_PeripheralInc = DMA_MemoryInc_Enable;

DMA_InitStructure_RX.DMA_PeripheralDataSize = DMA_MemoryDataSize_Byte;

DMA_InitStructure_RX.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; /// DMA_MemoryDataSize_Byte;

DMA_InitStructure_RX.DMA_Mode = DMA_Mode_Normal;//

DMA_Mode_Circular; //DMA_Mode_Normal;

DMA_InitStructure_RX.DMA_Priority = DMA_Priority_High;

DMA_InitStructure_RX.DMA_FIFOMode = DMA_FIFOMode_Enable;

DMA_InitStructure_RX.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;

DMA_InitStructure_RX.DMA_MemoryBurst = DMA_MemoryBurst_Single;

DMA_InitStructure_RX.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;

DMA_InitStructure_RX.DMA_Memory0BaseAddr = (uint32_t)&SVRXBUF;

/// (uint32_t)&RXBUF->preamble;

DMA_InitStructure_RX.DMA_BufferSize = 14;

DMA_Init (DMA1_Stream5, &DMA_InitStructure_RX);

/* Enable USART DMA */

USART_DMACmd(USART2, USART_DMAReq_Rx, ENABLE);

/* Enable DMA */

// configure Interrupt NVIC_InitStructure_RX.NVIC_IRQChannel = DMA1_Stream5_IRQn;

NVIC_InitStructure_RX.NVIC_IRQChannelPreemptionPriority = 1;

NVIC_InitStructure_RX.NVIC_IRQChannelSubPriority = 2;

NVIC_InitStructure_RX.NVIC_IRQChannelCmd = ENABLE;

NVIC_Init(&NVIC_InitStructure_RX);

/* Enable DMA interrupts */

DMA_ITConfig(DMA1_Stream5, DMA_IT_TC, ENABLE);

//DMA_Cmd(DMA1_Stream5, ENABLE);

//DMA_Cmd(DMA1_Stream6, ENABLE);

}

void DMA1_Stream6_IRQHandler(void) {//RX USART1 DMA It

DMA_ClearITPendingBit(DMA1_Stream6, DMA_IT_TCIF6);

} //===================================

int main(void) {

int btn=0;

int q11=0;

int q11_dot=0;

int q22_dot=0;

int q22=0;

int uu=0;

int a=0;

int swing_bit=1;

int balan_bit=0;

TIM2_10MS();

TIM_PWM_Configuration();

encoder_init();

int_VAO_RA();

UART(115200);

InitDMAUSART2();

LQR1_initialize();

// truyen_dulieu_8bit(check1s);

while(1) {

encoder_init();

position_arm();

position_pen();

swing_bit=1;

balan_bit=0;

GPIO_SetBits(GPIOC,GPIO_Pin_5);//////díable pendubot

while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0)) {

GPIO_ResetBits(GPIOC,GPIO_Pin_5);//////enable pendubot TIM1->CCR1 = ((U*100/24) * (8399+1) / 100);

while(check)////bien check cu sau 10ms thi cap nhat gia tri 1 lan {

position_arm();

position_pen();

q1=new_pos_arm;

q2=new_pos_pen;

q1_dot=veclo_arm;

q2_dot=veclo_pen;

rt_OneStep();

// if((q1>1.4)&&(q2>-0.3)) if((q1>1.3)&&(q2>-0.2))

{

swing_bit=0;

balan_bit=1;

} if(balan_bit) {

U_dk=U_blan;

} if(swing_bit) {

U_dk=U_sw;

} if(U_dk>0){GPIO_SetBits(GPIOC,GPIO_Pin_4); }////DIR=1 quay theo chieu thuan.

if(U_dk<0){GPIO_ResetBits(GPIOC,GPIO_Pin_4);}//DIR=0 q if(U_dk<-7.5||U_dk>7.5){U=7.5;}////

else {U=U_dk;}

if(check1==1) {

q11=q1*100;

q11_dot=q1_dot*100;

q22=q2*100;

q22_dot=q2_dot*100;

uu=U*100;

SVTXBUF[0]=q11;

SVTXBUF[1]=q11>>8;

SVTXBUF[2]=q11_dot;

SVTXBUF[3]=q11_dot>>8;

SVTXBUF[4]=q22;

SVTXBUF[5]=q22>>8;

SVTXBUF[6]=q22_dot;

SVTXBUF[7]=q22_dot>>8;

SVTXBUF[8]=uu;

SVTXBUF[9]=uu>>8;

truyen_4byte_8bit();

} check=0;

} if ((q1>3)||(q2>1.57))//

{ U_dk=0;

} } check1=0;

} }

void truyen_dulieu_8bit(int a) {

chuong trinh truyen so negative and possitive

int i=0;

char b[2];

b[0]=a;

b[1]=a>>8;

for(i=0;i<2;i++) {

while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET);

{ USART_SendData(USART2,b[i]);

} } } void int_VAO_RA(void) {

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

//sau do khoi tao cau truc vao ra cua cong D gpioInit.GPIO_Pin=GPIO_Pin_4|GPIO_Pin_5;

gpioInit.GPIO_Mode=GPIO_Mode_OUT;

gpioInit.GPIO_Speed=GPIO_Speed_100MHz ; gpioInit.GPIO_OType=GPIO_OType_PP;

gpioInit.GPIO_PuPd=GPIO_PuPd_UP;

//khoi tao GPIOC GPIO_Init(GPIOC, &gpioInit);

gpioInit.GPIO_Pin=GPIO_Pin_12|GPIO_Pin_13;

gpioInit.GPIO_Mode=GPIO_Mode_OUT;

gpioInit.GPIO_Speed=GPIO_Speed_100MHz ; gpioInit.GPIO_OType=GPIO_OType_PP;

gpioInit.GPIO_PuPd=GPIO_PuPd_UP;

GPIO_Init(GPIOD, &gpioInit);

/////////khoi tao nut nhan

gpioInit.GPIO_Pin=GPIO_Pin_0;

gpioInit.GPIO_Mode=GPIO_Mode_IN;

gpioInit.GPIO_Speed=GPIO_Speed_100MHz ; gpioInit.GPIO_OType=GPIO_OType_PP;

gpioInit.GPIO_PuPd=GPIO_PuPd_DOWN;

GPIO_Init(GPIOA, &gpioInit);

} void TIM_PWM_Configuration(void) {

RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

gpioInit.GPIO_Pin = GPIO_Pin_7 | GPIO_Pin_8;

gpioInit.GPIO_Mode = GPIO_Mode_AF;

gpioInit.GPIO_Speed = GPIO_Speed_100MHz;

gpioInit.GPIO_OType = GPIO_OType_PP;

gpioInit.GPIO_PuPd = GPIO_PuPd_NOPULL ; GPIO_Init(GPIOA, &gpioInit);

GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_TIM1);

GPIO_PinAFConfig(GPIOA, GPIO_PinSource8, GPIO_AF_TIM1);

/* Time base configuration */

TIM_TimeBaseStructure.TIM_Prescaler = 0; ///xung timer la 84mhz TIM_TimeBaseStructure.TIM_Period = 8399; ///xung 10khz.

TIM_TimeBaseStructure.TIM_ClockDivision = 0;

TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;

TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;

TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;

TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;

TIM_OCInitStructure.TIM_Pulse = 0;

TIM_OC1Init(TIM1, &TIM_OCInitStructure);

TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);

TIM_ARRPreloadConfig(TIM1, ENABLE);

/* TIM1 enable counter */

TIM_Cmd(TIM1, ENABLE);

TIM_CtrlPWMOutputs(TIM1, ENABLE);//chi timer 1 va timer 8 moi co dong lenh nay

} void UART(unsigned int BaudRate) {

RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);

/* Configure USART Tx as alternate function */

gpioInit.GPIO_OType = GPIO_OType_PP;

gpioInit.GPIO_PuPd = GPIO_PuPd_UP;

gpioInit.GPIO_Mode = GPIO_Mode_AF;

gpioInit.GPIO_Pin = GPIO_Pin_5;

gpioInit.GPIO_Speed = GPIO_Speed_100MHz;

GPIO_Init(GPIOD, &gpioInit);

/* Configure USART Rx as alternate function */

gpioInit.GPIO_Mode = GPIO_Mode_AF;

gpioInit.GPIO_Pin = GPIO_Pin_6;

GPIO_Init(GPIOD, &gpioInit);

USART_InitStructure.USART_BaudRate = BaudRate;

USART_InitStructure.USART_WordLength = USART_WordLength_8b;

USART_InitStructure.USART_StopBits = USART_StopBits_1;

USART_InitStructure.USART_Parity = USART_Parity_No;

USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;

USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;

USART_Init(USART2, &USART_InitStructure);

GPIO_PinAFConfig(GPIOD,GPIO_PinSource5,GPIO_AF_USART2);

GPIO_PinAFConfig(GPIOD,GPIO_PinSource6,GPIO_AF_USART2);

USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);

//cau hinh NVIC

NVIC_InitStructure.NVIC_IRQChannel=USART2_IRQn;

NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;

NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;

NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;

NVIC_Init(&NVIC_InitStructure);

//cho phep USART1 hoat dong USART_Cmd(USART2, ENABLE);

}

///////////////////// TAO TIMER 8MS CHO VONG DIEU KHIEN ////////////////

void TIM2_10MS(void) {

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

TIM_TimeBaseStructure.TIM_Prescaler = ((SystemCoreClock)/1000000)-1; //

TIM_TimeBaseStructure.TIM_Period = 1000 - 1;

TIM_TimeBaseStructure.TIM_ClockDivision = 0;

TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);

TIM_Cmd(TIM2, ENABLE);

NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;

NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;

NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;

NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;

NVIC_Init(&NVIC_InitStructure);

} void encoder_init(void) {

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

/* TIM2 channel1,2 configuration : PA0, PA1 */

gpioInit.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5|GPIO_Pin_6 | GPIO_Pin_7;

gpioInit.GPIO_Mode = GPIO_Mode_AF;

gpioInit.GPIO_Speed = GPIO_Speed_100MHz;

gpioInit.GPIO_OType = GPIO_OType_PP;

gpioInit.GPIO_PuPd = GPIO_PuPd_UP ; GPIO_Init(GPIOB, &gpioInit);

/* Connect TIM pin to AF2 */

GPIO_PinAFConfig(GPIOB, GPIO_PinSource4, GPIO_AF_TIM3);

GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3);

GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_TIM4);

GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_TIM4);

// Init TIM: encoder interface TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI1, TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);

TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);

/* TIM enable counter */

TIM_Cmd(TIM3, ENABLE);

TIM_Cmd(TIM4, ENABLE);

encoder_reset();

}

void position_arm(void) {

// read encoder interface x4 encVal_new22 = TIM3->CNT;

encVal_new23 = TIM3->CNT;

encVal_new24 = TIM3->CNT;

encVal_new=(encVal_new22+encVal_new23+encVal_new24)/3;

new_pos_arm=(((encVal_new-encVal_old)*6.28)/8000)+old_pos;

veclo_arm=(new_pos_arm-old_pos)*125;///T=8ms sau khi xu li chia 100 old_pos=new_pos_arm;

encVal_old=encVal_new;

} ///////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////

void position_pen(void) {

encVal_new2 = TIM4->CNT;

encVal_new3 = TIM4->CNT;

encVal_new4 = TIM4->CNT;

encVal_new1=(encVal_new2+encVal_new3+encVal_new4)/3;

new_pos_pen=(((encVal_new1-encVal_old1)*6.28)/10000)+old_pos_pen;

veclo_pen=(new_pos_pen-old_pos_pen)*125;///T=8ms encVal_old1=encVal_new1;

old_pos_pen=new_pos_pen;

} void encoder_reset(void) {

//////////////////////////////////khoi tao cac bien cho pen encVal_new = 0;

encVal_new22 = 0;

encVal_new23 = 0;

encVal_new24 = 0;

encVal_old = 30000;

veclo_arm=0;

new_pos_arm=0;

old_pos=-1.570;

TIM4->CNT = 30000;

veclo_arm1=0;

veclo_arm2=0;

veclo_arm3=0;

//////////////////////////////////khoi tao cac bien cho arm TIM3->CNT = 30000;

encVal_new1 = 0;

encVal_new2 = 0;

encVal_new3 = 0;

encVal_new4 = 0;

encVal_old1 = 30000;

new_pos_pen=0;

old_pos_pen=0;

veclo_pen=0;

veclo_pen1=0;

veclo_pen2=0;

veclo_pen3=0;

}

Hàm điều khiển LQR và hàm Trượt:

* File: LQR1.c *

* Real-Time Workshop code generated for Simulink model LQR1.

* Model version : 1.24 * Real-Time Workshop file version : 7.4 (R2009b) 29-Jun-2009 * Real-Time Workshop file generated on : Sun Mar 29 11:35:48 2015 * TLC version : 7.4 (Jul 14 2009)

* C/C++ source code generated on : Sun Mar 29 11:35:48 2015 *

* Target selection: ert.tlc * Embedded hardware selection: 32-bit Generic * Code generation objectives: Unspecified * Validation result: Not run

#include "LQR1.h"

#include "LQR1_private.h"

/* Exported block signals */

real_T q1; /* '<Root>/In1' */

real_T q1_dot; /* '<Root>/In2' */

real_T q2; /* '<Root>/In3' */

real_T q2_dot; /* '<Root>/In4' */

real_T led; /* '<Root>/Pulse Generator' */

real_T U_sw; /* '<Root>/PD_SW_CONTROLLER' */

real_T U_blan; /* '<Root>/LQR_CONTROLLER' */

/* Block states (auto storage) */

D_Work_LQR1 LQR1_DWork;

/* Real-time model */

RT_MODEL_LQR1 LQR1_M_;

RT_MODEL_LQR1 *LQR1_M = &LQR1_M_;

/* Model step function */

void LQR1_step(void) {

real_T eml_d;

real_T eml_T;

real_T eml_d11_bar;

real_T eml_S;

int32_T eml_L;

U_blan =-((((q1 - 1.5707963267948966E+000) *-99.4133 + -4.2601*

q1_dot) + -87.4779 * q2) + -2.6460 * q2_dot);

eml_d = 0.001 * cos(q2) + 0.0011;

/* '<S2>:1:15' */

eml_T = -0.001 * sin(q2);

eml_d11_bar = (0.002 * cos(q2) + 4.8000000000000004E-003) - eml_d * eml_d / 0.0011;

eml_S = (q1 - 1.5707963267948965E+000) * 70.0 + q1_dot;

if (eml_S > 0.0) { /* '<S2>:1:28' */

/* '<S2>:1:29' */

eml_L = 1;

} else if (eml_S < 0.0) { /* '<S2>:1:30' */

/* '<S2>:1:31' */

eml_L = -1;

} else { /* '<S2>:1:33' */

eml_L = 0;

}

/* '<S2>:1:36' */

U_sw = (-((((((eml_T * q2_dot - (-eml_T) * q1_dot * eml_d / 0.0011) * q1_dot + (eml_T * q2_dot + eml_T * q1_dot) * q2_dot) + ((cos(q1 + q2) *

1.009449 + 4.6165860000000007E+000 * cos(q1)) - cos(q1 + q2) * 1.009449 * eml_d / 0.0011)) / eml_d11_bar + 24.0 * q1_dot) + 0.7 * (real_T)eml_L) + 77.7 * eml_S)) / (1.0 / eml_d11_bar);

/ /* DiscretePulseGenerator: '<Root>/Pulse Generator' */

led = ((real_T)LQR1_DWork.clockTickCounter < LQR1_P.PulseGenerator_Duty)

(LQR1_DWork.clockTickCounter >= 0) ? LQR1_P.PulseGenerator_Amp : 0.0;

if ((real_T)LQR1_DWork.clockTickCounter >= LQR1_P.PulseGenerator_Period - 1.0)

{ LQR1_DWork.clockTickCounter = 0;

} else { LQR1_DWork.clockTickCounter = LQR1_DWork.clockTickCounter + 1;

} }

/* Model initialize function */

void LQR1_initialize(void) {

/* Registration code */

/* initialize error status */

rtmSetErrorStatus(LQR1_M, (NULL));

/* block I/O */

/* exported global signals */

led = 0.0;

U_sw = 0.0;

U_blan = 0.0;

/* states (dwork) */

(void) memset((void *)&LQR1_DWork, 0, sizeof(D_Work_LQR1));

/* external inputs */

q1 = 0.0;

q1_dot = 0.0;

q2 = 0.0;

q2_dot = 0.0;

/* Start for DiscretePulseGenerator: '<Root>/Pulse Generator' */

LQR1_DWork.clockTickCounter = 0;

}

/* Model terminate function */

void LQR1_terminate(void) {

/* (no terminate code required) */

}

* File trailer for Real-Time Workshop generated code.

* [EOF]

*/ Hàm ngắt:

nt check=1;

int check1s=1;

int temp=0;

void TIM2_IRQHandler(void) {

static uint32_t time=0;

if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) {

if(++time==8)///////////////////////TAO XUNG 8MS //if(++time==1000)/////////////////////// enable here to set timer TAO XUNG 1S {

check=1;

time = 0;

temp=temp+1;

}

if(temp==70) {

check1s=0;

temp=0;

} TIM_ClearITPendingBit(TIM2, TIM_IT_Update);

} } int check1=0;

int check3=0;

void USART2_IRQHandler(void) { //

char c;

if(USART_GetITStatus(USART2, USART_IT_RXNE)==SET)

{ c=USART_ReceiveData(USART2);

if(c==8) {check1=1;}

} }

Chương trình nhận dạng thông số Pendubot:

filename = 'dulieu_mau.xlsx';

fid=fopen('D:\CAOHOC\luan van\dulieu_nhan_dang\DULIEU4_OK\dulieu261.txt','r');

data=fscanf(fid,'%5d');

fclose(fid);

%m=length(data)/25;

n = 20; %5 La chieu dai mot mau du lieu q1 = zeros(1,n);

dq1 = zeros(1,n);

q2 = zeros(1,n);

dq2 = zeros(1,n);

U_dk = zeros(1,n);

tau = zeros(1,n);

for i=1:n j = i-1;

q1(1,i) = data(5*j+1)/1000;

dq1(1,i) = data(5*j+2)/100;

q2(1,i) = data(5*j+3)/1000;

dq2(1,i) = data(5*j+4)/100;

tau(1,i)= data(5*j+5)/100;

end

%--- tau = tau'; %%%chyen vi

q1 = q1';

dq1 = dq1';

q2 = q2';

dq2 = dq2';

%%--- g=9.804; %(SI Units = 9.804)

dL1 = (.5*dq1.^2);

dL2 = (.5*dq1.^2 + dq1.*dq2 +.5*dq2.^2);

dL3 = (cos(q2).*(dq1.^2 + dq1.*dq2));

dL4 = (g*sin(q1));

dL5 = (g*sin(q1+q2));

taudq1 = tau.*dq1; %tau is the open loop control effort

for i = 1:n-1 DL(i,1) = dL1(i+1)-dL1(i);

DL(i,2) = dL2(i+1)-dL2(i);

DL(i,3) = dL3(i+1)-dL3(i);

DL(i,4) = dL4(i+1)-dL4(i);

DL(i,5) = dL5(i+1)-dL5(i);

Itq(i,1) = (taudq1(i+1)+taudq1(i))*0.01*0.5;

end Theta = nnls(DL,Itq);

Theta1=Theta';

xlswrite(filename,Theta1)

Chương trình tuyến tính hóa Pendubot:

K=zeros(1,4);

%theta=[0.0078 0.0026 0.0033 0.7125 0.2002];

%theta=[theta1 theta2 theta3 theta4 theta5];

theta=[0.0037 0.0011 0.0010 0.4706 0.1029];

g=9.81;

x1d=pi/2;

x2d=0;

x3d=0;

x4d=0;

ud=0;

T=-theta(3)*sin(x3);

d11=theta(1)+theta(2)+2*theta(3)*cos(x3);

d12=theta(2)+theta(3)*cos(x3);

d21=theta(2)+theta(3)*cos(x3);

d22=theta(2);

g1=theta(4)*g*cos(x1)+theta(5)*g*cos(x1+x3);

g2=theta(5)*g*cos(x1+x3);

f1=-(T*d12*x2^2 + 2*T*d22*x2*x4 + T*d22*x4^2 - d12*g2 + d22*g1 - d22*u)/(d11*d22 - d12*d21);

f2=(T*d11*x2^2 + 2*T*d21*x2*x4 + T*d21*x4^2 - d11*g2 + d21*g1 - d21*u)/(d11*d22 - d12*d21);

%%%%%%%%%%%%%%%%%%%%%%TINH MA TRAN A

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

a21=diff(f1,x1);

a22=diff(f1,x2);

a23=diff(f1,x3);

a24=diff(f1,x4);

a21kq=subs(a21,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

a22kq=subs(a22,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

a23kq=subs(a23,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

a24kq=subs(a24,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%

a41=diff(f2,x1);

a42=diff(f2,x2);

a43=diff(f2,x3);

a44=diff(f2,x4);

a41kq=subs(a41,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

a42kq=subs(a42,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

a43kq=subs(a43,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

a44kq=subs(a44,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

%%%%%%%%%%%%%%TINH MA TRAN B%%%%%%%%%%%%%

b2=diff(f1,u);

b4=diff(f2,u);

b2kq=subs(b2,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

b4kq=subs(b4,{x1,x2,x3,x4,u},{x1d,x2d,x3d,x4d,ud});

A=[0,1,0,0;a21kq,a22kq,a23kq,a24kq;0,0,0,1;a41kq,a42kq,a43kq,a44kq];

B=[0;b2kq;0;b4kq];

Q=[10000,0,0,0;0,1,0,0;0,0,10000,0;0,0,0,1];

R=1;

C=[1 0 0 0;0 0 1 0];

K=lqrd(A,B,Q,R,0.01);

ob=rank(obsv(A,C));

ob1=rank(ctrb(A,B));