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 #include "comp.h"
  29
  30
  31
  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         //.MAX_DUTY=170,
  47         .spi_dat=&data,
  48         .test=0,
  49         .pwm1=0,.pwm2=0, .pwm3=0,
  50         .pwm1=0, .t_pwm2=0, .t_pwm3=0,
  51         .commutate=0,
  52         .duty=0,                        /* duty cycle of pwm */
  53         .index_dist=0,          /* distance to index position */
  54         .index_ok=0,
  55         .tf_count=0,            /*number of transfer*/
  56         .desired_pos=0,         /* desired position */
  57         .pos_reg_ena=0,
  58         .desired_spd=0,
  59         .spd_reg_ena=0,
  60         .old_pos={0},
  61         .spd_err_sum=0,
  62         .log_col_count=0,       /* pocet radku zaznamu */
  63         .log_col=0,
  64         .doLogs=0,
  65         .alpha_offset=960
  66 };
  67
  68 /**
  69  * \brief Initilizes GPCLK.
  70  */
  71 int clk_init()
  72 {
  73         initialise(); /*namapovani gpio*/
  74         initClock(PLLD_500_MHZ, 10, 0);
  75         gpioSetMode(4, FSEL_ALT0);
  76         return 0;
  77 }
  78 /*
  79  * \brief Terminates GPCLK.
  80  */
  81
  82 inline void clk_disable(){
  83         termClock(0);
  84 }
  85
  86
  87 /*
  88  * \brief
  89  * Free logs
  90  */
  91 void freeLogs(){
  92         int r;
  93         if (rps.log_col_count){
  94                 for (r=0;r<LOG_ROWS;r++){
  95                         free(rps.logs[r]);
  96                 }
  97         }
  98         rps.log_col_count=0;
  99         rps.doLogs=0;
 100 }
 101
 102 /*
 103  * \brief
 104  * Makes log.
 105  */
 106 void makeLog(){
 107         int r;
 108         if (rps.log_col==MAX_LOGS-1){
 109                 rps.doLogs=0;
 110                 return;
 111         }
 112         rps.logs[0][rps.log_col]=(int)rps.tf_count;
 113         rps.logs[1][rps.log_col]=(int)rps.spi_dat->pozice;
 114
 115         rps.logs[2][rps.log_col]=(int)rps.pwm1;
 116         rps.logs[3][rps.log_col]=(int)rps.pwm2;
 117         rps.logs[4][rps.log_col]=(int)rps.pwm3;
 118         rps.logs[5][rps.log_col]=rps.duty;
 119
 120         rps.logs[6][rps.log_col]=rps.desired_spd;
 121         rps.logs[7][rps.log_col]=rps.speed;
 122
 123         rps.logs[8][rps.log_col]=(int)(rps.spi_dat->ch1/rps.spi_dat->adc_m_count);
 124         rps.logs[9][rps.log_col]=(int)(rps.spi_dat->ch2/rps.spi_dat->adc_m_count);
 125         rps.logs[10][rps.log_col]=(int)(rps.spi_dat->ch0/rps.spi_dat->adc_m_count);
 126
 127         rps.log_col++;
 128      /*
 129         if (rps.log_col==rps.log_col_count-1){
 130                 rps.log_col_count*=2;
 131                 rps.log_col_count%=MAX_LOGS;
 132                 for (r=0;r<LOG_ROWS;r++){
 133                         rps.logs[r]=realloc(rps.logs[r],rps.log_col_count*sizeof(int));
 134                         if (rps.logs[r]==NULL){
 135                                 rps.doLogs=0;
 136                                 rps.error=1;
 137                         }
 138                 }
 139         }
 140      */
 141 }
 142
 143
 144
 145
 146 /**
 147  * \brief Signal handler pro Ctrl+C
 148  */
 149 void appl_stop(){
 150         uint8_t tx[16];
 151         sem_wait(&rps.thd_par_sem);
 152
 153         memset(tx,0,16*sizeof(int));
 154         rps.pwm1=0;
 155         rps.pwm2=0;
 156         rps.pwm3=0;
 157         spi_read(&rps);
 158
 159         spi_disable();
 160         clk_disable();
 161         freeLogs();
 162         /*muzeme zavrit semafor*/
 163         sem_destroy(&rps.thd_par_sem);
 164         printf("\nprogram bezpecne ukoncen\n");
 165 }
 166
 167 /**
 168  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
 169  */
 170 void * pos_monitor(void* param){
 171         while(1){
 172                 printData(&rps);
 173                 usleep(1000000);        /*1 Hz*/
 174         }
 175         return (void*)0;
 176 }
 177
 178 /*
 179  * \brief
 180  * Feedback loop.
 181  * TODO: replace bunch of 'IFs' with Object-like pattern
 182  */
 183 void * read_data(void* param){
 184         int i;
 185         struct rpi_in pocatek;
 186         struct rpi_state poc={
 187                 .spi_dat=&pocatek,
 188                 .test=0,
 189                 .pwm1=0, .pwm1=0, .pwm3=0
 190         };
 191         struct timespec t;
 192         int interval = 1000000; /* 1ms ~ 1kHz*/
 193         char first=1;
 194         uint16_t last_index;                            /*we have index up-to date*/
 195         spi_read(&poc);         /*pocatecni informace*/
 196         clock_gettime(CLOCK_MONOTONIC ,&t);
 197         /* start after one second */
 198         t.tv_sec++;
 199                 while(1){
 200                         /* wait until next shot */
 201                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
 202                         sem_wait(&rps.thd_par_sem);             /*---take semaphore---*/
 203
 204                         /*old positions*/
 205                         rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
 206                         spi_read(&rps);         /*exchange data*/
 207                         /*subtract initiate postion */
 208                         rps.tf_count++;
 209                         substractOffset(&data,poc.spi_dat);
 210                         compSpeed(&rps);                        /*spocita rychlost*/
 211
 212                         if (!rps.index_ok){
 213                                 if (first){
 214                                         last_index=data.index_position;
 215                                         first=0;
 216                                 }else if (last_index!=data.index_position){
 217                                         rps.index_ok=1;
 218                                         comIndDist(&rps);       /*vypocet vzdalenosti indexu*/
 219                                 }
 220                         }else{ /*index je v poradku*/
 221                                 comIndDist(&rps);               /*vypocet vzdalenosti indexu*/
 222                         }
 223
 224                         /* pocitame sirku plneni podle potreb rizeni*/
 225                         if (rps.pos_reg_ena){           /*pozicni rizeni*/
 226                                 pos_pid(&rps);
 227                         }else if(rps.spd_reg_ena){      /*rizeni na rychlost*/
 228                                 spd_pid(&rps);
 229                         }
 230
 231                         /* sirka plneni prepoctena na jednotlive pwm */
 232                         if (rps.index_ok && rps.commutate){
 233                                 /*simple_ind_dist_commutator(rps.duty);*/
 234                                 /*sin_commutator(rps.duty);*/
 235                                 inv_trans_comm(&rps);
 236                                 inv_trans_comm_2(&rps);
 237                         }else if(!rps.index_ok && rps.commutate){
 238                                 simple_hall_commutator(&rps);
 239                         }
 240
 241                         /*zalogujeme hodnoty*/
 242                         if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
 243                                 makeLog();
 244                         }
 245
 246                         sem_post(&rps.thd_par_sem);             /*--post semaphore---*/
 247
 248                         /* calculate next shot */
 249                         t.tv_nsec += interval;
 250
 251                         while (t.tv_nsec >= NSEC_PER_SEC) {
 252                                 t.tv_nsec -= NSEC_PER_SEC;
 253                                 t.tv_sec++;
 254                         }
 255
 256                 }
 257 }
 258
 259
 260 /**
 261  * \brief Main function.
 262  */
 263
 264 int main(){
 265         pthread_t base_thread_id;
 266         clk_init();             /* inicializace gpio hodin */
 267         spi_init();             /* iniicializace spi*/
 268
 269         /*semafor pro detekci zpracovani parametru vlaken*/
 270         sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
 271         setup_environment();
 272
 273         base_thread_id=pthread_self();
 274
 275         /*main control loop*/
 276         create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
 277
 278         /*monitor of current state*/
 279         create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
 280
 281         /*wait for commands*/
 282         poll_cmd(&rps);
 283
 284         return 0;
 285 }