X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/2fb1fa82b48a8f915fce254bfe887b891f476b68..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 4991443..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 @@ -197,66 +277,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 +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){ @@ -510,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; @@ -530,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*/ } - pid(); + + /* 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(); + } + + /* sirka plneni prepoctena na jednotlive pwm */ if (rps.index_ok && rps.commutate){ /*simple_ind_dist_commutator(rps.duty);*/ /*sin_commutator(rps.duty);*/ @@ -567,7 +632,6 @@ void * read_data(void* param){ } - /** * \brief Main function. */