X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/694d7155f582476ae9c042258e98fc867eb6b1ba..1cf729421e63f859e426a429f9ce935a86afd167:/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 56b926a..2eb9262 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -16,16 +16,17 @@ #include /*usleep*/ #include /*threads*/ #include /*nanosleep*/ +#include #include "rpin.h" /*gpclk*/ #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 MAX_DUTY 170 -#define PID_P 0.3 +#include "controllers.h" +#include "commutators.h" +#include "comp.h" +#include "logs.h" #define PRIOR_KERN 50 #define PRIOR_HIGH 49 @@ -40,6 +41,7 @@ struct rpi_in data; struct rpi_state rps={ + //.MAX_DUTY=170, .spi_dat=&data, .test=0, .pwm1=0,.pwm2=0, .pwm3=0, @@ -49,9 +51,19 @@ struct rpi_state rps={ .index_dist=0, /* distance to index position */ .index_ok=0, .tf_count=0, /*number of transfer*/ - .desired_pos=0 /* desired position */ + .desired_pos=0, /* desired position */ + .pos_reg_ena=0, + .desired_spd=0, + .spd_reg_ena=0, + .old_pos={0}, + .spd_err_sum=0, + .log_col_count=0, /* pocet radku zaznamu */ + .log_col=0, + .doLogs=0, + .alpha_offset=960 }; + /** * \brief Initilizes GPCLK. */ @@ -62,201 +74,28 @@ int clk_init() gpioSetMode(4, FSEL_ALT0); return 0; } -/* - * \brief Terminates GPCLK. - */ - -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 */ void appl_stop(){ + uint8_t tx[16]; + sem_wait(&rps.thd_par_sem); + + memset(tx,0,16*sizeof(int)); + rps.pwm1=0; + rps.pwm2=0; + rps.pwm3=0; + spi_read(&rps); + spi_disable(); - clk_disable(); + termClock(0); + freeLogs(&rps); /*muzeme zavrit semafor*/ sem_destroy(&rps.thd_par_sem); printf("\nprogram bezpecne ukoncen\n"); } -void substractOffset(struct rpi_in* data, struct rpi_in* offset){ - data->pozice_raw=data->pozice; - data->pozice-=offset->pozice; - return; -} -/* - * \brief - * Transformace pro uhel pocitany po smeru hodinovych rucicek - */ -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; -} -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; -} -/* - * \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 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){ - - /*Data format: - * tx[4] - bity 95 downto 88 - bits that are sent first - * tx[5] - bity 87 downto 80 - * tx[6] - bity 79 downto 72 - * tx[7] - bity 71 downto 64 - * tx[8] - bity 63 downto 56 - * tx[9] - bity 55 downto 48 - * tx[10] - bity 47 downto 40 - * tx[11] - bity 39 downto 32 - * tx[12] - bity 31 downto 24 - * tx[13] - bity 23 downto 16 - * tx[14] - bity 15 downto 8 - * tx[15] - bity 7 downto 0 - * - * bit 95 - ADC reset - * bit 94 - enable PWM1 - * bit 93 - enable PWM2 - * bit 92 - enable PWM3 - * bit 91 - shutdown1 - * bit 90 - shutdown2 - * bit 89 - shutdown3 - * . - * . - * Unused - * . - * . - * bits 47 .. 32 - match PWM1 - * bits 31 .. 16 - match PWM2 - * bits 15 .. 0 - match PWM3 - */ - - - uint16_t tmp; - - /* keep the 11-bit cap*/ - - if (rps.pwm1>2047) rps.pwm1=2047; - if (rps.pwm2>2047) rps.pwm2=2047; - if (rps.pwm3>2047) rps.pwm3=2047; - - tx[0]=rps.test; /*bit 94 - enable PWM1*/ - - /*now we have to switch the bytes due to endianess */ - /* ARMv6 & ARMv7 instructions are little endian */ - /*pwm1*/ - tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/ - tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/ - - /*pwm2*/ - tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/ - tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/ - - /*pwm3*/ - tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/ - tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/ - - -} /** * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru */ @@ -267,262 +106,25 @@ void * pos_monitor(void* param){ } 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 - #define DEGREE_120 1431655765 - #define DEGREE_180 2147483648 - #define DEGREE_240 2863311531 - #define DEGREE_300 3579139413 - uint32_t j,pos; - int32_t sin; - 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=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=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=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=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=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=sin*duty/1024; - if (pwm<0) pwm=0; - rps.pwm3=(uint16_t)pwm; - } - return 0; -} -/* - * \brief - * Test function to be placed in controll loop. - * Switches PWM's at point where they produce same force. - * This points are found thanks to IRC position, - */ -inline -void simple_ind_dist_commutator(int duty){ - if (duty>=0){ /* clockwise - so that position increase */ - /* pwm3 */ - if ((rps.index_dist>=45 && rps.index_dist<=373) || - (rps.index_dist>=1048 && rps.index_dist<=1377)){ - rps.pwm1=0; - rps.pwm2=0; - rps.pwm3=duty; - /* pwm1 */ - }else if ((rps.index_dist>=373 && rps.index_dist<=711) || - (rps.index_dist>=1377 && rps.index_dist<=1711)){ - rps.pwm1=duty; - rps.pwm2=0; - rps.pwm3=0; - /* pwm2 */ - }else if ((rps.index_dist>=0 && rps.index_dist<=45) || - (rps.index_dist>=711 && rps.index_dist<=1048) || - (rps.index_dist>=1711 && rps.index_dist<=1999)){ - rps.pwm1=0; - rps.pwm2=duty; - rps.pwm3=0; - } - }else{ /*counter-clockwise - position decrease */ - /* pwm3 */ - if ((rps.index_dist>=544 && rps.index_dist<=881) || - (rps.index_dist>=1544 && rps.index_dist<=1878)){ - rps.pwm1=0; - rps.pwm2=0; - rps.pwm3=-duty; - /* pwm1 */ - }else if ((rps.index_dist>=0 && rps.index_dist<=211) || - (rps.index_dist>=881 && rps.index_dist<=1210) || - (rps.index_dist>=1878 && rps.index_dist<=1999)){ - rps.pwm1=-duty; - rps.pwm2=0; - rps.pwm3=0; - /* pwm2 */ - }else if ((rps.index_dist>=211 && rps.index_dist<=544) || - (rps.index_dist>=1210 && rps.index_dist<=1544)){ - rps.pwm1=0; - rps.pwm2=-duty; - rps.pwm3=0; - } - } -} -/* - * \brief - * Test function to be placed in controll loop. - * Switches PWM's at point where they produce same force - */ -inline void simple_hall_commutator(int duty){ - if (duty>=0){ /* clockwise - so that position increase */ - /* pwm3 */ - if (data.hal2 && !data.hal3){ - rps.pwm1=0; - rps.pwm2=0; - rps.pwm3=duty; - /* pwm1 */ - }else if (data.hal1 && !data.hal2){ - rps.pwm1=duty; - rps.pwm2=0; - rps.pwm3=0; - /* pwm2 */ - }else if (!data.hal1 && data.hal3){ - rps.pwm1=0; - rps.pwm2=duty; - rps.pwm3=0; - } - }else{ /*counter-clockwise - position decrease */ - /* pwm3 */ - if (!data.hal2 && data.hal3){ - rps.pwm1=0; - rps.pwm2=0; - rps.pwm3=-duty; - /* pwm1 */ - }else if (!data.hal1 && data.hal2){ - rps.pwm1=-duty; - rps.pwm2=0; - rps.pwm3=0; - /* pwm2 */ - }else if (data.hal1 && !data.hal3){ - rps.pwm1=0; - rps.pwm2=-duty; - rps.pwm3=0; - } - } -} -/** - * \brief - * Computation of distance to index. - * - * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu - * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu - - * to je 3999 bodu - * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate - * signalu indexu - */ -void comIndDist(){ - uint16_t pos = 0x0FFF & data.pozice_raw; - uint16_t dist; - uint16_t index = data.index_position; - - if (index<1999){ /*index e<0,1998> */ - 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 if (pos */ - goto index_lost; - }else{ /*pozice e */ - /*proti smeru h.r. od indexu - podtecena pozice*/ - dist=pos-index-2096; - } - }else if (index<=2096){ /*index e<1999,2096>*/ - 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*/ - dist=4096+pos-index; - }else if (pos */ - goto index_lost; - }else if (pos */ - /*proti smeru h.r. od indexu*/ - dist=pos+2000-index; - }else{ /*pozice e */ - /*po smeru h.r. od indexu*/ - dist=pos-index; - } - } - - rps.index_dist = dist; - return; - index_lost: - rps.index_ok=0; - return; -} -/* - * \brief - * Very simple PID regulator. - * 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(){ - int duty_tmp; - duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice); - if (duty_tmp>MAX_DUTY){ - rps.duty=MAX_DUTY; - }else if (duty_tmp<-MAX_DUTY){ - rps.duty=-MAX_DUTY; - }else{ - rps.duty = duty_tmp; - } -} /* * \brief * Feedback loop. + * TODO: replace bunch of 'IFs' with Object-like pattern */ void * read_data(void* param){ int i; struct rpi_in pocatek; + struct rpi_state poc={ + .spi_dat=&pocatek, + .test=0, + .pwm1=0, .pwm1=0, .pwm3=0 + }; struct timespec t; int interval = 1000000; /* 1ms ~ 1kHz*/ - uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ; char first=1; uint16_t last_index; /*we have index up-to date*/ - pocatek = spi_read(tx); + spi_read(&poc); /*pocatecni informace*/ clock_gettime(CLOCK_MONOTONIC ,&t); /* start after one second */ t.tv_sec++; @@ -530,29 +132,49 @@ void * read_data(void* param){ /* wait until next shot */ clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); sem_wait(&rps.thd_par_sem); /*---take semaphore---*/ - prepare_tx(tx); /*save the data to send*/ - data = spi_read(tx); /*exchange data*/ + + /*old positions*/ + rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice; + spi_read(&rps); /*exchange data*/ /*subtract initiate postion */ rps.tf_count++; - substractOffset(&data,&pocatek); - comIndDist(); + substractOffset(&data,poc.spi_dat); + compSpeed(&rps); /*spocita rychlost*/ + 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(&rps); /*vypocet vzdalenosti indexu*/ } + }else{ /*index je v poradku*/ + comIndDist(&rps); /*vypocet vzdalenosti indexu*/ + } + + /* pocitame sirku plneni podle potreb rizeni*/ + if (rps.pos_reg_ena){ /*pozicni rizeni*/ + pos_pid(&rps); + }else if(rps.spd_reg_ena){ /*rizeni na rychlost*/ + spd_pid(&rps); } - pid(); + + /* sirka plneni prepoctena na jednotlive pwm */ if (rps.index_ok && rps.commutate){ /*simple_ind_dist_commutator(rps.duty);*/ /*sin_commutator(rps.duty);*/ - inv_trans_comm(rps.duty); - inv_trans_comm_2(rps.duty); + inv_trans_comm(&rps); + inv_trans_comm_2(&rps); }else if(!rps.index_ok && rps.commutate){ - simple_hall_commutator(rps.duty); + simple_hall_commutator(&rps); } + + /*zalogujeme hodnoty*/ + if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){ + makeLog(&rps); + } + sem_post(&rps.thd_par_sem); /*--post semaphore---*/ /* calculate next shot */ @@ -566,12 +188,9 @@ void * read_data(void* param){ } } - - /** * \brief Main function. */ - int main(){ pthread_t base_thread_id; clk_init(); /* inicializace gpio hodin */