X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/0db7b4491e1b1a1621cd35255aa4ad91652e1e85..b98f70ee3eed3af0cbbc9d5869ac1e7626534ef4:/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 4c51f51..e4ec9ac 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -16,6 +16,7 @@ #include /*usleep*/ #include /*threads*/ #include /*nanosleep*/ +#include #include "rpin.h" /*gpclk*/ #include "rp_spi.h" /*spi*/ @@ -24,8 +25,16 @@ #include "pmsm_state.h" #include "cmd_proc.h" -#define MAX_DUTY 128 -#define PID_P 0.1 + +#define PID_P 0.3 + +/* RL-tool results from first order system approx */ +/* P=0.16 I=13,4/freq=0.013 */ +/* Hodnoty upraveny podle skutecnych vysledku */ +/* P=0.8 I=0.01 */ + +#define PID_P_S 0.16 /*2.3 kmita*/ /*1.4 vhodne jen pro P regulator*/ +#define PID_I_S 0.0134 #define PRIOR_KERN 50 #define PRIOR_HIGH 49 @@ -49,7 +58,16 @@ 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 }; /** @@ -69,63 +87,85 @@ int clk_init() inline void clk_disable(){ termClock(0); } - -/** - * \brief Signal handler pro Ctrl+C +/* + * \brief + * Count minimum value of three numbers. + * Input values must be in range <-2^28;2^28>. */ -void appl_stop(){ - spi_disable(); - clk_disable(); - /*muzeme zavrit semafor*/ - sem_destroy(&rps.thd_par_sem); - printf("\nprogram bezpecne ukoncen\n"); -} +int32_t min(int32_t x, int32_t y, int32_t z){ + int32_t diff,sign; -void substractOffset(struct rpi_in* data, struct rpi_in* offset){ - data->pozice_raw=data->pozice; - data->pozice-=offset->pozice; - return; + 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 - * Transformace pro uhel pocitany po smeru hodinovych rucicek + * Free logs */ -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 * pwma, int32_t * pwmb, int32_t *pwmc,int32_t alpha, int32_t beta){ - *pwma=alpha; - *pwmb=-alpha/2+beta*887/1024; - *pwmc=-alpha/2-beta*887/1024; +void freeLogs(){ + int r; + if (rps.log_col_count){ + for (r=0;r>=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.pwm1=(uint16_t)pwma; - rps.pwm3=(uint16_t)pwmb; - rps.pwm2=(uint16_t)pwmc; +/* + * \brief + * Makes log. + */ +void makeLog(){ + int r; + if (rps.log_col==MAX_LOGS-1){ + rps.doLogs=0; + return; + } + rps.logs[0][rps.log_col]=(int)rps.tf_count; + rps.logs[1][rps.log_col]=(int)rps.spi_dat->pozice; + + rps.logs[2][rps.log_col]=(int)rps.pwm1; + rps.logs[3][rps.log_col]=(int)rps.pwm2; + rps.logs[4][rps.log_col]=(int)rps.pwm3; + rps.logs[5][rps.log_col]=rps.duty; + + rps.logs[6][rps.log_col]=rps.desired_spd; + rps.logs[7][rps.log_col]=rps.speed; + + rps.logs[8][rps.log_col]=(int)(rps.spi_dat->ch1/rps.spi_dat->adc_m_count); + rps.logs[9][rps.log_col]=(int)(rps.spi_dat->ch2/rps.spi_dat->adc_m_count); + rps.logs[10][rps.log_col]=(int)(rps.spi_dat->ch0/rps.spi_dat->adc_m_count); + + rps.log_col++; + /* + if (rps.log_col==rps.log_col_count-1){ + rps.log_col_count*=2; + rps.log_col_count%=MAX_LOGS; + for (r=0;rpozice=data->pozice_raw-offset->pozice_raw; + 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+=rps.alpha_offset; /*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; +} + /** * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru */ @@ -428,7 +590,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){ @@ -439,9 +601,41 @@ inline void pid(){ rps.duty = duty_tmp; } } +/* + * \brief + * Very simple PID regulator. + * Now only with P-part so that the error doesnt go to zero. + * FIXME: make better + */ +inline void spd_pid(){ + int duty_tmp; + int error; + error=rps.desired_spd - rps.speed; + rps.spd_err_sum+=error; + duty_tmp = PID_P_S*error+PID_I_S*rps.spd_err_sum; + 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 + * Computate speed. + */ +void compSpeed(){ + signed long int spd; + spd=rps.spi_dat->pozice-rps.old_pos[rps.tf_count%OLD_POS_NUM]; + rps.speed=(int32_t)spd; +} + /* * \brief * Feedback loop. + * TODO: replace bunch of 'IFs' with Object-like pattern */ void * read_data(void* param){ int i; @@ -459,28 +653,50 @@ 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---*/ + + /*old positions*/ + rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice; prepare_tx(tx); /*save the data to send*/ data = spi_read(tx); /*exchange data*/ /*subtract initiate postion */ rps.tf_count++; substractOffset(&data,&pocatek); - comIndDist(); + compSpeed(); /*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(); /*vypocet vzdalenosti indexu*/ } + }else{ /*index je v poradku*/ + comIndDist(); /*vypocet vzdalenosti indexu*/ + } + + /* pocitame sirku plneni podle potreb rizeni*/ + if (rps.pos_reg_ena){ /*pozicni rizeni*/ + pos_pid(); + }else if(rps.spd_reg_ena){ /*rizeni na rychlost*/ + spd_pid(); } - 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); }else if(!rps.index_ok && rps.commutate){ simple_hall_commutator(rps.duty); } + + /*zalogujeme hodnoty*/ + if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){ + makeLog(); + } + sem_post(&rps.thd_par_sem); /*--post semaphore---*/ /* calculate next shot */ @@ -495,7 +711,6 @@ void * read_data(void* param){ } - /** * \brief Main function. */