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
  19 #include "rpin.h"       /*gpclk*/
  20 #include "rp_spi.h"     /*spi*/
  21 #include "misc.h"       /*structure for priorities*/
  22
  23
  24 #define PRUM_PROUD      2061
  25 #define PRUM_SOUC       6183
  26
  27 #define PRIOR_KERN      50
  28 #define PRIOR_HIGH      49
  29 #define PRIOR_LOW       20
  30
  31 #define THREAD_SHARED   0
  32 #define INIT_VALUE      0       /*init value for semaphor*/
  33
  34 struct sigaction sighnd; /*struktura pro signal handler*/
  35 struct rpi_in data;
  36
  37 uint8_t test;
  38 uint16_t pwm1, pwm2, pwm3;
  39 char commutate;
  40 int simple_hall_duty;
  41
  42
  43 /**
  44  * \brief Initilizes GPCLK.
  45  */
  46 int clk_init()
  47 {
  48         initialise(); /*namapovani gpio*/
  49         initClock(PLLD_500_MHZ, 10, 0);
  50         gpioSetMode(4, FSEL_ALT0);
  51         return 0;
  52 }
  53 /*
  54  * \brief Terminates GPCLK.
  55  */
  56
  57 inline void clk_disable(){
  58         termClock(0);
  59 }
  60
  61 /**
  62  * \brief Signal handler pro Ctrl+C
  63  */
  64 void sighnd_fnc(){
  65         spi_disable();
  66         clk_disable();
  67         printf("\nprogram bezpecne ukoncen\n");
  68         exit(0);
  69 }
  70
  71 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
  72         data->pozice_raw=data->pozice;
  73         data->pozice-=offset->pozice;
  74         return;
  75 }
  76 /*
  77  * pocita procentualni odchylku od prumerneho proudu
  78  */
  79 float diff_p(float value){
  80         return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
  81 }
  82 /*
  83  * pocita procentualni odchylku od prumerneho souctu proudu
  84  */
  85 float diff_s(float value){
  86         return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
  87 }
  88 /*
  89  * tiskne potrebna data
  90  */
  91 void printData(struct rpi_in data){
  92         float cur0, cur1, cur2;
  93         int i;
  94         if (data.adc_m_count){
  95                 cur0=data.ch0/data.adc_m_count;
  96                 cur1=data.ch1/data.adc_m_count;
  97                 cur2=data.ch2/data.adc_m_count;
  98         }
  99         for (i = 0; i < 16; i++) {
 100                         if (!(i % 6))
 101                                 puts("");
 102                         printf("%.2X ", data.debug_rx[i]);
 103         }
 104         puts("");
 105         printf("\npozice=%d\n",(int32_t)data.pozice);
 106         printf("raw_pozice=%d\n",(int32_t)data.pozice_raw);
 107         printf("raw_pozice last11=%u\n",(data.pozice_raw&0x7FF));
 108         printf("index position=%d\n",(int16_t)data.index_position);
 109         printf("hal1=%d, hal2=%d, hal3=%d\n",data.hal1,data.hal2,data.hal3);
 110         printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&test),!!(0x20&test),!!(0x10&test));
 111         printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&test),!!(0x04&test),!!(0x02&test));
 112         printf("PWM1=%u(L.s.)\n",pwm1);
 113         printf("PWM2=%u(L.s.)\n",pwm2);
 114         printf("PWM3=%u(L.s.)\n",pwm3);
 115         printf("Pocet namerenych proudu=%u\n",data.adc_m_count);
 116         printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data.ch1,cur1,diff_p(cur1));
 117         printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch2,cur2,diff_p(cur2));
 118         printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch0,cur0,diff_p(cur0));
 119         printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
 120         printf("duty=%d\n",simple_hall_duty);
 121         if (commutate) printf("commutation in progress\n");
 122 }
 123 void prepare_tx(uint8_t * tx){
 124
 125         /*Data format:
 126          * tx[4] - bity 95 downto 88 - bits that are sent first
 127          * tx[5] - bity 87 downto 80
 128          * tx[6] - bity 79 downto 72
 129          * tx[7] - bity 71 downto 64
 130          * tx[8] - bity 63 downto 56
 131          * tx[9] - bity 55 downto 48
 132          * tx[10] - bity 47 downto 40
 133          * tx[11] - bity 39 downto 32
 134          * tx[12] - bity 31 downto 24
 135          * tx[13] - bity 23 downto 16
 136          * tx[14] - bity 15 downto 8
 137          * tx[15] - bity 7 downto 0
 138          *
 139          * bit 95 - ADC reset
 140          * bit 94 - enable PWM1
 141          * bit 93 - enable PWM2
 142          * bit 92 - enable PWM3
 143          * bit 91 - shutdown1
 144          * bit 90 - shutdown2
 145          * bit 89 - shutdown3
 146          *      .
 147          *      .
 148          *      .
 149          * bits 66 .. 56 - match PWM1
 150          * bits 55 .. 45 - match PWM2
 151          * bit 11,12 - Unused
 152          * bits 42 .. 32  - match PWM3
 153          */
 154
 155
 156         uint16_t tmp;
 157
 158         /* keep the cap*/
 159         if (pwm1>2047) pwm1=2047;
 160         if (pwm2>2047) pwm2=2047;
 161         if (pwm3>2047) pwm3=2047;
 162
 163         tx[0]=test; /*bit 94 - enable PWM1*/
 164
 165         /*pwm1*/
 166         tx[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&pwm1)[1]); /*MSB*/
 167         tx[8]=((uint8_t*)&pwm1)[0]; /*LSB*/
 168
 169         /*pwm2*/
 170         tmp=pwm2;
 171         tmp<<=5;
 172         tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/
 173         tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/
 174
 175         /*pwm3*/
 176         tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&pwm3)[1]); /*MSB*/
 177         tx[11]=((uint8_t*)&pwm3)[0]; /*LSB*/
 178
 179
 180 }
 181 /**
 182  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
 183  */
 184 void * pos_monitor(void* param){
 185         set_priority(param);            /*set priority*/
 186         while(1){
 187                 printData(data);
 188                 usleep(1000000);        /*1 Hz*/
 189         }
 190         return (void*)0;
 191 }
 192 /*
 193  * \brief
 194  * Test function to be placed in controll loop.
 195  * Switches PWM's at point where they produce same force
 196  */
 197 inline void simple_hall_commutator(struct rpi_in data, int duty){
 198         if (duty>=0){ /* clockwise - so that position increase */
 199                 /* pwm3 */
 200                 if (data.hal2 && !data.hal3){
 201                         pwm1=0;
 202                         pwm2=0;
 203                         pwm3=duty;
 204                         /* pwm1 */
 205                 }else if (data.hal1 && !data.hal2){
 206                         pwm1=duty;
 207                         pwm2=0;
 208                         pwm3=0;
 209                         /* pwm2 */
 210                 }else if (!data.hal1 && data.hal3){
 211                         pwm1=0;
 212                         pwm2=duty;
 213                         pwm3=0;
 214                 }
 215         }else{  /*counter-clockwise - position decrease */
 216                 /* pwm3 */
 217                 if (!data.hal2 && data.hal3){
 218                         pwm1=0;
 219                         pwm2=0;
 220                         pwm3=-duty;
 221                         /* pwm1 */
 222                 }else if (!data.hal1 && data.hal2){
 223                         pwm1=-duty;
 224                         pwm2=0;
 225                         pwm3=0;
 226                         /* pwm2 */
 227                 }else if (data.hal1 && !data.hal3){
 228                         pwm1=0;
 229                         pwm2=-duty;
 230                         pwm3=0;
 231                 }
 232         }
 233 }
 234 /**
 235  * Funkce pravidelne vycita data z motoru
 236  */
 237 void * read_data(void* param){
 238         int i;
 239         struct rpi_in pocatek;
 240         uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ;
 241         set_priority(param);                            /*set priority*/
 242         pocatek = spi_read(tx);
 243                 while(1){
 244                         prepare_tx(tx);
 245                         data = spi_read(tx);
 246                         substractOffset(&data,&pocatek);
 247                         if (commutate){
 248                                 simple_hall_commutator(data,simple_hall_duty);
 249                         }
 250                         usleep(1000);                           /*1kHz*/
 251                 }
 252 }
 253
 254 /**
 255  * \brief Main function.
 256  */
 257
 258 int main(){
 259         unsigned int tmp;
 260
 261         /*nastaveni priorit vlaken*/
 262         struct thread_param tsp;
 263         tsp.sch_policy = SCHED_FIFO;
 264
 265         /*nastaveni signalu pro vypnuti pomoci Ctrl+C*/
 266         sighnd.sa_handler=&sighnd_fnc;
 267         sigaction(SIGINT, &sighnd, NULL );
 268
 269         clk_init();             /* inicializace gpio hodin */
 270         spi_init();             /* iniicializace spi*/
 271
 272         /*semafor pro detekci zpracovani parametru vlaken*/
 273         sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE);
 274
 275         /*vlakna*/
 276         pthread_t tid;                  /*identifikator vlakna*/
 277         pthread_attr_t attr;            /*atributy vlakna*/
 278         pthread_attr_init(&attr);       /*inicializuj implicitni atributy*/
 279
 280
 281
 282         /*ziskavani dat z motoru*//*vysoka priorita*/
 283         tsp.sch_prior = PRIOR_HIGH;
 284         pthread_create(&tid, &attr, read_data, (void*)&tsp);
 285
 286         /*vypisovani lokalni pozice*//*nizka priorita*/
 287         tsp.sch_prior = PRIOR_LOW;
 288         pthread_create(&tid, &attr, pos_monitor, (void*)&tsp);
 289
 290
 291         /*muzeme zavrit semafor*/
 292         sem_destroy(&thd_par_sem);
 293         /*
 294          * Note:
 295          * pri pouziti scanf("%u",&simple_hall_duty); dochazelo
 296          * k preukladani hodnot na promenne test. Dost divne.
 297          */
 298         while (1){
 299                 scanf("%u",&tmp);
 300                 printf("volba=%u\n",tmp);
 301                 switch (tmp){
 302                 case 1:
 303                         scanf("%u",&tmp);
 304                         pwm1=tmp&0xFFF;
 305                         break;
 306                 case 2:
 307                         scanf("%u",&tmp);
 308                         pwm2=tmp&0xFFF;
 309                         break;
 310                 case 3:
 311                         scanf("%u",&tmp);
 312                         pwm3=tmp&0xFFF;
 313                         break;
 314                 case 4:
 315                         scanf("%u",&tmp);
 316                         test=tmp&0xFF;
 317                         break;
 318                 case 5:
 319                         commutate=!commutate;
 320                         /* switch off pwms at the end of commutation */
 321                         pwm1&=commutate*0xFFFF;
 322                         pwm2&=commutate*0xFFFF;
 323                         pwm3&=commutate*0xFFFF;
 324                         break;
 325                 case 6:
 326                         scanf("%d",&tmp);
 327                         simple_hall_duty=tmp;
 328                         break;
 329
 330                 default:
 331                         break;
 332                 }
 333
 334         }
 335         return 0;
 336 }