X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/65c0ac775770408e2acec9a903d4f2071ab0a165..1a8d767b097d1536b264d8bae3e9d5c16617f835:/pmsm-control/test_sw/main_pmsm.c diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c index f666358..4ae245b 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -21,13 +21,11 @@ #include "rp_spi.h" /*spi*/ #include "misc.h" /*structure for priorities*/ #include "pxmc_sin_fixed.h" /*to test sin commutation */ +#include "pmsm_state.h" +#include "cmd_proc.h" - - -#define PRUM_PROUD 2061 -#define PRUM_SOUC 6183 -#define MAX_DUTY 128 -#define PID_P 0.1 +#define MAX_DUTY 170 +#define PID_P 0.3 #define PRIOR_KERN 50 #define PRIOR_HIGH 49 @@ -37,37 +35,25 @@ #define INIT_VALUE 1 /*init value for semaphor*/ -#define PXMC_SIN_FIX_TAB_BITS 9 -#define PXMC_SIN_FIX_IDX_SLR 23 -#define PXMC_SIN_FIX_XD_MASK 0x007fffff -#define PXMC_SIN_FIX_XD_SLR 8 -#define PXMC_SIN_FIX_A_MASK 0xffffc000 -#define PXMC_SIN_FIX_B_SLL 19 -#define PXMC_SIN_FIX_B_SAR 16 -#define PXMC_SIN_FIX_B_XD_SAR 6 -#define PXMC_SIN_FIX_ZIC_MASK 0x00002000 -#define PXMC_SIN_FIX_ZIC_BIT 13 - -#define PXMC_SIN_FIX_PI2 0x40000000 -#define PXMC_SIN_FIX_2PI3 0x55555555 - #define NSEC_PER_SEC (1000000000) /* The number of nsecs per sec. */ - struct rpi_in data; -struct rpi_state{ - uint8_t test; - uint16_t pwm1, pwm2, pwm3; - uint16_t t_pwm1, t_pwm2, t_pwm3; - char commutate; - int duty; /* duty cycle of pwm */ - uint16_t index_dist; /* distance to index position */ - unsigned char index_ok; - uint32_t tf_count; /*number of transfer*/ - int desired_pos; /* desired position */ -}rps; - +struct rpi_state rps={ + .spi_dat=&data, + .test=0, + .pwm1=0,.pwm2=0, .pwm3=0, + .pwm1=0, .t_pwm2=0, .t_pwm3=0, + .commutate=0, + .duty=0, /* duty cycle of pwm */ + .index_dist=0, /* distance to index position */ + .index_ok=0, + .tf_count=0, /*number of transfer*/ + .desired_pos=0, /* desired position */ + .pos_reg_ena=0, + .desired_spd=0, + .spd_reg_ena=0 +}; /** * \brief Initilizes GPCLK. @@ -86,6 +72,24 @@ int clk_init() inline void clk_disable(){ termClock(0); } +/* + * \brief + * Count minimum value of three numbers. + * Input values must be in range <-2^28;2^28>. + */ +int32_t min(int32_t x, int32_t y, int32_t z){ + int32_t diff,sign; + + diff=x-y; /*rozdil*/ + sign=(*((uint32_t*)&diff))>>31; /*znamenko -> detekuje, ze y je vetsi*/ + x=y+sign*diff; /*ulozime mensi cislo, pokud sign>0, pak diff<0 */ + + diff=x-z; /*rozdil*/ + sign=(*((uint32_t*)&diff))>>31; /*znamenko -> detekuje, ze z je vetsi*/ + x=z+sign*diff; /*ulozime mensi cislo, pokud sign>0, pak diff<0 */ + + return x; +} /** * \brief Signal handler pro Ctrl+C @@ -94,7 +98,7 @@ void appl_stop(){ spi_disable(); clk_disable(); /*muzeme zavrit semafor*/ - sem_destroy(&thd_par_sem); + sem_destroy(&rps.thd_par_sem); printf("\nprogram bezpecne ukoncen\n"); } @@ -104,64 +108,97 @@ void substractOffset(struct rpi_in* data, struct rpi_in* offset){ return; } /* - * pocita procentualni odchylku od prumerneho proudu + * \brief + * Transformace pro uhel pocitany po smeru hodinovych rucicek */ -float diff_p(float value){ - return ((float)value-PRUM_PROUD)*100/PRUM_PROUD; +void dq2alphabeta(int32_t *alpha, int32_t *beta, int d, int q, int32_t sin, int32_t cos){ + *alpha=cos*d+sin*q; + *beta=-sin*d+cos*q; + return; } -/* - * pocita procentualni odchylku od prumerneho souctu proudu - */ -float diff_s(float value){ - return ((float)value-PRUM_SOUC)*100/PRUM_SOUC; +void alphabeta2pwm3(int32_t * ia, int32_t * ib, int32_t *ic,int32_t alpha, int32_t beta){ + *ia=alpha; + *ib=-alpha/2+beta*887/1024; + *ic=-alpha/2-beta*887/1024; } /* - * tiskne potrebna data + * \brief + * Preocita napeti na jednotlivych civkach na napeti, + * ktera budou privedena na svorky motoru. + * Tedy na A(yel)-pwm1, B(red)-pwm2, C(blk)-pwm3 */ -void printData(){ - struct rpi_in data_p; - struct rpi_state s; /*state*/ - float cur0, cur1, cur2; - int i; - /* copy the data */ - sem_wait(&thd_par_sem); - data_p = data; - s=rps; - sem_post(&thd_par_sem); - - if (data_p.adc_m_count){ - cur0=data_p.ch0/data_p.adc_m_count; - cur1=data_p.ch1/data_p.adc_m_count; - cur2=data_p.ch2/data_p.adc_m_count; - } - for (i = 0; i < 16; i++) { - if (!(i % 6)) - puts(""); - printf("%.2X ", data_p.debug_rx[i]); - } - puts(""); - printf("\npozice=%d\n",(int32_t)data_p.pozice); - printf("chtena pozice=%d\n",s.desired_pos); - printf("transfer count=%u\n",s.tf_count); - printf("raw_pozice=%u\n",data_p.pozice_raw); - printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF)); - printf("index position=%u\n",data_p.index_position); - printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3); - printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test)); - printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test)); - printf("PWM1=%u(L.s.)\n",s.pwm1); - printf("PWM2=%u(L.s.)\n",s.pwm2); - printf("PWM3=%u(L.s.)\n",s.pwm3); - printf("distance to index=%u\n",s.index_dist); - printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3); - printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count); - printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1)); - printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2)); - printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0)); - printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2)); - printf("duty=%d\n",s.duty); - if (s.index_ok) printf("index ok\n"); - if (s.commutate) printf("commutation in progress\n"); +void transDelta(int32_t * u1, int32_t * u2, int32_t *u3, int32_t ub , int32_t uc){ + int32_t t; + + /*vypocte napeti tak, aby odpovidaly rozdily*/ + *u1=uc; + *u2=uc+ub; + *u3=0; + + /*najde zaporne napeti*/ + t=min(*u1,*u2,*u3); + + /*dorovna zaporna napeti na nulu*/ + *u1-=t; + *u2-=t; + *u3-=t; +} +void inv_trans_comm(int duty){ + uint32_t pos; + int32_t sin, cos; + int32_t alpha, beta; + int32_t pwma,pwmb,pwmc; + pos=rps.index_dist; + /*melo by byt urceno co nejpresneji, aby faze 'a' splyvala s osou 'alpha'*/ + pos+=717; + /*use it as cyclic 32-bit logic*/ + pos*=4294967; + pxmc_sincos_fixed_inline(&sin, &cos, pos, 16); + dq2alphabeta(&alpha, &beta,0,duty, sin, cos); + alpha>>=16; + beta>>=16; + alphabeta2pwm3(&pwma,&pwmb, &pwmc,alpha,beta); + + if (pwma<0) pwma=0; + if (pwmb<0) pwmb=0; + if (pwmc<0) pwmc=0; + + + rps.t_pwm1=(uint16_t)pwma; + rps.t_pwm3=(uint16_t)pwmb; + rps.t_pwm2=(uint16_t)pwmc; +} + +void inv_trans_comm_2(int duty){ + uint32_t pos; + int32_t sin, cos; + int32_t alpha, beta; + int32_t ua,ub,uc; + int32_t ia,ib,ic; + int32_t u1,u2,u3; + pos=rps.index_dist; + + pos+=960; /*zarovnani faze 'a' s osou 'alpha'*/ + + /*pro výpočet sin a cos je pouzita 32-bit cyklicka logika*/ + pos*=4294967; + pxmc_sincos_fixed_inline(&sin, &cos, pos, 16); + + dq2alphabeta(&alpha, &beta,0,duty, sin, cos); + alpha>>=16; + beta>>=16; + + alphabeta2pwm3(&ia,&ib, &ic,alpha,beta); + + ua=ia; + ub=ib; + uc=ic; + + transDelta(&u1,&u2, &u3,ub,uc); + + rps.pwm1=(uint16_t)u1; + rps.pwm2=(uint16_t)u2; + rps.pwm3=(uint16_t)u3; } void prepare_tx(uint8_t * tx){ @@ -228,12 +265,25 @@ void prepare_tx(uint8_t * tx){ */ void * pos_monitor(void* param){ while(1){ - printData(); + printData(&rps); usleep(1000000); /*1 Hz*/ } return (void*)0; } - +/* + * \brief + * Multiplication of 11 bit + * Zaporne vysledky prvede na nulu. + */ +inline uint16_t mult_cap(int32_t s,int d){ + int j; + int res=0; + for(j=0;j!=11;j++){ + /* multiplicate as if maximum sinus value was unity */ + res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j)); + } + return res; +} inline int sin_commutator(int duty){ #define DEGREE_60 715827883 @@ -243,64 +293,50 @@ int sin_commutator(int duty){ #define DEGREE_300 3579139413 uint32_t j,pos; int32_t sin; - uint16_t pwm; - pos=rps.index_dist*4294967; + pos=rps.index_dist; + int32_t pwm; + /*aby prictene uhly mohla byt kulata cisla, musime index posunout*/ + pos+=38; + /*use it as cyclic 32-bit logic*/ + pos*=4294967; if (duty>=0){ /*clockwise rotation*/ /* 1st phase */ sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm1=pwm; + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm1=(uint16_t)pwm; /* 2nd phase */ sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm2=pwm; + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm2=(uint16_t)pwm; /* 3rd phase */ sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm3=pwm; + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm3=(uint16_t)pwm; }else{ duty=-duty; /* 1st phase */ sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm1=pwm; + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm1=(uint16_t)pwm; /* 2nd phase */ sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm2=pwm; + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm2=(uint16_t)pwm; /* 3rd phase */ sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm3=pwm; + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm3=(uint16_t)pwm; } return 0; } @@ -409,31 +445,56 @@ inline void simple_hall_commutator(int duty){ * signalu indexu */ void comIndDist(){ - uint16_t pos_12 = 0x0FFF & data.pozice_raw; - if (data.index_position<2048){ - if (pos_12 */ + if (pos */ /*proti smeru h.r. od indexu*/ - rps.index_dist=pos_12+2000-data.index_position; - }else if (pos_12<=data.index_position+2048){ + dist=pos+2000-index; + }else if (pos<=index+1999){ /*pozice e */ /*po smeru h.r. od indexu*/ - rps.index_dist=pos_12-data.index_position; - }else{ + dist=pos-index; + }else if (pos */ + goto index_lost; + }else{ /*pozice e */ /*proti smeru h.r. od indexu - podtecena pozice*/ - rps.index_dist=pos_12-data.index_position-2096; + dist=pos-index-2096; } - }else{ - if (pos_12*/ + if (pos */ + goto index_lost; + }else if (pos */ + /*proti smeru h.r. od indexu*/ + dist=pos+2000-index; + }else if (pos<=index+1999){ /*pozice e */ + /*po smeru h.r. od indexu*/ + dist=pos-index; + }else { /*pozice e */ + goto index_lost; + } + }else{ /*index e<2097,4095> */ + if (pos<=index-2097){ /*pozice e<0,index-2097> */ /*po smeru h.r. od indexu - pretecena pozice*/ - rps.index_dist=4096+pos_12-data.index_position; - }else if (pos_12 */ + goto index_lost; + }else if (pos */ /*proti smeru h.r. od indexu*/ - rps.index_dist=pos_12+2000-data.index_position; - }else{ + dist=pos+2000-index; + }else{ /*pozice e */ /*po smeru h.r. od indexu*/ - rps.index_dist=pos_12-data.index_position; + dist=pos-index; } - } + + rps.index_dist = dist; + return; + + index_lost: + rps.index_ok=0; + return; } /* * \brief @@ -441,7 +502,7 @@ void comIndDist(){ * Now only with P-part so that the error doesnt go to zero. * TODO: add anti-wind up and I and D parts */ -inline void pid(){ +inline void pos_pid(){ int duty_tmp; duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice); if (duty_tmp>MAX_DUTY){ @@ -455,6 +516,7 @@ inline void pid(){ /* * \brief * Feedback loop. + * TODO: replace bunch of 'IFs' with Object-like pattern */ void * read_data(void* param){ int i; @@ -471,29 +533,38 @@ void * read_data(void* param){ while(1){ /* wait until next shot */ clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); - sem_wait(&thd_par_sem); /*---take semaphore---*/ + sem_wait(&rps.thd_par_sem); /*---take semaphore---*/ prepare_tx(tx); /*save the data to send*/ data = spi_read(tx); /*exchange data*/ /*subtract initiate postion */ rps.tf_count++; substractOffset(&data,&pocatek); - comIndDist(); + if (!rps.index_ok){ if (first){ last_index=data.index_position; first=0; }else if (last_index!=data.index_position){ rps.index_ok=1; + comIndDist(); /*vypocet vzdalenosti indexu*/ } + }else{ /*index je v poradku*/ + comIndDist(); /*vypocet vzdalenosti indexu*/ + } + /* pocitame sirku plneni podle potreb rizeni*/ + if (rps.pos_reg_ena){ + pos_pid(); } - pid(); + /* sirka plneni prepoctena na jednotlive pwm */ if (rps.index_ok && rps.commutate){ /*simple_ind_dist_commutator(rps.duty);*/ - sin_commutator(rps.duty); + /*sin_commutator(rps.duty);*/ + inv_trans_comm(rps.duty); + inv_trans_comm_2(rps.duty); }else if(!rps.index_ok && rps.commutate){ simple_hall_commutator(rps.duty); } - sem_post(&thd_par_sem); /*--post semaphore---*/ + sem_post(&rps.thd_par_sem); /*--post semaphore---*/ /* calculate next shot */ t.tv_nsec += interval; @@ -507,74 +578,7 @@ void * read_data(void* param){ } -/** - * \brief - * Commands detection. - */ -void poll_cmd(){ - unsigned int tmp; - /* - * Note: - * pri pouziti scanf("%u",&simple_hall_duty); dochazelo - * k preukladani hodnot na promenne test. Dost divne. - */ - while (1){ - scanf("%u",&tmp); - printf("volba=%u\n",tmp); - switch (tmp){ - case 1: - scanf("%u",&tmp); - sem_wait(&thd_par_sem); - rps.pwm1=tmp&0xFFF; - sem_post(&thd_par_sem); - break; - case 2: - scanf("%u",&tmp); - sem_wait(&thd_par_sem); - rps.pwm2=tmp&0xFFF; - sem_post(&thd_par_sem); - break; - case 3: - scanf("%u",&tmp); - sem_wait(&thd_par_sem); - rps.pwm3=tmp&0xFFF; - sem_post(&thd_par_sem); - break; - case 4: - scanf("%u",&tmp); - sem_wait(&thd_par_sem); - rps.test=tmp&0xFF; - sem_post(&thd_par_sem); - break; - case 5: - sem_wait(&thd_par_sem); - rps.commutate=!rps.commutate; - /* switch off pwms at the end of commutation */ - rps.pwm1&=rps.commutate*0xFFFF; - rps.pwm2&=rps.commutate*0xFFFF; - rps.pwm3&=rps.commutate*0xFFFF; - sem_post(&thd_par_sem); - break; - case 6: - scanf("%d",&tmp); - sem_wait(&thd_par_sem); - rps.duty=tmp; - sem_post(&thd_par_sem); - break; - case 7: - scanf("%d",&tmp); - sem_wait(&thd_par_sem); - rps.desired_pos=tmp; - sem_post(&thd_par_sem); - break; - - default: - break; - } - } - return ; -} /** * \brief Main function. */ @@ -585,7 +589,7 @@ int main(){ spi_init(); /* iniicializace spi*/ /*semafor pro detekci zpracovani parametru vlaken*/ - sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE); + sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE); setup_environment(); base_thread_id=pthread_self(); @@ -597,7 +601,7 @@ int main(){ create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL); /*wait for commands*/ - poll_cmd(); + poll_cmd(&rps); return 0; }