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