X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/1a8d767b097d1536b264d8bae3e9d5c16617f835..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 4ae245b..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,9 +25,17 @@ #include "pmsm_state.h" #include "cmd_proc.h" -#define MAX_DUTY 170 + #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 #define PRIOR_LOW 20 @@ -52,7 +61,13 @@ struct rpi_state rps={ .desired_pos=0, /* desired position */ .pos_reg_ena=0, .desired_spd=0, - .spd_reg_ena=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 }; /** @@ -91,20 +106,152 @@ int32_t min(int32_t x, int32_t y, int32_t z){ return x; } +/* + * \brief + * Free logs + */ +void freeLogs(){ + int r; + if (rps.log_col_count){ + for (r=0;rpozice; + + 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;r2047) 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(); + freeLogs(); /*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; + data->pozice=data->pozice_raw-offset->pozice_raw; return; } /* @@ -178,7 +325,7 @@ void inv_trans_comm_2(int duty){ int32_t u1,u2,u3; pos=rps.index_dist; - pos+=960; /*zarovnani faze 'a' s osou 'alpha'*/ + 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; @@ -200,66 +347,7 @@ void inv_trans_comm_2(int duty){ 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 */ @@ -513,6 +601,37 @@ inline void pos_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. @@ -534,11 +653,15 @@ 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); + compSpeed(); /*spocita rychlost*/ if (!rps.index_ok){ if (first){ @@ -551,10 +674,14 @@ void * read_data(void* param){ }else{ /*index je v poradku*/ comIndDist(); /*vypocet vzdalenosti indexu*/ } + /* pocitame sirku plneni podle potreb rizeni*/ - if (rps.pos_reg_ena){ + if (rps.pos_reg_ena){ /*pozicni rizeni*/ pos_pid(); + }else if(rps.spd_reg_ena){ /*rizeni na rychlost*/ + spd_pid(); } + /* sirka plneni prepoctena na jednotlive pwm */ if (rps.index_ok && rps.commutate){ /*simple_ind_dist_commutator(rps.duty);*/ @@ -564,6 +691,12 @@ void * read_data(void* param){ }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 */ @@ -578,7 +711,6 @@ void * read_data(void* param){ } - /** * \brief Main function. */