X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/0bed16505e1c8bc5de08c08ff05c36607130f218..85772a932e99215c8c86d92feb203ee928df0e5f:/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 7f66662..dd2372d 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,9 @@ #include "pmsm_state.h" #include "cmd_proc.h" -#define MAX_DUTY 128 -#define PID_P 0.1 + +#define PID_P 0.3 +#define PID_P_S 0.7 #define PRIOR_KERN 50 #define PRIOR_HIGH 49 @@ -49,7 +51,11 @@ 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} }; /** @@ -87,11 +93,86 @@ int32_t min(int32_t x, int32_t y, int32_t z){ return x; } +/* + * \brief + * Pripravi psi buffer + */ +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*/ + + +} + /** * \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; + prepare_tx(tx); /*save the data to send*/ + data=spi_read(tx); + spi_disable(); clk_disable(); /*muzeme zavrit semafor*/ @@ -100,8 +181,7 @@ void appl_stop(){ } void substractOffset(struct rpi_in* data, struct rpi_in* offset){ - data->pozice_raw=data->pozice; - data->pozice-=offset->pozice; + data->pozice=data->pozice_raw-offset->pozice_raw; return; } /* @@ -113,10 +193,10 @@ void dq2alphabeta(int32_t *alpha, int32_t *beta, int d, int q, int32_t sin, int3 *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 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 @@ -124,27 +204,21 @@ void alphabeta2pwm3(int32_t * pwma, int32_t * pwmb, int32_t *pwmc,int32_t alpha, * ktera budou privedena na svorky motoru. * Tedy na A(yel)-pwm1, B(red)-pwm2, C(blk)-pwm3 */ -void compPhases(int32_t * pwm1, int32_t * pwm2, int32_t *pwm3, int32_t ua , int32_t ub){ - int32_t u1,u2,u3; +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=ua+ub; - u2=ua; - u3=0; + *u1=uc; + *u2=uc+ub; + *u3=0; /*najde zaporne napeti*/ - t=min(u1,u2,u3); - - /*dorovna zaporna napeti napeti na nulu*/ - u1-=t; - u2-=t; - u3-=t; - - *pwm1=u1; - *pwm2=u2; - *pwm3=u3; + 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; @@ -176,85 +250,34 @@ void inv_trans_comm_2(int duty){ uint32_t pos; int32_t sin, cos; int32_t alpha, beta; - int32_t pwma,pwmb,pwmc; - int32_t pwm1,pwm2,pwm3; + int32_t ua,ub,uc; + int32_t ia,ib,ic; + int32_t u1,u2,u3; pos=rps.index_dist; - /*melo by byt urceno co nejpresneji, aby faze 'a' splyvala s osou 'alpha'*/ - pos+=460; - /*use it as cyclic 32-bit logic*/ + + 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(&pwma,&pwmb, &pwmc,alpha,beta); - compPhases(&pwm1,&pwm2, &pwm3,pwma,pwmb); + alphabeta2pwm3(&ia,&ib, &ic,alpha,beta); - rps.pwm1=(uint16_t)pwm1; - rps.pwm2=(uint16_t)pwm2; - rps.pwm3=(uint16_t)pwm3; -} -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*/ + 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 */ @@ -497,7 +520,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){ @@ -508,9 +531,38 @@ 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; + duty_tmp = PID_P_S*(rps.desired_spd - rps.speed); + 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; @@ -528,21 +580,36 @@ 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);*/ @@ -565,7 +632,6 @@ void * read_data(void* param){ } - /** * \brief Main function. */