From: Martin Prudek Date: Wed, 20 May 2015 15:40:27 +0000 (+0200) Subject: Controllers moved to separate file. X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/commitdiff_plain/34c6632a92d19c06bbdef9dcc4eb754b21ade81d Controllers moved to separate file. --- diff --git a/pmsm-control/test_sw/Makefile b/pmsm-control/test_sw/Makefile index 91f533a..d2e19af 100644 --- a/pmsm-control/test_sw/Makefile +++ b/pmsm-control/test_sw/Makefile @@ -49,5 +49,5 @@ blikej: howto_gpio.o rpi_hw.o spi: rp_spi.o gcc -o spi rp_spi.c #pro rpi -pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o - gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o -lpthread -lrt +pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o + gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o -lpthread -lrt diff --git a/pmsm-control/test_sw/controllers.c b/pmsm-control/test_sw/controllers.c new file mode 100644 index 0000000..059ade6 --- /dev/null +++ b/pmsm-control/test_sw/controllers.c @@ -0,0 +1,40 @@ + +#include "controllers.h" + +/* + * \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 + */ +void pos_pid(struct rpi_state* state){ + int duty_tmp; + duty_tmp = PID_P*(state->desired_pos - (int32_t)state->spi_dat->pozice); + if (duty_tmp>MAX_DUTY){ + state->duty=MAX_DUTY; + }else if (duty_tmp<-MAX_DUTY){ + state->duty=-MAX_DUTY; + }else{ + state->duty = duty_tmp; + } +} + +/* + * \brief + * Very simple PID regulator. + * FIXME: make better + */ +void spd_pid(struct rpi_state* state){ + int duty_tmp; + int error; + error=state->desired_spd - state->speed; + state->spd_err_sum+=error; + duty_tmp = PID_P_S*error+PID_I_S*state->spd_err_sum; + if (duty_tmp>MAX_DUTY){ + state->duty=MAX_DUTY; + }else if (duty_tmp<-MAX_DUTY){ + state->duty=-MAX_DUTY; + }else{ + state->duty = duty_tmp; + } +} diff --git a/pmsm-control/test_sw/controllers.h b/pmsm-control/test_sw/controllers.h new file mode 100644 index 0000000..520745e --- /dev/null +++ b/pmsm-control/test_sw/controllers.h @@ -0,0 +1,38 @@ +/** + * \brief + * Speed and position regulators. + * \author Martin Prudek + * \file controllers.h + * + */ +#ifndef CONTROLLERS +#define CONTROLLERS + +#include "pmsm_state.h" /*pmsm state*/ +#include "rp_spi.h" /*spi struct*/ + +#define PID_P 0.3 + +/* RL-tool results from first order system approx */ +/* P=0.16 I=13,4/freq=0.013 */ +/* Hodnoty upraveny podle skutecnych vysledku */ +/* P=0.8 I=0.01 */ + +#define PID_P_S 0.16 /*2.3 kmita*/ /*1.4 vhodne jen pro P regulator*/ +#define PID_I_S 0.0134 + +/** + * \brief + * Very simple position P regulator. + * Now only with P-part so that the error doesnt go to zero. + */ +void pos_pid(struct rpi_state* state); + +/** + * \brief + * Very simple PI speed regulator. + */ +void spd_pid(struct rpi_state* state); + + +#endif /*CONTROLLERS*/ diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c index e4ec9ac..84b1cdd 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -24,17 +24,10 @@ #include "pxmc_sin_fixed.h" /*to test sin commutation */ #include "pmsm_state.h" #include "cmd_proc.h" +#include "controllers.h" -#define PID_P 0.3 -/* RL-tool results from first order system approx */ -/* P=0.16 I=13,4/freq=0.013 */ -/* Hodnoty upraveny podle skutecnych vysledku */ -/* P=0.8 I=0.01 */ - -#define PID_P_S 0.16 /*2.3 kmita*/ /*1.4 vhodne jen pro P regulator*/ -#define PID_I_S 0.0134 #define PRIOR_KERN 50 #define PRIOR_HIGH 49 @@ -49,6 +42,7 @@ struct rpi_in data; struct rpi_state rps={ + //.MAX_DUTY=170, .spi_dat=&data, .test=0, .pwm1=0,.pwm2=0, .pwm3=0, @@ -372,6 +366,7 @@ inline uint16_t mult_cap(int32_t s,int d){ } return res; } + inline int sin_commutator(int duty){ #define DEGREE_60 715827883 @@ -584,43 +579,7 @@ void comIndDist(){ 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 pos_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 - * 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 @@ -677,9 +636,9 @@ void * read_data(void* param){ /* pocitame sirku plneni podle potreb rizeni*/ if (rps.pos_reg_ena){ /*pozicni rizeni*/ - pos_pid(); + pos_pid(&rps); }else if(rps.spd_reg_ena){ /*rizeni na rychlost*/ - spd_pid(); + spd_pid(&rps); } /* sirka plneni prepoctena na jednotlive pwm */ diff --git a/pmsm-control/test_sw/pmsm_state.h b/pmsm-control/test_sw/pmsm_state.h index b3a3125..5d02be2 100644 --- a/pmsm-control/test_sw/pmsm_state.h +++ b/pmsm-control/test_sw/pmsm_state.h @@ -16,6 +16,8 @@ struct rpi_in; struct rpi_state{ + //const unsigned MAX_DUTY; /*Max duty*/ + struct rpi_in* spi_dat; /* spi data */ sem_t thd_par_sem; /* data metual exlusion access */ uint8_t test; /* configuratin byte - pwm enabl. bits etc. */