From 1a8d767b097d1536b264d8bae3e9d5c16617f835 Mon Sep 17 00:00:00 2001
From: Martin Prudek <prudemar@fel.cvut.cz>
Date: Sat, 16 May 2015 12:24:12 +0200
Subject: [PATCH] Controll loop now more complex.

---
 pmsm-control/test_sw/cmd_proc.c   | 13 +++++++++++--
 pmsm-control/test_sw/main_pmsm.c  | 19 +++++++++++++++----
 pmsm-control/test_sw/pmsm_state.h | 25 ++++++++++++++++---------
 3 files changed, 42 insertions(+), 15 deletions(-)

diff --git a/pmsm-control/test_sw/cmd_proc.c b/pmsm-control/test_sw/cmd_proc.c
index d5e3020..46ff070 100644
--- a/pmsm-control/test_sw/cmd_proc.c
+++ b/pmsm-control/test_sw/cmd_proc.c
@@ -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);
 }
diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index 56b926a..4ae245b 100644
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -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);*/
diff --git a/pmsm-control/test_sw/pmsm_state.h b/pmsm-control/test_sw/pmsm_state.h
index 7c631f1..b50b250 100644
--- a/pmsm-control/test_sw/pmsm_state.h
+++ b/pmsm-control/test_sw/pmsm_state.h
@@ -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*/
-- 
2.39.2