Программа "control_system"

Текст программы

Листов 4

РП.68124-01.12 01

2009

Аннотация

Данный документ содержит текст программы "control_system", которая предназначена для управления двузвенным мини-роботом, представляющим собой колебательную системы типа "качели". Документ содержит тексты основного файла "control_system. c", написанного на языке C.

Текст программы

/*********************************************************************

*

* control_system. c: программа управления сервоприводом

*

*

* Автор: Сичевая Э. А.

*

********************************************************************/

#include <stdio. h>

#include <stdlib. h>

#include <unistd. h>

#include <sched. h>

#include <errno. h>

#include <sys/io. h>

#include <pthread. h>

#include <math. h>

#define ge 9.8 // ускорение свободного падения

#define f00.5 // значение по умолчанию для начального угла

#define x04 // значение по умолчанию для начальной длины подвеса

#define dx00 // начальная скорость подвеса

#define ro0.15 // значение по умолчанию коэффициента трения

#define hd0.0002 // значение по умолчанию для шага интегрирования

#define md0.5 // значение по умолчанию для массы подвеса

// значения по умолчанию для геометрических параметров объекта управления

#define l110

#define l210

#define L22

/*

Структура для хранения информации о текущем положении точки,

а также текущем и предыдущем значении первой и второй производной

*/

typedef struct {

double val;

double d [2];

double d2 [2];

} point_data;

void SU_Regulator (point_data*, point_data*);

double SU_Converter (double x);

int main (int argc, char* argv [])

{

/* Для значений положения и угла */

point_datax, f;

int iter, i;

double h, m;

struct timespec TimeSpec;

struct sched_param SchedParam;

struct timespec ttt, ret;

FILE *pFOut;

int nNumIter, nDivider;

nNumIter = 1000000;

nDivider = 100;

h = hd;

m = md;

/*

открываем выходной файл для перезаписи

*/

if ( (pFOut = fopen ("output. dat","w")) == NULL)

{

if ( (pFOut = fopen ("output. dat","a")) == NULL)

{

printf ("Cannot open file. failed");

return - 1;

}

}

/*обнуляем значения структур*/

memset (&SchedParam, 0, sizeof (struct sched_param));

memset (&TimeSpec, 0, sizeof (struct timespec));

memset (&ttt, 0, sizeof (struct timespec));

memset (&ret, 0, sizeof (struct timespec));

SchedParam. sched_priority = 2;

/*

производим вычисления

*/

/*выставляем дисциплину управления*/

if (sched_setscheduler (0, SCHED_FIFO, &SchedParam))

{

printf ("Error while setting scheduler parameters, exiting. \n");

return - 1;

}

/*получаем привелегии для доступа к устройствам ввода-вывода*/

if (ioperm (LPT_BASE, 1, 1))

{

printf ("Error while obtaining IO permissions, exiting. \n");

return - 1;

}

/*

устанавливаем начальные значения

*/

x. d2 [0] = 0;

x. d [0] = dx0;

x. val = x0;

f. d2 [0] = 0;

f. d [0] = 0;

f. val = f0;

ttt. tv_sec = 0;

ttt. tv_nsec = 15000000;

TimeSpec. tv_sec = 0;

TimeSpec. tv_nsec = 66666; // количество микросекунд, приходящихся на 1 градус

for (iter = 0; iter < nNumIter; iter++)

{

/*выводим 1 в 1 дата-вывод LPT-порта*/

outb (0x01, LPT_BASE);

/*ждем до окончания импульса в зависимости от выводимого угла (по таблице angle_var) */

for (i = 0; i < (int) f. val; i++)

{

nanosleep (&TimeSpec);

}

outb (0x00, LPT_BASE);

/*

вычисления текущего положения модели

*/

f. d2 [1] = x. d [0] *f. d [0] /x. val - ge*f. val/x. val - (ro * f. d [0]) / (m*x. val*x. val);

f. d [1] = f. d [0] + h/2 * (f. d2 [1] +f. d2 [0]);

f. val = f. val + h/2 * (f. d [1] + f. d [0]);

f. d2 [0] = f. d2 [1];

f. d [0] = f. d [1];

if (! (iter % nDivider))

{

SU_Regulator (&x, &f);

fprintf (pFOut, "%i \t %f \t %f \t %f \t %f \t\n", iter, f. val, x. val, x. d [1], SU_Converter (x. val));

}

/*засыпаем на 15 мс*/

nanosleep (&ttt, NULL);

}

return 0;

}

/**************************************************************************

*

* void SU_Regulator (point_data* x, point_data* f)

* Описание функции: Функция регулятора. Производит вычисление

*значения управляющего сигнала.

*

* Входные параметры: f - указатель на структуру типа point_data, в

*которой хранятся значения угла подвеса,

*его скорости и ускорения

* Выходные параметры: x - указатель на структуру типа point_data, в

*которой хранятся значения положения подвеса,

*его скорости и ускорения - сигнал управления

*

**************************************************************************/

void SU_Regulator (point_data* x, point_data* f)

{

doubleampl_f, /* амплитуда колебаний */

k_ampl, /* коэффициент усиления регулятора ускорения */

A_ampl, /* дополнительный коэфициент */

u_reg; /* управление */

signedsDf,/* Знак скорости изменения угла */

sF,/* Знак угла */

sD2x; /* Знак ускорения изменения положения подвеса */

k_ampl = 5;

A_ampl = 0.3;

/* Вычисляем знаки углов */

sDf = (f->d [1] > 0);

sF= (f->val> 0);

sD2x = sDf ^ sF; /* XOR */

sD2x = sD2x * 2 - 1; /* Приводим к базису (1; - 1) */

/* Вычисляем амплитуду угла */

ampl_f = (k_ampl*A_ampl) * (A_ampl - fabs (f->val));

x->d2 [1] = ampl_f*sD2x;

x->d [1] = x->d [0] + h/2 * (x->d2 [1] + x->d2 [0]);

x->val = x->val + h/2 * (x->d [1] + x->d [0]);

x->d2 [0] = x->d2 [1];

x->d [0] = x->d [1];

u_reg = f->d [1] * x->d [1];

return;

}

/**************************************************************************

*

* void SU_Regulator (point_data* x, point_data* f)

* Описание функции: Функция преобразования значения положения подвеса

*в угол поворота исполнительного механизма.

*

* Входные параметры: x - значение положения подвеса

*

* Выходные параметры: фактический угол поворота исполнительного механизма

*

**************************************************************************/

double SU_Converter (double x)

{

return acos ( ( (l1*l1) + (l2*l2) - (L-x) * (L-x)) / (2*l1*l2));

}

Приложение 4. Программа и методика испытаний