Commutation function now implemented with use of polymorphism.

[fpga/rpi-motor-control.git] / pmsm-control / test_sw / main_pmsm.c

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index 56b654417374c35d540dbc693c0f18d2bdb54443..ac3563d59b578396991ba6f0088415bb28668929 100644 (file)
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -26,9 +26,7 @@
 #include "controllers.h"
 #include "commutators.h"
 #include "comp.h"
-
-
-
+#include "logs.h"
 
 #define PRIOR_KERN     50
 #define PRIOR_HIGH     49
@@ -62,9 +60,11 @@ struct rpi_state rps={
        .log_col_count=0,       /* pocet radku zaznamu */
        .log_col=0,
        .doLogs=0,
-       .alpha_offset=960
+       .alpha_offset=960,
+       .main_commutator=zero_commutator        /*komutace vypnuta*/
 };
 
+
 /**
  * \brief Initilizes GPCLK.
  */
@@ -75,73 +75,6 @@ int clk_init()
        gpioSetMode(4, FSEL_ALT0);
        return 0;
 }
-/*
- * \brief Terminates GPCLK.
- */
-
-inline void clk_disable(){
-       termClock(0);
-}
-
-
-/*
- * \brief
- * Free logs
- */
-void freeLogs(){
-       int r;
-       if (rps.log_col_count){
-               for (r=0;r<LOG_ROWS;r++){
-                       free(rps.logs[r]);
-               }
-       }
-       rps.log_col_count=0;
-       rps.doLogs=0;
-}
-
-/*
- * \brief
- * Makes log.
- */
-void makeLog(){
-       int r;
-       if (rps.log_col==MAX_LOGS-1){
-               rps.doLogs=0;
-               return;
-       }
-       rps.logs[0][rps.log_col]=(int)rps.tf_count;
-       rps.logs[1][rps.log_col]=(int)rps.spi_dat->pozice;
-
-       rps.logs[2][rps.log_col]=(int)rps.pwm1;
-       rps.logs[3][rps.log_col]=(int)rps.pwm2;
-       rps.logs[4][rps.log_col]=(int)rps.pwm3;
-       rps.logs[5][rps.log_col]=rps.duty;
-
-       rps.logs[6][rps.log_col]=rps.desired_spd;
-       rps.logs[7][rps.log_col]=rps.speed;
-
-       rps.logs[8][rps.log_col]=(int)(rps.spi_dat->ch1/rps.spi_dat->adc_m_count);
-       rps.logs[9][rps.log_col]=(int)(rps.spi_dat->ch2/rps.spi_dat->adc_m_count);
-       rps.logs[10][rps.log_col]=(int)(rps.spi_dat->ch0/rps.spi_dat->adc_m_count);
-
-       rps.log_col++;
-     /*
-        if (rps.log_col==rps.log_col_count-1){
-               rps.log_col_count*=2;
-               rps.log_col_count%=MAX_LOGS;
-               for (r=0;r<LOG_ROWS;r++){
-                       rps.logs[r]=realloc(rps.logs[r],rps.log_col_count*sizeof(int));
-                       if (rps.logs[r]==NULL){
-                               rps.doLogs=0;
-                               rps.error=1;
-                       }
-               }
-        }
-     */
-}
-
-
-
 
 /**
  * \brief Signal handler pro Ctrl+C
@@ -157,8 +90,8 @@ void appl_stop(){
        spi_read(&rps);
 
        spi_disable();
-       clk_disable();
-       freeLogs();
+       termClock(0);
+       freeLogs(&rps);
        /*muzeme zavrit semafor*/
        sem_destroy(&rps.thd_par_sem);
         printf("\nprogram bezpecne ukoncen\n");
@@ -180,7 +113,7 @@ void * pos_monitor(void* param){
  * Feedback loop.
  * TODO: replace bunch of 'IFs' with Object-like pattern
  */
-void * read_data(void* param){
+void * control_loop(void* param){
        int i;
        struct rpi_in pocatek;
        struct rpi_state poc={
@@ -203,10 +136,9 @@ void * read_data(void* param){
 
                        /*old positions*/
                        rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
-                       spi_read(&rps);         /*exchange data*/
-                       /*subtract initiate postion */
+                       spi_read(&rps);                         /*exchange data*/
                        rps.tf_count++;
-                       substractOffset(&data,poc.spi_dat);
+                       substractOffset(&data,poc.spi_dat);     /*subtract initiate postion */
                        compSpeed(&rps);                        /*spocita rychlost*/
 
                        if (!rps.index_ok){
@@ -214,7 +146,7 @@ void * read_data(void* param){
                                        last_index=data.index_position;
                                        first=0;
                                }else if (last_index!=data.index_position){
-                                       rps.index_ok=1;
+                                       setIndexOK(&rps);
                                        comIndDist(&rps);       /*vypocet vzdalenosti indexu*/
                                }
                        }else{ /*index je v poradku*/
@@ -228,19 +160,12 @@ void * read_data(void* param){
                                spd_pid(&rps);
                        }
 
-                       /* sirka plneni prepoctena na jednotlive pwm */
-                       if (rps.index_ok && rps.commutate){
-                               /*simple_ind_dist_commutator(rps.duty);*/
-                               /*sin_commutator(rps.duty);*/
-                               inv_trans_comm(&rps);
-                               inv_trans_comm_2(&rps);
-                       }else if(!rps.index_ok && rps.commutate){
-                               simple_hall_commutator(&rps);
-                       }
+                       /* sirku plneni prepocteme na jednotlive pwm */
+                       rps.main_commutator(&rps);
 
                        /*zalogujeme hodnoty*/
                        if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
-                               makeLog();
+                               makeLog(&rps);
                        }
 
                        sem_post(&rps.thd_par_sem);             /*--post semaphore---*/
@@ -256,11 +181,9 @@ void * read_data(void* param){
                }
 }
 
-
 /**
  * \brief Main function.
  */
-
 int main(){
        pthread_t base_thread_id;
        clk_init();             /* inicializace gpio hodin */
@@ -273,7 +196,7 @@ int main(){
        base_thread_id=pthread_self();
 
        /*main control loop*/
-       create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
+       create_rt_task(&base_thread_id,PRIOR_HIGH,control_loop,NULL);
 
        /*monitor of current state*/
        create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);