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 "pmsm_state.h"
  25 #include "cmd_proc.h"
  26 #include "controllers.h"
  27 #include "commutators.h"
  28
  29
  30
  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         //.MAX_DUTY=170,
  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         .alpha_offset=960
  65 };
  66
  67 /**
  68  * \brief Initilizes GPCLK.
  69  */
  70 int clk_init()
  71 {
  72         initialise(); /*namapovani gpio*/
  73         initClock(PLLD_500_MHZ, 10, 0);
  74         gpioSetMode(4, FSEL_ALT0);
  75         return 0;
  76 }
  77 /*
  78  * \brief Terminates GPCLK.
  79  */
  80
  81 inline void clk_disable(){
  82         termClock(0);
  83 }
  84
  85
  86 /*
  87  * \brief
  88  * Free logs
  89  */
  90 void freeLogs(){
  91         int r;
  92         if (rps.log_col_count){
  93                 for (r=0;r<LOG_ROWS;r++){
  94                         free(rps.logs[r]);
  95                 }
  96         }
  97         rps.log_col_count=0;
  98         rps.doLogs=0;
  99 }
 100
 101 /*
 102  * \brief
 103  * Makes log.
 104  */
 105 void makeLog(){
 106         int r;
 107         if (rps.log_col==MAX_LOGS-1){
 108                 rps.doLogs=0;
 109                 return;
 110         }
 111         rps.logs[0][rps.log_col]=(int)rps.tf_count;
 112         rps.logs[1][rps.log_col]=(int)rps.spi_dat->pozice;
 113
 114         rps.logs[2][rps.log_col]=(int)rps.pwm1;
 115         rps.logs[3][rps.log_col]=(int)rps.pwm2;
 116         rps.logs[4][rps.log_col]=(int)rps.pwm3;
 117         rps.logs[5][rps.log_col]=rps.duty;
 118
 119         rps.logs[6][rps.log_col]=rps.desired_spd;
 120         rps.logs[7][rps.log_col]=rps.speed;
 121
 122         rps.logs[8][rps.log_col]=(int)(rps.spi_dat->ch1/rps.spi_dat->adc_m_count);
 123         rps.logs[9][rps.log_col]=(int)(rps.spi_dat->ch2/rps.spi_dat->adc_m_count);
 124         rps.logs[10][rps.log_col]=(int)(rps.spi_dat->ch0/rps.spi_dat->adc_m_count);
 125
 126         rps.log_col++;
 127      /*
 128         if (rps.log_col==rps.log_col_count-1){
 129                 rps.log_col_count*=2;
 130                 rps.log_col_count%=MAX_LOGS;
 131                 for (r=0;r<LOG_ROWS;r++){
 132                         rps.logs[r]=realloc(rps.logs[r],rps.log_col_count*sizeof(int));
 133                         if (rps.logs[r]==NULL){
 134                                 rps.doLogs=0;
 135                                 rps.error=1;
 136                         }
 137                 }
 138         }
 139      */
 140 }
 141
 142
 143
 144
 145 /**
 146  * \brief Signal handler pro Ctrl+C
 147  */
 148 void appl_stop(){
 149         uint8_t tx[16];
 150         sem_wait(&rps.thd_par_sem);
 151
 152         memset(tx,0,16*sizeof(int));
 153         rps.pwm1=0;
 154         rps.pwm2=0;
 155         rps.pwm3=0;
 156         spi_read(&rps);
 157
 158         spi_disable();
 159         clk_disable();
 160         freeLogs();
 161         /*muzeme zavrit semafor*/
 162         sem_destroy(&rps.thd_par_sem);
 163         printf("\nprogram bezpecne ukoncen\n");
 164 }
 165
 166 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
 167         data->pozice=data->pozice_raw-offset->pozice_raw;
 168         return;
 169 }
 170
 171
 172
 173 /**
 174  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
 175  */
 176 void * pos_monitor(void* param){
 177         while(1){
 178                 printData(&rps);
 179                 usleep(1000000);        /*1 Hz*/
 180         }
 181         return (void*)0;
 182 }
 183 /*
 184  * \brief
 185  * Multiplication of 11 bit
 186  * Zaporne vysledky prvede na nulu.
 187  */
 188 inline uint16_t mult_cap(int32_t s,int d){
 189         int j;
 190         int res=0;
 191         for(j=0;j!=11;j++){
 192                 /* multiplicate as if maximum sinus value was unity */
 193                 res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
 194         }
 195         return res;
 196 }
 197
 198
 199 /**
 200  * \brief
 201  * Computation of distance to index.
 202  *
 203  * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
 204  * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
 205  *      to je 3999 bodu
 206  *      =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
 207  *              signalu indexu
 208  */
 209 void comIndDist(){
 210         uint16_t pos = 0x0FFF & data.pozice_raw;
 211         uint16_t dist;
 212         uint16_t index = data.index_position;
 213
 214         if (index<1999){                /*index e<0,1998> */
 215                 if (pos<index){                 /*pozice e<0,index-1> */
 216                         /*proti smeru h.r. od indexu*/
 217                         dist=pos+2000-index;
 218                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 219                         /*po smeru h.r. od indexu*/
 220                         dist=pos-index;
 221                 }else if (pos<index+2096){      /*pozice e<index+2000,index+2095> */
 222                         goto index_lost;
 223                 }else{                          /*pozice e<index+2096,4095> */
 224                         /*proti smeru h.r. od indexu - podtecena pozice*/
 225                         dist=pos-index-2096;
 226                 }
 227         }else if (index<=2096){         /*index e<1999,2096>*/
 228                 if (pos<index-1999){            /*pozice e<0,index-2000> */
 229                         goto index_lost;
 230                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 231                         /*proti smeru h.r. od indexu*/
 232                         dist=pos+2000-index;
 233                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 234                         /*po smeru h.r. od indexu*/
 235                         dist=pos-index;
 236                 }else {                         /*pozice e<index+2000,4095> */
 237                         goto index_lost;
 238                 }
 239         }else{                          /*index e<2097,4095> */
 240                 if (pos<=index-2097){           /*pozice e<0,index-2097> */
 241                         /*po smeru h.r. od indexu - pretecena pozice*/
 242                         dist=4096+pos-index;
 243                 }else if (pos<index-1999){      /*pozice e<index-2096,index-2000> */
 244                         goto index_lost;
 245                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 246                         /*proti smeru h.r. od indexu*/
 247                         dist=pos+2000-index;
 248                 }else{                          /*pozice e<index,4095> */
 249                         /*po smeru h.r. od indexu*/
 250                         dist=pos-index;
 251                 }
 252         }
 253
 254         rps.index_dist = dist;
 255         return;
 256
 257         index_lost:
 258                 rps.index_ok=0;
 259                 return;
 260 }
 261
 262
 263 /*
 264  * \brief
 265  * Computate speed.
 266  */
 267 void compSpeed(){
 268         signed long int spd;
 269         spd=rps.spi_dat->pozice-rps.old_pos[rps.tf_count%OLD_POS_NUM];
 270         rps.speed=(int32_t)spd;
 271 }
 272
 273
 274
 275 /*
 276  * \brief
 277  * Feedback loop.
 278  * TODO: replace bunch of 'IFs' with Object-like pattern
 279  */
 280 void * read_data(void* param){
 281         int i;
 282         struct rpi_in pocatek;
 283         struct rpi_state poc={
 284                 .spi_dat=&pocatek,
 285                 .test=0,
 286                 .pwm1=0, .pwm1=0, .pwm3=0
 287         };
 288         struct timespec t;
 289         int interval = 1000000; /* 1ms ~ 1kHz*/
 290         char first=1;
 291         uint16_t last_index;                            /*we have index up-to date*/
 292         spi_read(&poc);         /*pocatecni informace*/
 293         clock_gettime(CLOCK_MONOTONIC ,&t);
 294         /* start after one second */
 295         t.tv_sec++;
 296                 while(1){
 297                         /* wait until next shot */
 298                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
 299                         sem_wait(&rps.thd_par_sem);             /*---take semaphore---*/
 300
 301                         /*old positions*/
 302                         rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
 303                         spi_read(&rps);         /*exchange data*/
 304                         /*subtract initiate postion */
 305                         rps.tf_count++;
 306                         substractOffset(&data,poc.spi_dat);
 307                         compSpeed();                    /*spocita rychlost*/
 308
 309                         if (!rps.index_ok){
 310                                 if (first){
 311                                         last_index=data.index_position;
 312                                         first=0;
 313                                 }else if (last_index!=data.index_position){
 314                                         rps.index_ok=1;
 315                                         comIndDist();   /*vypocet vzdalenosti indexu*/
 316                                 }
 317                         }else{ /*index je v poradku*/
 318                                 comIndDist();           /*vypocet vzdalenosti indexu*/
 319                         }
 320
 321                         /* pocitame sirku plneni podle potreb rizeni*/
 322                         if (rps.pos_reg_ena){           /*pozicni rizeni*/
 323                                 pos_pid(&rps);
 324                         }else if(rps.spd_reg_ena){      /*rizeni na rychlost*/
 325                                 spd_pid(&rps);
 326                         }
 327
 328                         /* sirka plneni prepoctena na jednotlive pwm */
 329                         if (rps.index_ok && rps.commutate){
 330                                 /*simple_ind_dist_commutator(rps.duty);*/
 331                                 /*sin_commutator(rps.duty);*/
 332                                 inv_trans_comm(&rps);
 333                                 inv_trans_comm_2(&rps);
 334                         }else if(!rps.index_ok && rps.commutate){
 335                                 simple_hall_commutator(&rps);
 336                         }
 337
 338                         /*zalogujeme hodnoty*/
 339                         if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
 340                                 makeLog();
 341                         }
 342
 343                         sem_post(&rps.thd_par_sem);             /*--post semaphore---*/
 344
 345                         /* calculate next shot */
 346                         t.tv_nsec += interval;
 347
 348                         while (t.tv_nsec >= NSEC_PER_SEC) {
 349                                 t.tv_nsec -= NSEC_PER_SEC;
 350                                 t.tv_sec++;
 351                         }
 352
 353                 }
 354 }
 355
 356
 357 /**
 358  * \brief Main function.
 359  */
 360
 361 int main(){
 362         pthread_t base_thread_id;
 363         clk_init();             /* inicializace gpio hodin */
 364         spi_init();             /* iniicializace spi*/
 365
 366         /*semafor pro detekci zpracovani parametru vlaken*/
 367         sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
 368         setup_environment();
 369
 370         base_thread_id=pthread_self();
 371
 372         /*main control loop*/
 373         create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
 374
 375         /*monitor of current state*/
 376         create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
 377
 378         /*wait for commands*/
 379         poll_cmd(&rps);
 380
 381         return 0;
 382 }