pmsm-control/test_sw/main_pmsm.c

   1 /**
   2  * \file main_pmsm.c
   3  * \author Martin Prudek
   4  * \brief Mainfile pro pmsm control.
   5  */
   6
   7 #ifndef NULL
   8 #define NULL (void*) 0
   9 #endif /*NULL*/
  10
  11
  12 #include <stdlib.h>     /*exit*/
  13 #include <signal.h>     /*signal handler Ctrl+C*/
  14 #include <stdio.h>      /*printf*/
  15 #include <sched.h>      /*sheduler*/
  16 #include <unistd.h>     /*usleep*/
  17 #include <pthread.h>    /*threads*/
  18 #include <time.h>       /*nanosleep*/
  19 #include <string.h>
  20
  21 #include "rpin.h"       /*gpclk*/
  22 #include "rp_spi.h"     /*spi*/
  23 #include "misc.h"       /*structure for priorities*/
  24 #include "pxmc_sin_fixed.h"     /*to test sin commutation */
  25 #include "pmsm_state.h"
  26 #include "cmd_proc.h"
  27
  28 #define MAX_DUTY        170
  29 #define PID_P           0.3
  30 #define PID_P_S         0.3
  31
  32 #define PRIOR_KERN      50
  33 #define PRIOR_HIGH      49
  34 #define PRIOR_LOW       20
  35
  36 #define THREAD_SHARED   0
  37 #define INIT_VALUE      1       /*init value for semaphor*/
  38
  39
  40 #define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */
  41
  42
  43 struct rpi_in data;
  44 struct rpi_state rps={
  45         .spi_dat=&data,
  46         .test=0,
  47         .pwm1=0,.pwm2=0, .pwm3=0,
  48         .pwm1=0, .t_pwm2=0, .t_pwm3=0,
  49         .commutate=0,
  50         .duty=0,                        /* duty cycle of pwm */
  51         .index_dist=0,          /* distance to index position */
  52         .index_ok=0,
  53         .tf_count=0,            /*number of transfer*/
  54         .desired_pos=0,         /* desired position */
  55         .pos_reg_ena=0,
  56         .desired_spd=0,
  57         .spd_reg_ena=0
  58 };
  59
  60 /**
  61  * \brief Initilizes GPCLK.
  62  */
  63 int clk_init()
  64 {
  65         initialise(); /*namapovani gpio*/
  66         initClock(PLLD_500_MHZ, 10, 0);
  67         gpioSetMode(4, FSEL_ALT0);
  68         return 0;
  69 }
  70 /*
  71  * \brief Terminates GPCLK.
  72  */
  73
  74 inline void clk_disable(){
  75         termClock(0);
  76 }
  77 /*
  78  * \brief
  79  * Count minimum value of three numbers.
  80  * Input values must be in range <-2^28;2^28>.
  81  */
  82 int32_t min(int32_t x, int32_t y, int32_t z){
  83         int32_t diff,sign;
  84
  85         diff=x-y; /*rozdil*/
  86         sign=(*((uint32_t*)&diff))>>31; /*znamenko -> detekuje, ze y je vetsi*/
  87         x=y+sign*diff; /*ulozime mensi cislo, pokud sign>0, pak diff<0 */
  88
  89         diff=x-z; /*rozdil*/
  90         sign=(*((uint32_t*)&diff))>>31; /*znamenko -> detekuje, ze z je vetsi*/
  91         x=z+sign*diff; /*ulozime mensi cislo, pokud sign>0, pak diff<0 */
  92
  93         return x;
  94 }
  95 /*
  96  * \brief
  97  * Pripravi psi buffer
  98  */
  99 void prepare_tx(uint8_t * tx){
 100
 101         /*Data format:
 102          * tx[4] - bity 95 downto 88 - bits that are sent first
 103          * tx[5] - bity 87 downto 80
 104          * tx[6] - bity 79 downto 72
 105          * tx[7] - bity 71 downto 64
 106          * tx[8] - bity 63 downto 56
 107          * tx[9] - bity 55 downto 48
 108          * tx[10] - bity 47 downto 40
 109          * tx[11] - bity 39 downto 32
 110          * tx[12] - bity 31 downto 24
 111          * tx[13] - bity 23 downto 16
 112          * tx[14] - bity 15 downto 8
 113          * tx[15] - bity 7 downto 0
 114          *
 115          * bit 95 - ADC reset
 116          * bit 94 - enable PWM1
 117          * bit 93 - enable PWM2
 118          * bit 92 - enable PWM3
 119          * bit 91 - shutdown1
 120          * bit 90 - shutdown2
 121          * bit 89 - shutdown3
 122          *      .
 123          *      .
 124          *      Unused
 125          *      .
 126          *      .
 127          * bits 47 .. 32 - match PWM1
 128          * bits 31 .. 16 - match PWM2
 129          * bits 15 .. 0  - match PWM3
 130          */
 131
 132
 133         uint16_t tmp;
 134
 135         /* keep the 11-bit cap*/
 136
 137         if (rps.pwm1>2047) rps.pwm1=2047;
 138         if (rps.pwm2>2047) rps.pwm2=2047;
 139         if (rps.pwm3>2047) rps.pwm3=2047;
 140
 141         tx[0]=rps.test; /*bit 94 - enable PWM1*/
 142
 143         /*now we have to switch the bytes due to endianess */
 144         /* ARMv6 & ARMv7 instructions are little endian */
 145         /*pwm1*/
 146         tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/
 147         tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/
 148
 149         /*pwm2*/
 150         tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/
 151         tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/
 152
 153         /*pwm3*/
 154         tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/
 155         tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/
 156
 157
 158 }
 159
 160
 161 /**
 162  * \brief Signal handler pro Ctrl+C
 163  */
 164 void appl_stop(){
 165         uint8_t tx[16];
 166         sem_wait(&rps.thd_par_sem);
 167
 168         memset(tx,0,16*sizeof(int));
 169         rps.pwm1=0;
 170         rps.pwm2=0;
 171         rps.pwm3=0;
 172         prepare_tx(tx);                 /*save the data to send*/
 173         data=spi_read(tx);
 174
 175         spi_disable();
 176         clk_disable();
 177         /*muzeme zavrit semafor*/
 178         sem_destroy(&rps.thd_par_sem);
 179         printf("\nprogram bezpecne ukoncen\n");
 180 }
 181
 182 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
 183         data->pozice_raw=data->pozice;
 184         data->pozice-=offset->pozice;
 185         return;
 186 }
 187 /*
 188  * \brief
 189  * Transformace pro uhel pocitany po smeru hodinovych rucicek
 190  */
 191 void dq2alphabeta(int32_t *alpha, int32_t *beta, int d, int q, int32_t sin, int32_t cos){
 192         *alpha=cos*d+sin*q;
 193         *beta=-sin*d+cos*q;
 194         return;
 195 }
 196 void alphabeta2pwm3(int32_t * ia, int32_t * ib, int32_t *ic,int32_t alpha, int32_t beta){
 197         *ia=alpha;
 198         *ib=-alpha/2+beta*887/1024;
 199         *ic=-alpha/2-beta*887/1024;
 200 }
 201 /*
 202  * \brief
 203  * Preocita napeti na jednotlivych civkach na napeti,
 204  *      ktera budou privedena na svorky motoru.
 205  *      Tedy na A(yel)-pwm1, B(red)-pwm2, C(blk)-pwm3
 206  */
 207 void transDelta(int32_t * u1, int32_t * u2, int32_t *u3, int32_t ub , int32_t uc){
 208         int32_t t;
 209
 210         /*vypocte napeti tak, aby odpovidaly rozdily*/
 211         *u1=uc;
 212         *u2=uc+ub;
 213         *u3=0;
 214
 215         /*najde zaporne napeti*/
 216         t=min(*u1,*u2,*u3);
 217
 218         /*dorovna zaporna napeti na nulu*/
 219         *u1-=t;
 220         *u2-=t;
 221         *u3-=t;
 222 }
 223 void inv_trans_comm(int duty){
 224         uint32_t pos;
 225         int32_t sin, cos;
 226         int32_t alpha, beta;
 227         int32_t pwma,pwmb,pwmc;
 228         pos=rps.index_dist;
 229         /*melo by byt urceno co nejpresneji, aby faze 'a' splyvala s osou 'alpha'*/
 230         pos+=717;
 231         /*use it as cyclic 32-bit logic*/
 232         pos*=4294967;
 233         pxmc_sincos_fixed_inline(&sin, &cos, pos, 16);
 234         dq2alphabeta(&alpha, &beta,0,duty, sin, cos);
 235         alpha>>=16;
 236         beta>>=16;
 237         alphabeta2pwm3(&pwma,&pwmb, &pwmc,alpha,beta);
 238
 239         if (pwma<0) pwma=0;
 240         if (pwmb<0) pwmb=0;
 241         if (pwmc<0) pwmc=0;
 242
 243
 244         rps.t_pwm1=(uint16_t)pwma;
 245         rps.t_pwm3=(uint16_t)pwmb;
 246         rps.t_pwm2=(uint16_t)pwmc;
 247 }
 248
 249 void inv_trans_comm_2(int duty){
 250         uint32_t pos;
 251         int32_t sin, cos;
 252         int32_t alpha, beta;
 253         int32_t ua,ub,uc;
 254         int32_t ia,ib,ic;
 255         int32_t u1,u2,u3;
 256         pos=rps.index_dist;
 257
 258         pos+=960; /*zarovnani faze 'a' s osou 'alpha'*/
 259
 260         /*pro výpočet sin a cos je pouzita 32-bit cyklicka logika*/
 261         pos*=4294967;
 262         pxmc_sincos_fixed_inline(&sin, &cos, pos, 16);
 263
 264         dq2alphabeta(&alpha, &beta,0,duty, sin, cos);
 265         alpha>>=16;
 266         beta>>=16;
 267
 268         alphabeta2pwm3(&ia,&ib, &ic,alpha,beta);
 269
 270         ua=ia;
 271         ub=ib;
 272         uc=ic;
 273
 274         transDelta(&u1,&u2, &u3,ub,uc);
 275
 276         rps.pwm1=(uint16_t)u1;
 277         rps.pwm2=(uint16_t)u2;
 278         rps.pwm3=(uint16_t)u3;
 279 }
 280
 281 /**
 282  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
 283  */
 284 void * pos_monitor(void* param){
 285         while(1){
 286                 printData(&rps);
 287                 usleep(1000000);        /*1 Hz*/
 288         }
 289         return (void*)0;
 290 }
 291 /*
 292  * \brief
 293  * Multiplication of 11 bit
 294  * Zaporne vysledky prvede na nulu.
 295  */
 296 inline uint16_t mult_cap(int32_t s,int d){
 297         int j;
 298         int res=0;
 299         for(j=0;j!=11;j++){
 300                 /* multiplicate as if maximum sinus value was unity */
 301                 res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
 302         }
 303         return res;
 304 }
 305 inline
 306 int sin_commutator(int duty){
 307         #define DEGREE_60        715827883
 308         #define DEGREE_120      1431655765
 309         #define DEGREE_180      2147483648
 310         #define DEGREE_240      2863311531
 311         #define DEGREE_300      3579139413
 312         uint32_t j,pos;
 313         int32_t sin;
 314         pos=rps.index_dist;
 315         int32_t pwm;
 316         /*aby prictene uhly mohla byt kulata cisla, musime index posunout*/
 317         pos+=38;
 318         /*use it as cyclic 32-bit logic*/
 319         pos*=4294967;
 320         if (duty>=0){   /*clockwise rotation*/
 321                 /* 1st phase */
 322                 sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/
 323                 pwm=sin*duty/1024;
 324                 if (pwm<0) pwm=0;
 325                 rps.pwm1=(uint16_t)pwm;
 326
 327                 /* 2nd phase */
 328                 sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/
 329                 pwm=sin*duty/1024;
 330                 if (pwm<0) pwm=0;
 331                 rps.pwm2=(uint16_t)pwm;
 332
 333                 /* 3rd phase */
 334                 sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/
 335                 pwm=sin*duty/1024;
 336                 if (pwm<0) pwm=0;
 337                 rps.pwm3=(uint16_t)pwm;
 338         }else{
 339                 duty=-duty;
 340
 341                 /* 1st phase */
 342                 sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/
 343                 pwm=sin*duty/1024;
 344                 if (pwm<0) pwm=0;
 345                 rps.pwm1=(uint16_t)pwm;
 346
 347                 /* 2nd phase */
 348                 sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/
 349                 pwm=sin*duty/1024;
 350                 if (pwm<0) pwm=0;
 351                 rps.pwm2=(uint16_t)pwm;
 352
 353                 /* 3rd phase */
 354                 sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/
 355                 pwm=sin*duty/1024;
 356                 if (pwm<0) pwm=0;
 357                 rps.pwm3=(uint16_t)pwm;
 358         }
 359         return 0;
 360 }
 361 /*
 362  * \brief
 363  * Test function to be placed in controll loop.
 364  * Switches PWM's at point where they produce same force.
 365  * This points are found thanks to IRC position,
 366  */
 367 inline
 368 void simple_ind_dist_commutator(int duty){
 369         if (duty>=0){ /* clockwise - so that position increase */
 370                 /* pwm3 */
 371                 if ((rps.index_dist>=45 && rps.index_dist<=373) ||
 372                 (rps.index_dist>=1048 && rps.index_dist<=1377)){
 373                         rps.pwm1=0;
 374                         rps.pwm2=0;
 375                         rps.pwm3=duty;
 376                         /* pwm1 */
 377                 }else if ((rps.index_dist>=373 && rps.index_dist<=711) ||
 378                 (rps.index_dist>=1377 && rps.index_dist<=1711)){
 379                         rps.pwm1=duty;
 380                         rps.pwm2=0;
 381                         rps.pwm3=0;
 382                         /* pwm2 */
 383                 }else if ((rps.index_dist>=0 && rps.index_dist<=45) ||
 384                 (rps.index_dist>=711 && rps.index_dist<=1048) ||
 385                 (rps.index_dist>=1711 && rps.index_dist<=1999)){
 386                         rps.pwm1=0;
 387                         rps.pwm2=duty;
 388                         rps.pwm3=0;
 389                 }
 390         }else{  /*counter-clockwise - position decrease */
 391                 /* pwm3 */
 392                 if ((rps.index_dist>=544 && rps.index_dist<=881) ||
 393                 (rps.index_dist>=1544 && rps.index_dist<=1878)){
 394                         rps.pwm1=0;
 395                         rps.pwm2=0;
 396                         rps.pwm3=-duty;
 397                         /* pwm1 */
 398                 }else if ((rps.index_dist>=0 && rps.index_dist<=211) ||
 399                 (rps.index_dist>=881 && rps.index_dist<=1210) ||
 400                 (rps.index_dist>=1878 && rps.index_dist<=1999)){
 401                         rps.pwm1=-duty;
 402                         rps.pwm2=0;
 403                         rps.pwm3=0;
 404                         /* pwm2 */
 405                 }else if ((rps.index_dist>=211 && rps.index_dist<=544) ||
 406                 (rps.index_dist>=1210 && rps.index_dist<=1544)){
 407                         rps.pwm1=0;
 408                         rps.pwm2=-duty;
 409                         rps.pwm3=0;
 410                 }
 411         }
 412 }
 413 /*
 414  * \brief
 415  * Test function to be placed in controll loop.
 416  * Switches PWM's at point where they produce same force
 417  */
 418 inline void simple_hall_commutator(int duty){
 419         if (duty>=0){ /* clockwise - so that position increase */
 420                 /* pwm3 */
 421                 if (data.hal2 && !data.hal3){
 422                         rps.pwm1=0;
 423                         rps.pwm2=0;
 424                         rps.pwm3=duty;
 425                         /* pwm1 */
 426                 }else if (data.hal1 && !data.hal2){
 427                         rps.pwm1=duty;
 428                         rps.pwm2=0;
 429                         rps.pwm3=0;
 430                         /* pwm2 */
 431                 }else if (!data.hal1 && data.hal3){
 432                         rps.pwm1=0;
 433                         rps.pwm2=duty;
 434                         rps.pwm3=0;
 435                 }
 436         }else{  /*counter-clockwise - position decrease */
 437                 /* pwm3 */
 438                 if (!data.hal2 && data.hal3){
 439                         rps.pwm1=0;
 440                         rps.pwm2=0;
 441                         rps.pwm3=-duty;
 442                         /* pwm1 */
 443                 }else if (!data.hal1 && data.hal2){
 444                         rps.pwm1=-duty;
 445                         rps.pwm2=0;
 446                         rps.pwm3=0;
 447                         /* pwm2 */
 448                 }else if (data.hal1 && !data.hal3){
 449                         rps.pwm1=0;
 450                         rps.pwm2=-duty;
 451                         rps.pwm3=0;
 452                 }
 453         }
 454 }
 455 /**
 456  * \brief
 457  * Computation of distance to index.
 458  *
 459  * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
 460  * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
 461  *      to je 3999 bodu
 462  *      =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
 463  *              signalu indexu
 464  */
 465 void comIndDist(){
 466         uint16_t pos = 0x0FFF & data.pozice_raw;
 467         uint16_t dist;
 468         uint16_t index = data.index_position;
 469
 470         if (index<1999){                /*index e<0,1998> */
 471                 if (pos<index){                 /*pozice e<0,index-1> */
 472                         /*proti smeru h.r. od indexu*/
 473                         dist=pos+2000-index;
 474                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 475                         /*po smeru h.r. od indexu*/
 476                         dist=pos-index;
 477                 }else if (pos<index+2096){      /*pozice e<index+2000,index+2095> */
 478                         goto index_lost;
 479                 }else{                          /*pozice e<index+2096,4095> */
 480                         /*proti smeru h.r. od indexu - podtecena pozice*/
 481                         dist=pos-index-2096;
 482                 }
 483         }else if (index<=2096){         /*index e<1999,2096>*/
 484                 if (pos<index-1999){            /*pozice e<0,index-2000> */
 485                         goto index_lost;
 486                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 487                         /*proti smeru h.r. od indexu*/
 488                         dist=pos+2000-index;
 489                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 490                         /*po smeru h.r. od indexu*/
 491                         dist=pos-index;
 492                 }else {                         /*pozice e<index+2000,4095> */
 493                         goto index_lost;
 494                 }
 495         }else{                          /*index e<2097,4095> */
 496                 if (pos<=index-2097){           /*pozice e<0,index-2097> */
 497                         /*po smeru h.r. od indexu - pretecena pozice*/
 498                         dist=4096+pos-index;
 499                 }else if (pos<index-1999){      /*pozice e<index-2096,index-2000> */
 500                         goto index_lost;
 501                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 502                         /*proti smeru h.r. od indexu*/
 503                         dist=pos+2000-index;
 504                 }else{                          /*pozice e<index,4095> */
 505                         /*po smeru h.r. od indexu*/
 506                         dist=pos-index;
 507                 }
 508         }
 509
 510         rps.index_dist = dist;
 511         return;
 512
 513         index_lost:
 514                 rps.index_ok=0;
 515                 return;
 516 }
 517 /*
 518  * \brief
 519  * Very simple PID regulator.
 520  * Now only with P-part so that the error doesnt go to zero.
 521  * TODO: add anti-wind up and I and D parts
 522  */
 523 inline void pos_pid(){
 524         int duty_tmp;
 525         duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice);
 526         if (duty_tmp>MAX_DUTY){
 527                 rps.duty=MAX_DUTY;
 528         }else if (duty_tmp<-MAX_DUTY){
 529                 rps.duty=-MAX_DUTY;
 530         }else{
 531                 rps.duty = duty_tmp;
 532         }
 533 }
 534 /*
 535  * \brief
 536  * Very simple PID regulator.
 537  * Now only with P-part so that the error doesnt go to zero.
 538  * FIXME: make better
 539  */
 540 inline void spd_pid(){
 541         int duty_tmp;
 542         signed long int speed=rps.spi_dat->pozice-rps.old_pos;
 543         duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice);
 544         if (duty_tmp>MAX_DUTY){
 545                 rps.duty=MAX_DUTY;
 546         }else if (duty_tmp<-MAX_DUTY){
 547                 rps.duty=-MAX_DUTY;
 548         }else{
 549                 rps.duty = duty_tmp;
 550         }
 551 }
 552 /*
 553  * \brief
 554  * Feedback loop.
 555  * TODO: replace bunch of 'IFs' with Object-like pattern
 556  */
 557 void * read_data(void* param){
 558         int i;
 559         struct rpi_in pocatek;
 560         struct timespec t;
 561         int interval = 1000000; /* 1ms ~ 1kHz*/
 562         uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ;
 563         char first=1;
 564         uint16_t last_index;                            /*we have index up-to date*/
 565         pocatek = spi_read(tx);
 566         clock_gettime(CLOCK_MONOTONIC ,&t);
 567         /* start after one second */
 568         t.tv_sec++;
 569                 while(1){
 570                         /* wait until next shot */
 571                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
 572                         sem_wait(&rps.thd_par_sem);             /*---take semaphore---*/
 573
 574                         rps.old_pos=rps.spi_dat->pozice;        /*old position*/
 575                         prepare_tx(tx);                 /*save the data to send*/
 576                         data = spi_read(tx);            /*exchange data*/
 577                         /*subtract initiate postion */
 578                         rps.tf_count++;
 579                         substractOffset(&data,&pocatek);
 580
 581                         if (!rps.index_ok){
 582                                 if (first){
 583                                         last_index=data.index_position;
 584                                         first=0;
 585                                 }else if (last_index!=data.index_position){
 586                                         rps.index_ok=1;
 587                                         comIndDist();   /*vypocet vzdalenosti indexu*/
 588                                 }
 589                         }else{ /*index je v poradku*/
 590                                 comIndDist();           /*vypocet vzdalenosti indexu*/
 591                         }
 592                         /* pocitame sirku plneni podle potreb rizeni*/
 593                         if (rps.pos_reg_ena){
 594                                 pos_pid();
 595                         }
 596                         /* sirka plneni prepoctena na jednotlive pwm */
 597                         if (rps.index_ok && rps.commutate){
 598                                 /*simple_ind_dist_commutator(rps.duty);*/
 599                                 /*sin_commutator(rps.duty);*/
 600                                 inv_trans_comm(rps.duty);
 601                                 inv_trans_comm_2(rps.duty);
 602                         }else if(!rps.index_ok && rps.commutate){
 603                                 simple_hall_commutator(rps.duty);
 604                         }
 605                         sem_post(&rps.thd_par_sem);             /*--post semaphore---*/
 606
 607                         /* calculate next shot */
 608                         t.tv_nsec += interval;
 609
 610                         while (t.tv_nsec >= NSEC_PER_SEC) {
 611                                 t.tv_nsec -= NSEC_PER_SEC;
 612                                 t.tv_sec++;
 613                         }
 614
 615                 }
 616 }
 617
 618
 619
 620 /**
 621  * \brief Main function.
 622  */
 623
 624 int main(){
 625         pthread_t base_thread_id;
 626         clk_init();             /* inicializace gpio hodin */
 627         spi_init();             /* iniicializace spi*/
 628
 629         /*semafor pro detekci zpracovani parametru vlaken*/
 630         sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
 631         setup_environment();
 632
 633         base_thread_id=pthread_self();
 634
 635         /*main control loop*/
 636         create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
 637
 638         /*monitor of current state*/
 639         create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
 640
 641         /*wait for commands*/
 642         poll_cmd(&rps);
 643
 644         return 0;
 645 }