X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/694d7155f582476ae9c042258e98fc867eb6b1ba..500f4f3e341a3443641469d44e2219a94939da6a:/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..4d58037 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,10 @@ #include "pmsm_state.h" #include "cmd_proc.h" -#define MAX_DUTY 170 + #define PID_P 0.3 +#define PID_P_S 0.8 /*2.3 kmita*/ /*1.4 vhodne jen pro P regulator*/ +#define PID_I_S 0.01 #define PRIOR_KERN 50 #define PRIOR_HIGH 49 @@ -49,7 +52,15 @@ 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 }; /** @@ -88,20 +99,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; } /* @@ -197,66 +340,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 */ @@ -499,7 +583,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){ @@ -510,9 +594,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; @@ -530,21 +646,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);*/ @@ -553,6 +684,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 */ @@ -567,7 +704,6 @@ void * read_data(void* param){ } - /** * \brief Main function. */