X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/a836c7be2ad5acf66edf1afd1538ffd4fa6f0f4c..48407cad35d152771ac1a86652dae3eede6741a2:/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 7e29253..56b6544 100644
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -15,34 +15,55 @@
 #include <sched.h>	/*sheduler*/
 #include <unistd.h>	/*usleep*/
 #include <pthread.h>	/*threads*/
+#include <time.h>	/*nanosleep*/
+#include <string.h>
 
 #include "rpin.h"	/*gpclk*/
 #include "rp_spi.h"	/*spi*/
 #include "misc.h" 	/*structure for priorities*/
+#include "pmsm_state.h"
+#include "cmd_proc.h"
+#include "controllers.h"
+#include "commutators.h"
+#include "comp.h"
+
 
 
-#define PRUM_PROUD 	2061
-#define PRUM_SOUC	6183
 
 #define PRIOR_KERN	50
 #define PRIOR_HIGH	49
 #define PRIOR_LOW	20
 
 #define THREAD_SHARED 	0
-#define INIT_VALUE	0 	/*init value for semaphor*/
+#define INIT_VALUE	1 	/*init value for semaphor*/
 
-struct sigaction sighnd; /*struktura pro signal handler*/
-struct rpi_in data;
-struct rpi_state{
-	uint8_t test;
-	uint16_t pwm1, pwm2, pwm3;
-	char commutate;
-	int simple_hall_duty;
-	uint16_t index_dist;		/* distance to index position */
-	unsigned char index_ok;
-	uint32_t tf_count;		/*number of transfer*/
-}rps;
 
+#define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */
+
+
+struct rpi_in data;
+struct rpi_state rps={
+	//.MAX_DUTY=170,
+	.spi_dat=&data,
+	.test=0,
+	.pwm1=0,.pwm2=0, .pwm3=0,
+	.pwm1=0, .t_pwm2=0, .t_pwm3=0,
+	.commutate=0,
+	.duty=0,			/* duty cycle of pwm */
+	.index_dist=0,		/* distance to index position */
+	.index_ok=0,
+	.tf_count=0,		/*number of transfer*/
+	.desired_pos=0,		/* desired position */
+	.pos_reg_ena=0,
+	.desired_spd=0,
+	.spd_reg_ena=0,
+	.old_pos={0},
+	.spd_err_sum=0,
+	.log_col_count=0,	/* pocet radku zaznamu */
+	.log_col=0,
+	.doLogs=0,
+	.alpha_offset=960
+};
 
 /**
  * \brief Initilizes GPCLK.
@@ -62,329 +83,203 @@ inline void clk_disable(){
 	termClock(0);
 }
 
-/**
- * \brief Signal handler pro Ctrl+C
- */
-void sighnd_fnc(){
-	spi_disable();
-	clk_disable();
-	/*muzeme zavrit semafor*/
-	sem_destroy(&thd_par_sem);
-        printf("\nprogram bezpecne ukoncen\n");
-        exit(0);
-}
 
-void substractOffset(struct rpi_in* data, struct rpi_in* offset){
-	data->pozice_raw=data->pozice;
-	data->pozice-=offset->pozice;
-	return;
-}
-/*
- * pocita procentualni odchylku od prumerneho proudu
- */
-float diff_p(float value){
-	return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
-}
 /*
- * pocita procentualni odchylku od prumerneho souctu proudu
+ * \brief
+ * Free logs
  */
-float diff_s(float value){
-	return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
+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;
 }
+
 /*
- * tiskne potrebna data
+ * \brief
+ * Makes log.
  */
-void printData(){
-	struct rpi_in data_p;
-	struct rpi_state s;	/*state*/
-	float cur0, cur1, cur2;
-	int i;
-	/* copy the data */
-	sem_wait(&thd_par_sem);
-	data_p = data;
-	s=rps;
-	sem_post(&thd_par_sem);
-
-	if (data_p.adc_m_count){
-		cur0=data_p.ch0/data_p.adc_m_count;
-		cur1=data_p.ch1/data_p.adc_m_count;
-		cur2=data_p.ch2/data_p.adc_m_count;
+void makeLog(){
+	int r;
+	if (rps.log_col==MAX_LOGS-1){
+		rps.doLogs=0;
+		return;
 	}
-	for (i = 0; i < 16; i++) {
-			if (!(i % 6))
-				puts("");
-			printf("%.2X ", data_p.debug_rx[i]);
-	}
-	puts("");
-	printf("\npozice=%d\n",(int32_t)data_p.pozice);
-	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));
-	printf("index position=%u\n",data_p.index_position);
-	printf("distance to index=%u\n",s.index_dist);
-	printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
-	printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
-	printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
-	printf("PWM1=%u(L.s.)\n",s.pwm1);
-	printf("PWM2=%u(L.s.)\n",s.pwm2);
-	printf("PWM3=%u(L.s.)\n",s.pwm3);
-	printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
-	printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
-	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);
-	if (s.index_ok) printf("index ok\n");
-	if (s.commutate) printf("commutation in progress\n");
+	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;
+        		}
+        	}
+        }
+     */
 }
-void prepare_tx(uint8_t * tx){
-
-	/*Data format:
-	 * tx[4] - bity 95 downto 88 - bits that are sent first
-	 * tx[5] - bity 87 downto 80
-	 * tx[6] - bity 79 downto 72
-	 * tx[7] - bity 71 downto 64
-	 * tx[8] - bity 63 downto 56
-	 * tx[9] - bity 55 downto 48
-	 * tx[10] - bity 47 downto 40
-	 * tx[11] - bity 39 downto 32
-	 * tx[12] - bity 31 downto 24
-	 * tx[13] - bity 23 downto 16
-	 * tx[14] - bity 15 downto 8
-	 * tx[15] - bity 7 downto 0
-	 *
-	 * bit 95 - ADC reset
-	 * bit 94 - enable PWM1
-	 * bit 93 - enable PWM2
-	 * bit 92 - enable PWM3
-	 * bit 91 - shutdown1
-	 * bit 90 - shutdown2
-	 * bit 89 - shutdown3
-	 * 	.
-	 * 	.
-	 * 	.
-	 * bits 66 .. 56 - match PWM1
-	 * bits 55 .. 45 - match PWM2
-	 * bit 11,12 - Unused
-	 * bits 42 .. 32  - match PWM3
-	 */
-
-
-	uint16_t tmp;
-
-	/* keep the cap*/
-	if (rps.pwm1>2047) rps.pwm1=2047;
-	if (rps.pwm2>2047) rps.pwm2=2047;
-	if (rps.pwm3>2047) rps.pwm3=2047;
-
-	tx[0]=rps.test; /*bit 94 - enable PWM1*/
-
-	/*pwm1*/
-	tx[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&rps.pwm1)[1]); /*MSB*/
-	tx[8]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/
-
-	/*pwm2*/
-	tmp=rps.pwm2;
-	tmp<<=5;
-	tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/
-	tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/
-
-	/*pwm3*/
-	tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&rps.pwm3)[1]); /*MSB*/
-	tx[11]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/
 
 
+
+
+/**
+ * \brief Signal handler pro Ctrl+C
+ */
+void appl_stop(){
+	uint8_t tx[16];
+	sem_wait(&rps.thd_par_sem);
+
+	memset(tx,0,16*sizeof(int));
+	rps.pwm1=0;
+	rps.pwm2=0;
+	rps.pwm3=0;
+	spi_read(&rps);
+
+	spi_disable();
+	clk_disable();
+	freeLogs();
+	/*muzeme zavrit semafor*/
+	sem_destroy(&rps.thd_par_sem);
+        printf("\nprogram bezpecne ukoncen\n");
 }
+
 /**
  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
  */
 void * pos_monitor(void* param){
-	set_priority(param);		/*set priority*/
 	while(1){
-		printData();
+		printData(&rps);
 		usleep(1000000); 	/*1 Hz*/
 	}
 	return (void*)0;
 }
+
 /*
  * \brief
- * Test function to be placed in controll loop.
- * Switches PWM's at point where they produce same force
+ * Feedback loop.
+ * TODO: replace bunch of 'IFs' with Object-like pattern
  */
-inline void simple_hall_commutator(struct rpi_in data, int duty){
-	if (duty>=0){ /* clockwise - so that position increase */
-		/* pwm3 */
-		if (data.hal2 && !data.hal3){
-			rps.pwm1=0;
-			rps.pwm2=0;
-			rps.pwm3=duty;
-			/* pwm1 */
-		}else if (data.hal1 && !data.hal2){
-			rps.pwm1=duty;
-			rps.pwm2=0;
-			rps.pwm3=0;
-			/* pwm2 */
-		}else if (!data.hal1 && data.hal3){
-			rps.pwm1=0;
-			rps.pwm2=duty;
-			rps.pwm3=0;
-		}
-	}else{	/*counter-clockwise - position decrease */
-		/* pwm3 */
-		if (!data.hal2 && data.hal3){
-			rps.pwm1=0;
-			rps.pwm2=0;
-			rps.pwm3=-duty;
-			/* pwm1 */
-		}else if (!data.hal1 && data.hal2){
-			rps.pwm1=-duty;
-			rps.pwm2=0;
-			rps.pwm3=0;
-			/* pwm2 */
-		}else if (data.hal1 && !data.hal3){
-			rps.pwm1=0;
-			rps.pwm2=-duty;
-			rps.pwm3=0;
-		}
-	}
-}
-/**
- * Funkce pravidelne vycita data z motoru
- */
-inline void comIndDist(){
-	rps.index_dist=0x0FFF & (data.pozice_raw - data.index_position);
-	/*
-	 * if distance is bigger than 2047, the distance underflown
-	 * -> if 12th bit is set, substract 2096
-	 */
-	rps.index_dist-=((rps.index_dist & 0x0800)>>11)*2096;
-}
 void * read_data(void* param){
 	int i;
 	struct rpi_in pocatek;
-	uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ;
+	struct rpi_state poc={
+		.spi_dat=&pocatek,
+		.test=0,
+		.pwm1=0, .pwm1=0, .pwm3=0
+	};
+	struct timespec t;
+	int interval = 1000000; /* 1ms ~ 1kHz*/
 	char first=1;
 	uint16_t last_index;				/*we have index up-to date*/
-	set_priority(param);				/*set priority*/
-	pocatek = spi_read(tx);
+	spi_read(&poc);		/*pocatecni informace*/
+	clock_gettime(CLOCK_MONOTONIC ,&t);
+	/* start after one second */
+	t.tv_sec++;
 		while(1){
-			prepare_tx(tx);			/*save the data to send*/
-			sem_wait(&thd_par_sem);		/*---take semaphore---*/
-			data = spi_read(tx);		/*exchange data*/
+			/* wait until next shot */
+			clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
+			sem_wait(&rps.thd_par_sem);		/*---take semaphore---*/
+
+			/*old positions*/
+			rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
+			spi_read(&rps);		/*exchange data*/
 			/*subtract initiate postion */
 			rps.tf_count++;
-			substractOffset(&data,&pocatek);
-			comIndDist();
+			substractOffset(&data,poc.spi_dat);
+			compSpeed(&rps);			/*spocita rychlost*/
+
 			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(&rps);	/*vypocet vzdalenosti indexu*/
 				}
+			}else{ /*index je v poradku*/
+				comIndDist(&rps);		/*vypocet vzdalenosti indexu*/
 			}
+
+			/* 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);
+			}
+
+			/* sirka plneni prepoctena na jednotlive pwm */
 			if (rps.index_ok && rps.commutate){
-				simple_hall_commutator(data,rps.simple_hall_duty);
+				/*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(data,rps.simple_hall_duty);
+				simple_hall_commutator(&rps);
 			}
-			sem_post(&thd_par_sem);		/*--post semaphore---*/
-			usleep(1000);				/*1kHz*/
+
+			/*zalogujeme hodnoty*/
+			if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
+				makeLog();
+			}
+
+			sem_post(&rps.thd_par_sem);		/*--post semaphore---*/
+
+			/* calculate next shot */
+			t.tv_nsec += interval;
+
+			while (t.tv_nsec >= NSEC_PER_SEC) {
+				t.tv_nsec -= NSEC_PER_SEC;
+				t.tv_sec++;
+			}
+
 		}
 }
 
+
 /**
  * \brief Main function.
  */
 
 int main(){
-	unsigned int tmp;
-
-	/*nastaveni priorit vlaken*/
-	struct thread_param tsp;
-	tsp.sch_policy = SCHED_FIFO;
-
-	/*nastaveni signalu pro vypnuti pomoci Ctrl+C*/
-	sighnd.sa_handler=&sighnd_fnc;
-	sigaction(SIGINT, &sighnd, NULL );
-
+	pthread_t base_thread_id;
 	clk_init(); 		/* inicializace gpio hodin */
 	spi_init(); 		/* iniicializace spi*/
 
 	/*semafor pro detekci zpracovani parametru vlaken*/
-	sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE);
-
-	/*vlakna*/
-	pthread_t tid;			/*identifikator vlakna*/
-	pthread_attr_t attr; 		/*atributy vlakna*/
-	pthread_attr_init(&attr);	/*inicializuj implicitni atributy*/
-
-
-
-	/*ziskavani dat z motoru*//*vysoka priorita*/
-	tsp.sch_prior = PRIOR_HIGH;
-	pthread_create(&tid, &attr, read_data, (void*)&tsp);
-
-	/*vypisovani lokalni pozice*//*nizka priorita*/
-	tsp.sch_prior = PRIOR_LOW;
-	sem_wait(&thd_par_sem);
-	pthread_create(&tid, &attr, pos_monitor, (void*)&tsp);
-
-
-
-	/*
-	 * Note:
-	 * pri pouziti scanf("%u",&simple_hall_duty); dochazelo
-	 * k preukladani hodnot na promenne test. Dost divne.
-	 */
-	while (1){
-		scanf("%u",&tmp);
-		printf("volba=%u\n",tmp);
-		switch (tmp){
-		case 1:
-			scanf("%u",&tmp);
-			sem_wait(&thd_par_sem);
-			rps.pwm1=tmp&0xFFF;
-			sem_post(&thd_par_sem);
-			break;
-		case 2:
-			scanf("%u",&tmp);
-			sem_wait(&thd_par_sem);
-			rps.pwm2=tmp&0xFFF;
-			sem_post(&thd_par_sem);
-			break;
-		case 3:
-			scanf("%u",&tmp);
-			sem_wait(&thd_par_sem);
-			rps.pwm3=tmp&0xFFF;
-			sem_post(&thd_par_sem);
-			break;
-		case 4:
-			scanf("%u",&tmp);
-			sem_wait(&thd_par_sem);
-			rps.test=tmp&0xFF;
-			sem_post(&thd_par_sem);
-			break;
-		case 5:
-			sem_wait(&thd_par_sem);
-			rps.commutate=!rps.commutate;
-			/* switch off pwms at the end of commutation */
-			rps.pwm1&=rps.commutate*0xFFFF;
-			rps.pwm2&=rps.commutate*0xFFFF;
-			rps.pwm3&=rps.commutate*0xFFFF;
-			sem_post(&thd_par_sem);
-			break;
-		case 6:
-			scanf("%d",&tmp);
-			sem_wait(&thd_par_sem);
-			rps.simple_hall_duty=tmp;
-			sem_post(&thd_par_sem);
-			break;
-
-		default:
-			break;
-		}
+	sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
+	setup_environment();
+
+	base_thread_id=pthread_self();
+
+	/*main control loop*/
+	create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
+
+	/*monitor of current state*/
+	create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
+
+	/*wait for commands*/
+	poll_cmd(&rps);
 
-	}
 	return 0;
 }