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