Controll loop now more complex.
authorMartin Prudek <prudemar@fel.cvut.cz>
Sat, 16 May 2015 10:24:12 +0000 (12:24 +0200)
committerMartin Prudek <prudemar@fel.cvut.cz>
Sat, 16 May 2015 10:24:12 +0000 (12:24 +0200)
pmsm-control/test_sw/cmd_proc.c
pmsm-control/test_sw/main_pmsm.c
pmsm-control/test_sw/pmsm_state.h

index d5e3020..46ff070 100644 (file)
@@ -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("duty:[hodnota] - Nastavi pevnou sirku plneni.");
+
        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*/
-       state->commutate=1;
        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;
+       state->pos_reg_ena=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){
-
+       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);
+       state->pos_reg_ena=1;
        state->desired_pos=pos;
        sem_post(&state->thd_par_sem);
 }
index 56b926a..4ae245b 100644 (file)
@@ -49,7 +49,10 @@ struct rpi_state rps={
        .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
  */
-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){
@@ -513,6 +516,7 @@ inline void pid(){
 /*
  * \brief
  * Feedback loop.
+ * TODO: replace bunch of 'IFs' with Object-like pattern
  */
 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);
-                       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;
+                                       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);*/
index 7c631f1..b50b250 100644 (file)
@@ -7,17 +7,24 @@
 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 */
-       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_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*/