#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.
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
}
this->index_dist = dist;
+
+ repairPositionErr(this);
+
return;
index_lost:
}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);