Simple IRC position commutator added. And Very simple PID reg. added.

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index 7e29253afa3def8717b91e810172d6023a60fb8e..4a03c3adff0246e6f76967275807c3f92fea714a 100644 (file)
--- 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;
 }
+