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 #include "logs.h"
  30
  31 #define PRIOR_KERN      50
  32 #define PRIOR_HIGH      49
  33 #define PRIOR_LOW       20
  34
  35 #define THREAD_SHARED   0
  36 #define INIT_VALUE      1       /*init value for semaphor*/
  37
  38
  39 #define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */
  40
  41
  42 struct rpi_in data;
  43 struct rpi_state rps={
  44         //.MAX_DUTY=170,
  45         .spi_dat=&data,
  46         .test=0,
  47         .pwm1=0,.pwm2=0, .pwm3=0,
  48         .pwm1=0, .t_pwm2=0, .t_pwm3=0,
  49         .commutate=0,
  50         .duty=0,                        /* duty cycle of pwm */
  51         .index_dist=0,          /* distance to index position */
  52         .index_ok=0,
  53         .tf_count=0,            /*number of transfer*/
  54         .desired_pos=0,         /* desired position */
  55         .pos_reg_ena=0,
  56         .desired_spd=0,
  57         .spd_reg_ena=0,
  58         .old_pos={0},
  59         .spd_err_sum=0,
  60         .log_col_count=0,       /* pocet radku zaznamu */
  61         .log_col=0,
  62         .doLogs=0,
  63         .alpha_offset=960
  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 /**
  85  * \brief Signal handler pro Ctrl+C
  86  */
  87 void appl_stop(){
  88         uint8_t tx[16];
  89         sem_wait(&rps.thd_par_sem);
  90
  91         memset(tx,0,16*sizeof(int));
  92         rps.pwm1=0;
  93         rps.pwm2=0;
  94         rps.pwm3=0;
  95         spi_read(&rps);
  96
  97         spi_disable();
  98         clk_disable();
  99         freeLogs(&rps);
 100         /*muzeme zavrit semafor*/
 101         sem_destroy(&rps.thd_par_sem);
 102         printf("\nprogram bezpecne ukoncen\n");
 103 }
 104
 105 /**
 106  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
 107  */
 108 void * pos_monitor(void* param){
 109         while(1){
 110                 printData(&rps);
 111                 usleep(1000000);        /*1 Hz*/
 112         }
 113         return (void*)0;
 114 }
 115
 116 /*
 117  * \brief
 118  * Feedback loop.
 119  * TODO: replace bunch of 'IFs' with Object-like pattern
 120  */
 121 void * read_data(void* param){
 122         int i;
 123         struct rpi_in pocatek;
 124         struct rpi_state poc={
 125                 .spi_dat=&pocatek,
 126                 .test=0,
 127                 .pwm1=0, .pwm1=0, .pwm3=0
 128         };
 129         struct timespec t;
 130         int interval = 1000000; /* 1ms ~ 1kHz*/
 131         char first=1;
 132         uint16_t last_index;                            /*we have index up-to date*/
 133         spi_read(&poc);         /*pocatecni informace*/
 134         clock_gettime(CLOCK_MONOTONIC ,&t);
 135         /* start after one second */
 136         t.tv_sec++;
 137                 while(1){
 138                         /* wait until next shot */
 139                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
 140                         sem_wait(&rps.thd_par_sem);             /*---take semaphore---*/
 141
 142                         /*old positions*/
 143                         rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
 144                         spi_read(&rps);         /*exchange data*/
 145                         /*subtract initiate postion */
 146                         rps.tf_count++;
 147                         substractOffset(&data,poc.spi_dat);
 148                         compSpeed(&rps);                        /*spocita rychlost*/
 149
 150                         if (!rps.index_ok){
 151                                 if (first){
 152                                         last_index=data.index_position;
 153                                         first=0;
 154                                 }else if (last_index!=data.index_position){
 155                                         rps.index_ok=1;
 156                                         comIndDist(&rps);       /*vypocet vzdalenosti indexu*/
 157                                 }
 158                         }else{ /*index je v poradku*/
 159                                 comIndDist(&rps);               /*vypocet vzdalenosti indexu*/
 160                         }
 161
 162                         /* pocitame sirku plneni podle potreb rizeni*/
 163                         if (rps.pos_reg_ena){           /*pozicni rizeni*/
 164                                 pos_pid(&rps);
 165                         }else if(rps.spd_reg_ena){      /*rizeni na rychlost*/
 166                                 spd_pid(&rps);
 167                         }
 168
 169                         /* sirka plneni prepoctena na jednotlive pwm */
 170                         if (rps.index_ok && rps.commutate){
 171                                 /*simple_ind_dist_commutator(rps.duty);*/
 172                                 /*sin_commutator(rps.duty);*/
 173                                 inv_trans_comm(&rps);
 174                                 inv_trans_comm_2(&rps);
 175                         }else if(!rps.index_ok && rps.commutate){
 176                                 simple_hall_commutator(&rps);
 177                         }
 178
 179                         /*zalogujeme hodnoty*/
 180                         if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
 181                                 makeLog(&rps);
 182                         }
 183
 184                         sem_post(&rps.thd_par_sem);             /*--post semaphore---*/
 185
 186                         /* calculate next shot */
 187                         t.tv_nsec += interval;
 188
 189                         while (t.tv_nsec >= NSEC_PER_SEC) {
 190                                 t.tv_nsec -= NSEC_PER_SEC;
 191                                 t.tv_sec++;
 192                         }
 193
 194                 }
 195 }
 196
 197
 198 /**
 199  * \brief Main function.
 200  */
 201
 202 int main(){
 203         pthread_t base_thread_id;
 204         clk_init();             /* inicializace gpio hodin */
 205         spi_init();             /* iniicializace spi*/
 206
 207         /*semafor pro detekci zpracovani parametru vlaken*/
 208         sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
 209         setup_environment();
 210
 211         base_thread_id=pthread_self();
 212
 213         /*main control loop*/
 214         create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
 215
 216         /*monitor of current state*/
 217         create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
 218
 219         /*wait for commands*/
 220         poll_cmd(&rps);
 221
 222         return 0;
 223 }