X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/7b66566a24a0bccd2c061f59c35cdfc7fb88f354..cc9c3ee873b6560091e62e58b51a1507030e0d60:/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 65c6200..da278cc 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -15,31 +15,44 @@ #include /*sheduler*/ #include /*usleep*/ #include /*threads*/ +#include /*nanosleep*/ #include "rpin.h" /*gpclk*/ #include "rp_spi.h" /*spi*/ #include "misc.h" /*structure for priorities*/ +#include "pxmc_sin_fixed.h" /*to test sin commutation */ +#include "pmsm_state.h" +#include "cmd_proc.h" - -#define PRUM_PROUD 2061 -#define PRUM_SOUC 6183 +#define MAX_DUTY 128 +#define PID_P 0.1 #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*/ -struct sigaction sighnd; /*struktura pro signal handler*/ -struct rpi_in data; -uint8_t test; -uint16_t pwm1, pwm2, pwm3; -char commutate; -uint16_t simple_hall_duty; +#define NSEC_PER_SEC (1000000000) /* The number of nsecs per sec. */ +struct rpi_in data; +struct rpi_state rps={ + .spi_dat=&data, + .thd_par_sem=NULL, + .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 */ +}; + /** * \brief Initilizes GPCLK. */ @@ -61,11 +74,12 @@ inline void clk_disable(){ /** * \brief Signal handler pro Ctrl+C */ -void sighnd_fnc(){ +void appl_stop(){ spi_disable(); clk_disable(); + /*muzeme zavrit semafor*/ + sem_destroy(&rps.thd_par_sem); printf("\nprogram bezpecne ukoncen\n"); - exit(0); } void substractOffset(struct rpi_in* data, struct rpi_in* offset){ @@ -73,53 +87,8 @@ void substractOffset(struct rpi_in* data, struct rpi_in* offset){ 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){ - float cur0, cur1, cur2; - int i; - if (data.adc_m_count){ - cur0=data.ch0/data.adc_m_count; - cur1=data.ch1/data.adc_m_count; - cur2=data.ch2/data.adc_m_count; - } - for (i = 0; i < 16; i++) { - if (!(i % 6)) - puts(""); - printf("%.2X ", data.debug_rx[i]); - } - puts(""); - printf("\npozice=%d\n",(int32_t)data.pozice); - printf("raw_pozice=%d\n",(int32_t)data.pozice_raw); - printf("raw_pozice last11=%u\n",(data.pozice_raw&0x7FF)); - printf("index position=%d\n",(int16_t)data.index_position); - printf("hal1=%d, hal2=%d, hal3=%d\n",data.hal1,data.hal2,data.hal3); - printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&test),!!(0x20&test),!!(0x10&test)); - printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&test),!!(0x04&test),!!(0x02&test)); - printf("PWM1=%u(L.s.)\n",pwm1); - printf("PWM2=%u(L.s.)\n",pwm2); - printf("PWM3=%u(L.s.)\n",pwm3); - printf("Pocet namerenych proudu=%u\n",data.adc_m_count); - printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data.ch1,cur1,diff_p(cur1)); - printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch2,cur2,diff_p(cur2)); - printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch0,cur0,diff_p(cur0)); - printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2)); - printf("duty=%u\n",simple_hall_duty); - if (commutate) printf("commutation in progress\n"); -} + + void prepare_tx(uint8_t * tx){ /*Data format: @@ -145,36 +114,38 @@ void prepare_tx(uint8_t * tx){ * bit 89 - shutdown3 * . * . + * Unused + * . * . - * bits 66 .. 56 - match PWM1 - * bits 55 .. 45 - match PWM2 - * bit 11,12 - Unused - * bits 42 .. 32 - match PWM3 + * bits 47 .. 32 - match PWM1 + * bits 31 .. 16 - match PWM2 + * bits 15 .. 0 - match PWM3 */ uint16_t tmp; - /* keep the cap*/ - if (pwm1>2047) pwm1=2047; - if (pwm2>2047) pwm2=2047; - if (pwm3>2047) pwm3=2047; + /* 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]=test; /*bit 94 - enable PWM1*/ + 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[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&pwm1)[1]); /*MSB*/ - tx[8]=((uint8_t*)&pwm1)[0]; /*LSB*/ + tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/ + tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/ /*pwm2*/ - tmp=pwm2; - tmp<<=5; - tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/ - tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/ + tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/ + tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/ /*pwm3*/ - tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&pwm3)[1]); /*MSB*/ - tx[11]=((uint8_t*)&pwm3)[0]; /*LSB*/ + tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/ + tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/ } @@ -182,136 +153,334 @@ void prepare_tx(uint8_t * tx){ * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru */ void * pos_monitor(void* param){ - set_priority(param); /*set priority*/ while(1){ - printData(data); + printData(&rps); usleep(1000000); /*1 Hz*/ } return (void*)0; } +/* + * \brief + * Multiplication of 11 bit + * Zaporne vysledky prvede na nulu. + */ +inline uint16_t mult_cap(int32_t s,int d){ + int j; + int res=0; + for(j=0;j!=11;j++){ + /* multiplicate as if maximum sinus value was unity */ + res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j)); + } + return res; +} +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; + pos=rps.index_dist; + int32_t pwm; + /*aby prictene uhly mohla byt kulata cisla, musime index posunout*/ + pos+=38; + /*use it as cyclic 32-bit logic*/ + pos*=4294967; + if (duty>=0){ /*clockwise rotation*/ + /* 1st phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/ + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm1=(uint16_t)pwm; + + /* 2nd phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/ + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm2=(uint16_t)pwm; + + /* 3rd phase */ + sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/ + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm3=(uint16_t)pwm; + }else{ + duty=-duty; + + /* 1st phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/ + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm1=(uint16_t)pwm; + + /* 2nd phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/ + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm2=(uint16_t)pwm; + + /* 3rd phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/ + pwm=sin*duty/1024; + if (pwm<0) pwm=0; + rps.pwm3=(uint16_t)pwm; + } + return 0; +} /* * \brief * Test function to be placed in controll loop. - * Clockwise rotation with PWM with 64 out of 2048 duty cycle. + * Switches PWM's at point where they produce same force. + * This points are found thanks to IRC position, */ -inline void simple_hall_commutator(struct rpi_in data){ - /* pwm3 */ - if (data.hal2 && !data.hal3){ - pwm1=0; - pwm2=0; - pwm3=simple_hall_duty; - /* pwm1 */ - }else if (data.hal1 && !data.hal2){ - pwm1=simple_hall_duty; - pwm2=0; - pwm3=0; - /* pwm2 */ - }else if (!data.hal1 && data.hal3){ - pwm1=0; - pwm2=simple_hall_duty; - pwm3=0; +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 + * \brief + * Computation of distance to index. + * + * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu + * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu - + * to je 3999 bodu + * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate + * signalu indexu + */ +void comIndDist(){ + uint16_t pos = 0x0FFF & data.pozice_raw; + uint16_t dist; + uint16_t index = data.index_position; + + if (index<1999){ /*index e<0,1998> */ + if (pos */ + /*proti smeru h.r. od indexu*/ + dist=pos+2000-index; + }else if (pos<=index+1999){ /*pozice e */ + /*po smeru h.r. od indexu*/ + dist=pos-index; + }else if (pos */ + goto index_lost; + }else{ /*pozice e */ + /*proti smeru h.r. od indexu - podtecena pozice*/ + dist=pos-index-2096; + } + }else if (index<=2096){ /*index e<1999,2096>*/ + if (pos */ + goto index_lost; + }else if (pos */ + /*proti smeru h.r. od indexu*/ + dist=pos+2000-index; + }else if (pos<=index+1999){ /*pozice e */ + /*po smeru h.r. od indexu*/ + dist=pos-index; + }else { /*pozice e */ + goto index_lost; + } + }else{ /*index e<2097,4095> */ + if (pos<=index-2097){ /*pozice e<0,index-2097> */ + /*po smeru h.r. od indexu - pretecena pozice*/ + dist=4096+pos-index; + }else if (pos */ + goto index_lost; + }else if (pos */ + /*proti smeru h.r. od indexu*/ + dist=pos+2000-index; + }else{ /*pozice e */ + /*po smeru h.r. od indexu*/ + dist=pos-index; + } + } + + rps.index_dist = dist; + return; + + index_lost: + rps.index_ok=0; + return; +} +/* + * \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. */ void * read_data(void* param){ int i; struct rpi_in pocatek; + struct timespec t; + int interval = 1000000; /* 1ms ~ 1kHz*/ uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ; - set_priority(param); /*set priority*/ + char first=1; + uint16_t last_index; /*we have index up-to date*/ pocatek = spi_read(tx); + clock_gettime(CLOCK_MONOTONIC ,&t); + /* start after one second */ + t.tv_sec++; while(1){ - prepare_tx(tx); - data = spi_read(tx); + /* wait until next shot */ + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); + sem_wait(&rps.thd_par_sem); /*---take semaphore---*/ + prepare_tx(tx); /*save the data to send*/ + data = spi_read(tx); /*exchange data*/ + /*subtract initiate postion */ + rps.tf_count++; substractOffset(&data,&pocatek); - if (commutate){ - simple_hall_commutator(data); + comIndDist(); + if (!rps.index_ok){ + if (first){ + last_index=data.index_position; + first=0; + }else if (last_index!=data.index_position){ + rps.index_ok=1; + } } - usleep(1000); /*1kHz*/ + pid(); + if (rps.index_ok && rps.commutate){ + /*simple_ind_dist_commutator(rps.duty);*/ + sin_commutator(rps.duty); + }else if(!rps.index_ok && rps.commutate){ + simple_hall_commutator(rps.duty); + } + 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*/ - - + sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE); + setup_environment(); - /*ziskavani dat z motoru*//*vysoka priorita*/ - tsp.sch_prior = PRIOR_HIGH; - pthread_create(&tid, &attr, read_data, (void*)&tsp); + base_thread_id=pthread_self(); - /*vypisovani lokalni pozice*//*nizka priorita*/ - tsp.sch_prior = PRIOR_LOW; - pthread_create(&tid, &attr, pos_monitor, (void*)&tsp); + /*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); - /*muzeme zavrit semafor*/ - sem_destroy(&thd_par_sem); - /* - * 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); - pwm1=tmp&0xFFF; - break; - case 2: - scanf("%u",&tmp); - pwm2=tmp&0xFFF; - break; - case 3: - scanf("%u",&tmp); - pwm3=tmp&0xFFF; - break; - case 4: - scanf("%u",&tmp); - test=tmp&0xFF; - break; - case 5: - commutate=!commutate; - /* switch off pwms at the end of commutation */ - pwm1&=commutate*0xFFFF; - pwm2&=commutate*0xFFFF; - pwm3&=commutate*0xFFFF; - break; - case 6: - scanf("%u",&tmp); - simple_hall_duty=tmp&0xFFF; - break; - - default: - break; - } + /*wait for commands*/ + poll_cmd(&rps); - } return 0; }