From 534ec7b705dc81c66501a108dc864256ef32651f Mon Sep 17 00:00:00 2001 From: Martin Prudek Date: Sat, 18 Apr 2015 20:27:21 +0200 Subject: [PATCH] Simple IRC position commutator added. And Very simple PID reg. added. --- pmsm-control/test_sw/main_pmsm.c | 98 ++++++++++++++++++++++++++++++-- 1 file changed, 92 insertions(+), 6 deletions(-) diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c index 7e29253..4a03c3a 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -23,6 +23,8 @@ #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 @@ -37,10 +39,11 @@ struct rpi_state{ uint8_t test; uint16_t pwm1, pwm2, pwm3; char commutate; - int simple_hall_duty; + 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; @@ -117,6 +120,7 @@ void printData(){ } 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)); @@ -133,7 +137,7 @@ void printData(){ 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.simple_hall_duty); + printf("duty=%d\n",s.duty); if (s.index_ok) printf("index ok\n"); if (s.commutate) printf("commutation in progress\n"); } @@ -206,12 +210,63 @@ void * pos_monitor(void* param){ } return (void*)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(struct rpi_in data, int duty){ +inline void simple_hall_commutator(int duty){ if (duty>=0){ /* clockwise - so that position increase */ /* pwm3 */ if (data.hal2 && !data.hal3){ @@ -259,6 +314,29 @@ inline void comIndDist(){ */ 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 + */ void * read_data(void* param){ int i; struct rpi_in pocatek; @@ -283,10 +361,11 @@ void * read_data(void* param){ rps.index_ok=1; } } + pid(); if (rps.index_ok && rps.commutate){ - simple_hall_commutator(data,rps.simple_hall_duty); + simple_ind_dist_commutator(rps.duty); }else if(!rps.index_ok && rps.commutate){ - simple_hall_commutator(data,rps.simple_hall_duty); + simple_hall_commutator(rps.duty); } sem_post(&thd_par_sem); /*--post semaphore---*/ usleep(1000); /*1kHz*/ @@ -377,7 +456,13 @@ int main(){ case 6: scanf("%d",&tmp); sem_wait(&thd_par_sem); - rps.simple_hall_duty=tmp; + 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; @@ -388,3 +473,4 @@ int main(){ } return 0; } + -- 2.39.2