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