From: Martin Prudek <prudemar@fel.cvut.cz>
Date: Sun, 4 Oct 2015 14:11:17 +0000 (+0200)
Subject: SW repair for 'IRC malfunction'
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/commitdiff_plain/6841e483f0b370a6ea2b41c958945d3db9285c90

SW repair for 'IRC malfunction'
---

diff --git a/pmsm-control/test_sw/Makefile b/pmsm-control/test_sw/Makefile
index 73aeb4a..7fa17f2 100644
--- a/pmsm-control/test_sw/Makefile
+++ b/pmsm-control/test_sw/Makefile
@@ -1,5 +1,8 @@
 #author Martin Prudek
 #sestavuje klienta a server pro ovladani DC motoru pomoci raspberry Pi
+clean:
+	rm -f ./*.o
+	rm -f ./dc_pos_hmod/*.o
 all:
 	echo "scp server client_rt client_wirpi"
 
diff --git a/pmsm-control/test_sw/cmd_proc.c b/pmsm-control/test_sw/cmd_proc.c
index f163602..2c5fdba 100644
--- a/pmsm-control/test_sw/cmd_proc.c
+++ b/pmsm-control/test_sw/cmd_proc.c
@@ -10,7 +10,7 @@
  * referencnich informaci z jineho zarizeni po protokolu
  * UDP. Jde zatim jen o testovaci rezim
  */
-#define UDP_CAPABLE_X
+#define UDP_CAPABLE
 
 #ifdef UDP_CAPABLE
 #include "udp_cli.h"
diff --git a/pmsm-control/test_sw/comp.c b/pmsm-control/test_sw/comp.c
index 764726a..9427481 100644
--- a/pmsm-control/test_sw/comp.c
+++ b/pmsm-control/test_sw/comp.c
@@ -8,6 +8,18 @@
 #include "pmsm_state.h"
 #include "rp_spi.h"
 
+#include <stdio.h>
+#include <stdlib.h>
+
+int transitions[6][6]={
+		     {  0,  0,881,  0,730,  0},
+		     {  0,  0, 50,  0,  0,230},
+		     {881, 50,  0,  0,  0,  0},
+		     {  0,  0,  0,  0,550,380},
+		     {730,  0,  0,550,  0,  0},
+		     {  0,230,  0,380,  0,  0}
+		    };
+
 /**
  * \brief
  * Substact initial position.
@@ -32,6 +44,41 @@ uint16_t mult_cap(int32_t s,int d){
 	return res;
 }
 
+/**
+ * \brief
+ * Attempt to resolve fail-state in case of irc cable malfunction.
+ *
+ * Pri prechodu mezi haly je pocitan rozdil aktualni a predpokladane
+ * vzdalenosti od indexu. Pokud prekroci MAX_DIFF, je prepnuto na
+ * komutaci jen pomoci halu.
+ */
+#define MAX_DIFF 90
+void repairPositionErr(struct rpi_state* this){
+	int new;
+	int old;
+	int trans;
+	/* doslo k prechodu mezi haly */
+	if (this->h1_old != this->spi_dat->hal1 ||
+	    this->h2_old != this->spi_dat->hal2 ||
+	    this->h3_old != this->spi_dat->hal3)
+	{
+		new=this->spi_dat->hal1*4+this->spi_dat->hal2*2+this->spi_dat->hal3;
+		old=this->h1_old*4+this->h2_old*2+this->h3_old;
+		trans = transitions[new-1][old-1]; /*predpokladana pozice */
+
+		/*pozice jsou v rozsahu 0-999*/
+		if (abs(this->index_dist-trans)>MAX_DIFF &&
+	            abs(this->index_dist-trans-1000)>MAX_DIFF){
+			setIndexLost(this);
+		}
+
+		/*nakonec preulozme stare hodnoty*/
+		this->h1_old = this->spi_dat->hal1;
+		this->h2_old = this->spi_dat->hal2;
+		this->h3_old = this->spi_dat->hal3;
+	}
+
+}
 
 /**
  * \brief
@@ -89,6 +136,9 @@ void comIndDist(struct rpi_state* this){
 	}
 
 	this->index_dist = dist;
+
+	repairPositionErr(this);
+
 	return;
 
 	index_lost:
diff --git a/pmsm-control/test_sw/controllers.h b/pmsm-control/test_sw/controllers.h
index 5b24148..e914a1d 100644
--- a/pmsm-control/test_sw/controllers.h
+++ b/pmsm-control/test_sw/controllers.h
@@ -18,7 +18,9 @@
 /*	Hodnoty upraveny podle skutecnych vysledku	*/
 /*	P=0.8	I=0.01					*/
 
-#define PID_P_S		0.16 		/*2.3 kmita*/ /*1.4 vhodne jen pro P regulator*/
+/* PID_P_S		0.16 */
+
+#define PID_P_S		0.08 		/*2.3 kmita*/ /*1.4 vhodne jen pro P regulator*/
 #define PID_I_S		0.0134
 
 /**
diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index 6f3c670..2f4731f 100644
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -149,6 +149,7 @@ void * control_loop(void* param){
 			}else{ /*index je v poradku*/
 				comIndDist(&rps);		/*vypocet vzdalenosti indexu*/
 			}
+			last_index=data.index_position;		/*preulozime stary index */
 
 			/* pocitame sirku plneni podle potreb rizeni*/
 			rps.main_controller(&rps);
diff --git a/pmsm-control/test_sw/pmsm_state.h b/pmsm-control/test_sw/pmsm_state.h
index 6fc77f4..47401d8 100644
--- a/pmsm-control/test_sw/pmsm_state.h
+++ b/pmsm-control/test_sw/pmsm_state.h
@@ -51,6 +51,8 @@ struct rpi_state{
 	void (*main_controller)(struct rpi_state*);	/* primarni regulator */
 
 	char error;			/* detekce chyboveho stavu */
+
+	int8_t h1_old,h2_old,h3_old;	/* stare hodnoty hallu */
 };
 
 /**