X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/70b98ccdfac6bb0b8284cb0b9b702e5eddb86a7c..1cf729421e63f859e426a429f9ce935a86afd167:/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 c9b9fe9..2eb9262 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -15,54 +15,53 @@ #include /*sheduler*/ #include /*usleep*/ #include /*threads*/ +#include /*nanosleep*/ +#include #include "rpin.h" /*gpclk*/ #include "rp_spi.h" /*spi*/ #include "misc.h" /*structure for priorities*/ -#include "pxmc_sin_fixed.h" /*to test sin commutation */ - - - -#define PRUM_PROUD 2061 -#define PRUM_SOUC 6183 -#define MAX_DUTY 128 -#define PID_P 0.1 +#include "pmsm_state.h" +#include "cmd_proc.h" +#include "controllers.h" +#include "commutators.h" +#include "comp.h" +#include "logs.h" #define PRIOR_KERN 50 #define PRIOR_HIGH 49 #define PRIOR_LOW 20 #define THREAD_SHARED 0 -#define INIT_VALUE 0 /*init value for semaphor*/ +#define INIT_VALUE 1 /*init value for semaphor*/ -#define PXMC_SIN_FIX_TAB_BITS 9 -#define PXMC_SIN_FIX_IDX_SLR 23 -#define PXMC_SIN_FIX_XD_MASK 0x007fffff -#define PXMC_SIN_FIX_XD_SLR 8 -#define PXMC_SIN_FIX_A_MASK 0xffffc000 -#define PXMC_SIN_FIX_B_SLL 19 -#define PXMC_SIN_FIX_B_SAR 16 -#define PXMC_SIN_FIX_B_XD_SAR 6 -#define PXMC_SIN_FIX_ZIC_MASK 0x00002000 -#define PXMC_SIN_FIX_ZIC_BIT 13 +#define NSEC_PER_SEC (1000000000) /* The number of nsecs per sec. */ -#define PXMC_SIN_FIX_PI2 0x40000000 -#define PXMC_SIN_FIX_2PI3 0x55555555 -struct sigaction sighnd; /*struktura pro signal handler*/ struct rpi_in data; -struct rpi_state{ - uint8_t test; - uint16_t pwm1, pwm2, pwm3; - uint16_t t_pwm1, t_pwm2, t_pwm3; - char commutate; - int duty; /* duty cycle of pwm */ - uint16_t index_dist; /* distance to index position */ - unsigned char index_ok; - uint32_t tf_count; /*number of transfer*/ - int desired_pos; /* desired position */ -}rps; +struct rpi_state rps={ + //.MAX_DUTY=170, + .spi_dat=&data, + .test=0, + .pwm1=0,.pwm2=0, .pwm3=0, + .pwm1=0, .t_pwm2=0, .t_pwm3=0, + .commutate=0, + .duty=0, /* duty cycle of pwm */ + .index_dist=0, /* distance to index position */ + .index_ok=0, + .tf_count=0, /*number of transfer*/ + .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 +}; /** @@ -75,496 +74,142 @@ int clk_init() gpioSetMode(4, FSEL_ALT0); return 0; } -/* - * \brief Terminates GPCLK. - */ - -inline void clk_disable(){ - termClock(0); -} /** * \brief Signal handler pro Ctrl+C */ -void sighnd_fnc(){ +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; + spi_read(&rps); + spi_disable(); - clk_disable(); + termClock(0); + freeLogs(&rps); /*muzeme zavrit semafor*/ - sem_destroy(&thd_par_sem); + sem_destroy(&rps.thd_par_sem); printf("\nprogram bezpecne ukoncen\n"); - exit(0); } -void substractOffset(struct rpi_in* data, struct rpi_in* offset){ - data->pozice_raw=data->pozice; - data->pozice-=offset->pozice; - return; -} -/* - * pocita procentualni odchylku od prumerneho proudu - */ -float diff_p(float value){ - return ((float)value-PRUM_PROUD)*100/PRUM_PROUD; -} -/* - * pocita procentualni odchylku od prumerneho souctu proudu - */ -float diff_s(float value){ - return ((float)value-PRUM_SOUC)*100/PRUM_SOUC; -} -/* - * tiskne potrebna data - */ -void printData(){ - struct rpi_in data_p; - struct rpi_state s; /*state*/ - float cur0, cur1, cur2; - int i; - /* copy the data */ - sem_wait(&thd_par_sem); - data_p = data; - s=rps; - sem_post(&thd_par_sem); - - if (data_p.adc_m_count){ - cur0=data_p.ch0/data_p.adc_m_count; - cur1=data_p.ch1/data_p.adc_m_count; - cur2=data_p.ch2/data_p.adc_m_count; - } - for (i = 0; i < 16; i++) { - if (!(i % 6)) - puts(""); - printf("%.2X ", data_p.debug_rx[i]); - } - puts(""); - printf("\npozice=%d\n",(int32_t)data_p.pozice); - printf("chtena pozice=%d\n",s.desired_pos); - printf("transfer count=%u\n",s.tf_count); - printf("raw_pozice=%d\n",(int32_t)data_p.pozice_raw); - printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF)); - printf("index position=%u\n",data_p.index_position); - printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3); - printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test)); - printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test)); - printf("PWM1=%u(L.s.)\n",s.pwm1); - printf("PWM2=%u(L.s.)\n",s.pwm2); - printf("PWM3=%u(L.s.)\n",s.pwm3); - printf("distance to index=%u\n",s.index_dist); - printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3); - printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count); - printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1)); - printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2)); - printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0)); - printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2)); - printf("duty=%d\n",s.duty); - if (s.index_ok) printf("index ok\n"); - if (s.commutate) printf("commutation in progress\n"); -} -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 */ void * pos_monitor(void* param){ - set_priority(param); /*set priority*/ while(1){ - printData(); + printData(&rps); usleep(1000000); /*1 Hz*/ } return (void*)0; } -inline -int sin_commutator(int duty){ - #define DEGREE_60 715827883 - #define DEGREE_120 1431655765 - #define DEGREE_180 2147483648 - #define DEGREE_240 2863311531 - #define DEGREE_300 3579139413 - uint32_t j,pos; - int32_t sin; - uint16_t pwm; - pos=rps.index_dist*4294967; - if (duty>=0){ /*clockwise rotation*/ - /* 1st phase */ - sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm1=pwm; - - /* 2nd phase */ - sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm2=pwm; - - /* 3rd phase */ - sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm3=pwm; - }else{ - duty=-duty; - - /* 1st phase */ - sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm1=pwm; - - /* 2nd phase */ - sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm2=pwm; - - /* 3rd phase */ - sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/ - pwm=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); - } - rps.pwm3=pwm; - } - return 0; -} -/* - * \brief - * Test function to be placed in controll loop. - * Switches PWM's at point where they produce same force. - * This points are found thanks to IRC position, - */ -inline -void simple_ind_dist_commutator(int duty){ - if (duty>=0){ /* clockwise - so that position increase */ - /* pwm3 */ - if ((rps.index_dist>=45 && rps.index_dist<=373) || - (rps.index_dist>=1048 && rps.index_dist<=1377)){ - rps.pwm1=0; - rps.pwm2=0; - rps.pwm3=duty; - /* pwm1 */ - }else if ((rps.index_dist>=373 && rps.index_dist<=711) || - (rps.index_dist>=1377 && rps.index_dist<=1711)){ - rps.pwm1=duty; - rps.pwm2=0; - rps.pwm3=0; - /* pwm2 */ - }else if ((rps.index_dist>=0 && rps.index_dist<=45) || - (rps.index_dist>=711 && rps.index_dist<=1048) || - (rps.index_dist>=1711 && rps.index_dist<=1999)){ - rps.pwm1=0; - rps.pwm2=duty; - rps.pwm3=0; - } - }else{ /*counter-clockwise - position decrease */ - /* pwm3 */ - if ((rps.index_dist>=544 && rps.index_dist<=881) || - (rps.index_dist>=1544 && rps.index_dist<=1878)){ - rps.pwm1=0; - rps.pwm2=0; - rps.pwm3=-duty; - /* pwm1 */ - }else if ((rps.index_dist>=0 && rps.index_dist<=211) || - (rps.index_dist>=881 && rps.index_dist<=1210) || - (rps.index_dist>=1878 && rps.index_dist<=1999)){ - rps.pwm1=-duty; - rps.pwm2=0; - rps.pwm3=0; - /* pwm2 */ - }else if ((rps.index_dist>=211 && rps.index_dist<=544) || - (rps.index_dist>=1210 && rps.index_dist<=1544)){ - rps.pwm1=0; - rps.pwm2=-duty; - rps.pwm3=0; - } - } -} -/* - * \brief - * Test function to be placed in controll loop. - * Switches PWM's at point where they produce same force - */ -inline void simple_hall_commutator(int duty){ - if (duty>=0){ /* clockwise - so that position increase */ - /* pwm3 */ - if (data.hal2 && !data.hal3){ - rps.pwm1=0; - rps.pwm2=0; - rps.pwm3=duty; - /* pwm1 */ - }else if (data.hal1 && !data.hal2){ - rps.pwm1=duty; - rps.pwm2=0; - rps.pwm3=0; - /* pwm2 */ - }else if (!data.hal1 && data.hal3){ - rps.pwm1=0; - rps.pwm2=duty; - rps.pwm3=0; - } - }else{ /*counter-clockwise - position decrease */ - /* pwm3 */ - if (!data.hal2 && data.hal3){ - rps.pwm1=0; - rps.pwm2=0; - rps.pwm3=-duty; - /* pwm1 */ - }else if (!data.hal1 && data.hal2){ - rps.pwm1=-duty; - rps.pwm2=0; - rps.pwm3=0; - /* pwm2 */ - }else if (data.hal1 && !data.hal3){ - rps.pwm1=0; - rps.pwm2=-duty; - rps.pwm3=0; - } - } -} -/** - * Funkce pravidelne vycita data z motoru - */ -inline void comIndDist(){ - rps.index_dist=0x0FFF & (data.pozice_raw - data.index_position); - /* - * if distance is bigger than 2047, the distance underflown - * -> if 12th bit is set, substract 2096 - */ - rps.index_dist-=((rps.index_dist & 0x0800)>>11)*2096; -} -/* - * \brief - * Very simple PID regulator. - * 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(){ - int duty_tmp; - duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice); - 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 * Feedback loop. - * TODO: replace usleep with real-time wait - * measure times + * TODO: replace bunch of 'IFs' with Object-like pattern */ void * read_data(void* param){ int i; struct rpi_in pocatek; - uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ; + struct rpi_state poc={ + .spi_dat=&pocatek, + .test=0, + .pwm1=0, .pwm1=0, .pwm3=0 + }; + struct timespec t; + int interval = 1000000; /* 1ms ~ 1kHz*/ char first=1; uint16_t last_index; /*we have index up-to date*/ - set_priority(param); /*set priority*/ - pocatek = spi_read(tx); + spi_read(&poc); /*pocatecni informace*/ + clock_gettime(CLOCK_MONOTONIC ,&t); + /* start after one second */ + t.tv_sec++; while(1){ - sem_wait(&thd_par_sem); /*---take semaphore---*/ - prepare_tx(tx); /*save the data to send*/ - data = spi_read(tx); /*exchange data*/ + /* 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; + spi_read(&rps); /*exchange data*/ /*subtract initiate postion */ rps.tf_count++; - substractOffset(&data,&pocatek); - comIndDist(); + substractOffset(&data,poc.spi_dat); + compSpeed(&rps); /*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(&rps); /*vypocet vzdalenosti indexu*/ } + }else{ /*index je v poradku*/ + comIndDist(&rps); /*vypocet vzdalenosti indexu*/ + } + + /* pocitame sirku plneni podle potreb rizeni*/ + if (rps.pos_reg_ena){ /*pozicni rizeni*/ + pos_pid(&rps); + }else if(rps.spd_reg_ena){ /*rizeni na rychlost*/ + spd_pid(&rps); } - pid(); + + /* sirka plneni prepoctena na jednotlive pwm */ if (rps.index_ok && rps.commutate){ /*simple_ind_dist_commutator(rps.duty);*/ - sin_commutator(rps.duty); + /*sin_commutator(rps.duty);*/ + inv_trans_comm(&rps); + inv_trans_comm_2(&rps); }else if(!rps.index_ok && rps.commutate){ - simple_hall_commutator(rps.duty); + simple_hall_commutator(&rps); } - sem_post(&thd_par_sem); /*--post semaphore---*/ - usleep(500); /*1kHz*/ + + /*zalogujeme hodnoty*/ + if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){ + makeLog(&rps); + } + + sem_post(&rps.thd_par_sem); /*--post semaphore---*/ + + /* calculate next shot */ + t.tv_nsec += interval; + + while (t.tv_nsec >= NSEC_PER_SEC) { + t.tv_nsec -= NSEC_PER_SEC; + t.tv_sec++; + } + } } /** * \brief Main function. */ - int main(){ - unsigned int tmp; - - /*nastaveni priorit vlaken*/ - struct thread_param tsp; - tsp.sch_policy = SCHED_FIFO; - - /*nastaveni signalu pro vypnuti pomoci Ctrl+C*/ - sighnd.sa_handler=&sighnd_fnc; - sigaction(SIGINT, &sighnd, NULL ); - + pthread_t base_thread_id; clk_init(); /* inicializace gpio hodin */ spi_init(); /* iniicializace spi*/ /*semafor pro detekci zpracovani parametru vlaken*/ - sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE); - - /*vlakna*/ - pthread_t tid; /*identifikator vlakna*/ - pthread_attr_t attr; /*atributy vlakna*/ - pthread_attr_init(&attr); /*inicializuj implicitni atributy*/ - - - - /*ziskavani dat z motoru*//*vysoka priorita*/ - tsp.sch_prior = PRIOR_HIGH; - pthread_create(&tid, &attr, read_data, (void*)&tsp); - - /*vypisovani lokalni pozice*//*nizka priorita*/ - tsp.sch_prior = PRIOR_LOW; - sem_wait(&thd_par_sem); - pthread_create(&tid, &attr, pos_monitor, (void*)&tsp); - - - - /* - * Note: - * pri pouziti scanf("%u",&simple_hall_duty); dochazelo - * k preukladani hodnot na promenne test. Dost divne. - */ - while (1){ - scanf("%u",&tmp); - printf("volba=%u\n",tmp); - switch (tmp){ - case 1: - scanf("%u",&tmp); - sem_wait(&thd_par_sem); - rps.pwm1=tmp&0xFFF; - sem_post(&thd_par_sem); - break; - case 2: - scanf("%u",&tmp); - sem_wait(&thd_par_sem); - rps.pwm2=tmp&0xFFF; - sem_post(&thd_par_sem); - break; - case 3: - scanf("%u",&tmp); - sem_wait(&thd_par_sem); - rps.pwm3=tmp&0xFFF; - sem_post(&thd_par_sem); - break; - case 4: - scanf("%u",&tmp); - sem_wait(&thd_par_sem); - rps.test=tmp&0xFF; - sem_post(&thd_par_sem); - break; - case 5: - sem_wait(&thd_par_sem); - rps.commutate=!rps.commutate; - /* switch off pwms at the end of commutation */ - rps.pwm1&=rps.commutate*0xFFFF; - rps.pwm2&=rps.commutate*0xFFFF; - rps.pwm3&=rps.commutate*0xFFFF; - sem_post(&thd_par_sem); - break; - case 6: - scanf("%d",&tmp); - sem_wait(&thd_par_sem); - rps.duty=tmp; - sem_post(&thd_par_sem); - break; - case 7: - scanf("%d",&tmp); - sem_wait(&thd_par_sem); - rps.desired_pos=tmp; - sem_post(&thd_par_sem); - break; - - default: - break; - } + sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE); + setup_environment(); + + base_thread_id=pthread_self(); + + /*main control loop*/ + create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL); + + /*monitor of current state*/ + create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL); + + /*wait for commands*/ + poll_cmd(&rps); - } return 0; } -