X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/48407cad35d152771ac1a86652dae3eede6741a2..32aaacc65b9f894bea0897421277f71375a7f825:/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 56b6544..f0fc982 100644
--- 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,12 @@ 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 */
+	.main_controller=zero_controller	/* rizeni vypnuto */
 };
 
+
 /**
  * \brief Initilizes GPCLK.
  */
@@ -75,73 +76,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 +91,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 +114,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 +137,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 +147,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*/
@@ -222,25 +155,14 @@ void * read_data(void* param){
 			}
 
 			/* pocitame sirku plneni podle potreb rizeni*/
-			if (rps.pos_reg_ena){           /*pozicni rizeni*/
-				pos_pid(&rps);
-			}else if(rps.spd_reg_ena){	/*rizeni na rychlost*/
-				spd_pid(&rps);
-			}
+			rps.main_controller(&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 +178,9 @@ void * read_data(void* param){
 		}
 }
 
-
 /**
  * \brief Main function.
  */
-
 int main(){
 	pthread_t base_thread_id;
 	clk_init(); 		/* inicializace gpio hodin */
@@ -273,7 +193,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);