Controll loop now more complex.

diff --git a/pmsm-control/test_sw/cmd_proc.c b/pmsm-control/test_sw/cmd_proc.c
index d5e30201f864ef8a2a79eccb6dfbd25acb6638ce..46ff07009a9a81638787ac206f7e12b78bddf29f 100644 (file)
--- a/pmsm-control/test_sw/cmd_proc.c
+++ b/pmsm-control/test_sw/cmd_proc.c
@@ -20,6 +20,8 @@ static void printHelp(){
        puts("0 - Vypne pwm a rizeni. Enable bity na nulu.");
        puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
        puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");
        puts("0 - Vypne pwm a rizeni. Enable bity na nulu.");
        puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
        puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");

+       puts("duty:[hodnota] - Nastavi pevnou sirku plneni.");
+

        puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
        puts("exit - Bezpecne ukonci program.");
 }
        puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
        puts("exit - Bezpecne ukonci program.");
 }


@@ -45,7 +47,6 @@ static void delCol(char * txt){
 static void start(struct rpi_state* state){
        sem_wait(&state->thd_par_sem);
        state->test=0x70;       /*konfiguracni byte*/
 static void start(struct rpi_state* state){
        sem_wait(&state->thd_par_sem);
        state->test=0x70;       /*konfiguracni byte*/

-       state->commutate=1;

        sem_post(&state->thd_par_sem);
 }
 
        sem_post(&state->thd_par_sem);
 }
 


@@ -57,6 +58,7 @@ static void stop(struct rpi_state* state){
        sem_wait(&state->thd_par_sem);
        state->test=0;          /*konfiguracni byte*/
        state->commutate=0;
        sem_wait(&state->thd_par_sem);
        state->test=0;          /*konfiguracni byte*/
        state->commutate=0;

+       state->pos_reg_ena=0;

        state->pwm1=0;
        state->pwm2=0;
        state->pwm3=0;
        state->pwm1=0;
        state->pwm2=0;
        state->pwm3=0;


@@ -68,7 +70,13 @@ static void stop(struct rpi_state* state){
  * Nastavi pevnou sirku plneni
  */
 static void dutySet(struct rpi_state* state, int duty){
  * Nastavi pevnou sirku plneni
  */
 static void dutySet(struct rpi_state* state, int duty){

-
+       sem_wait(&state->thd_par_sem);
+       if (duty>512) duty=512;
+       if (duty<-512) duty=-512;/*paranoia*/
+       state->duty=duty;
+       state->commutate=1;
+       state->pos_reg_ena=0;
+       sem_post(&state->thd_par_sem);

 }
 
 /*
 }
 
 /*


@@ -77,6 +85,7 @@ static void dutySet(struct rpi_state* state, int duty){
  */
 static void goAbsolute(struct rpi_state* state, int pos){
        sem_wait(&state->thd_par_sem);
  */
 static void goAbsolute(struct rpi_state* state, int pos){
        sem_wait(&state->thd_par_sem);

+       state->pos_reg_ena=1;

        state->desired_pos=pos;
        sem_post(&state->thd_par_sem);
 }
        state->desired_pos=pos;
        sem_post(&state->thd_par_sem);
 }

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index 56b926a11073d6f5a576be48533a6769d61eefca..4ae245b099f497af63c331fea9c32431b20852bb 100644 (file)
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -49,7 +49,10 @@ struct rpi_state rps={
        .index_dist=0,          /* distance to index position */
        .index_ok=0,
        .tf_count=0,            /*number of transfer*/
        .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

 };
 
 /**
 };
 
 /**


@@ -499,7 +502,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
  */
  * 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){
        int duty_tmp;
        duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice);
        if (duty_tmp>MAX_DUTY){


@@ -513,6 +516,7 @@ inline void pid(){
 /*
  * \brief
  * Feedback loop.
 /*
  * \brief
  * Feedback loop.

+ * TODO: replace bunch of 'IFs' with Object-like pattern

  */
 void * read_data(void* param){
        int i;
  */
 void * read_data(void* param){
        int i;


@@ -535,16 +539,23 @@ void * read_data(void* param){
                        /*subtract initiate postion */
                        rps.tf_count++;
                        substractOffset(&data,&pocatek);
                        /*subtract initiate postion */
                        rps.tf_count++;
                        substractOffset(&data,&pocatek);

-                       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;
                        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*/
+                       }
+                       /* pocitame sirku plneni podle potreb rizeni*/
+                       if (rps.pos_reg_ena){
+                               pos_pid();

                        }
                        }

-                       pid();
+                       /* sirka plneni prepoctena na jednotlive pwm */

                        if (rps.index_ok && rps.commutate){
                                /*simple_ind_dist_commutator(rps.duty);*/
                                /*sin_commutator(rps.duty);*/
                        if (rps.index_ok && rps.commutate){
                                /*simple_ind_dist_commutator(rps.duty);*/
                                /*sin_commutator(rps.duty);*/

diff --git a/pmsm-control/test_sw/pmsm_state.h b/pmsm-control/test_sw/pmsm_state.h
index 7c631f1c22e42d13dc454ec1014b9ccf0b2d2b6c..b50b25010aeb1edbbd46ba520b96904f48928482 100644 (file)
--- a/pmsm-control/test_sw/pmsm_state.h
+++ b/pmsm-control/test_sw/pmsm_state.h
@@ -7,17 +7,24 @@
 struct rpi_in;
 
 struct rpi_state{
 struct rpi_in;
 
 struct rpi_state{

-       struct rpi_in* spi_dat;
-       sem_t thd_par_sem;
-       uint8_t test;
-       uint16_t pwm1, pwm2, pwm3;
-       uint16_t t_pwm1, t_pwm2, t_pwm3;
-       char commutate;
-       int duty;                       /* duty cycle of pwm */
+       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. */
+       uint32_t tf_count;              /* number of transfer*/
+

        uint16_t index_dist;            /* distance to index position */
        uint16_t index_dist;            /* distance to index position */

-       unsigned char index_ok;
-       uint32_t tf_count;              /*number of transfer*/
+       unsigned char index_ok;         /* we have actual index position */
+
+       uint16_t pwm1, pwm2, pwm3;      /* pwm duty cycles*/
+       uint16_t t_pwm1, t_pwm2, t_pwm3;/* debug*/
+
+       int duty;                       /* duty cycle of pwm */

        int desired_pos;                /* desired position */
        int desired_pos;                /* desired position */

+       int desired_spd;                /* desired speed */
+
+       char commutate;                 /* zapina prepocet duty na jednotlive pwm */
+       char pos_reg_ena;               /* position regulation enable */
+       char spd_reg_ena;               /* speed rugulation enable */

 };
 
 #endif /*PMSM_STATE*/
 };
 
 #endif /*PMSM_STATE*/