spi: rp_spi.o
gcc -o spi rp_spi.c
#pro rpi
-pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o
- gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o -lpthread -lrt
+pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o comp.o
+ gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o comp.o -lpthread -lrt
--- /dev/null
+/**
+ * \brief
+ * Computatations of position, index, speed.
+ *
+ */
+
+#include "comp.h"
+#include "pmsm_state.h"
+#include "rp_spi.h"
+
+/**
+ * \brief
+ * Substact initial position.
+ */
+void substractOffset(struct rpi_in* data, struct rpi_in* offset){
+ data->pozice=data->pozice_raw-offset->pozice_raw;
+ return;
+}
+
+/*
+ * \brief
+ * Multiplication of 11 bit
+ * Zaporne vysledky prvede na nulu.
+ */
+uint16_t mult_cap(int32_t s,int d){
+ int j;
+ int res=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
+ }
+ return res;
+}
+
+
+/**
+ * \brief
+ * Computation of distance to index.
+ *
+ * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
+ * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
+ * to je 3999 bodu
+ * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
+ * signalu indexu
+ */
+void comIndDist(struct rpi_state* this){
+ uint16_t pos = 0x0FFF & this->spi_dat->pozice_raw;
+ uint16_t dist;
+ uint16_t index = this->spi_dat->index_position;
+
+ if (index<1999){ /*index e<0,1998> */
+ if (pos<index){ /*pozice e<0,index-1> */
+ /*proti smeru h.r. od indexu*/
+ dist=pos+2000-index;
+ }else if (pos<=index+1999){ /*pozice e<index,index+1999> */
+ /*po smeru h.r. od indexu*/
+ dist=pos-index;
+ }else if (pos<index+2096){ /*pozice e<index+2000,index+2095> */
+ goto index_lost;
+ }else{ /*pozice e<index+2096,4095> */
+ /*proti smeru h.r. od indexu - podtecena pozice*/
+ dist=pos-index-2096;
+ }
+ }else if (index<=2096){ /*index e<1999,2096>*/
+ if (pos<index-1999){ /*pozice e<0,index-2000> */
+ goto index_lost;
+ }else if (pos<index){ /*pozice e<index-1999,index-1> */
+ /*proti smeru h.r. od indexu*/
+ dist=pos+2000-index;
+ }else if (pos<=index+1999){ /*pozice e<index,index+1999> */
+ /*po smeru h.r. od indexu*/
+ dist=pos-index;
+ }else { /*pozice e<index+2000,4095> */
+ goto index_lost;
+ }
+ }else{ /*index e<2097,4095> */
+ if (pos<=index-2097){ /*pozice e<0,index-2097> */
+ /*po smeru h.r. od indexu - pretecena pozice*/
+ dist=4096+pos-index;
+ }else if (pos<index-1999){ /*pozice e<index-2096,index-2000> */
+ goto index_lost;
+ }else if (pos<index){ /*pozice e<index-1999,index-1> */
+ /*proti smeru h.r. od indexu*/
+ dist=pos+2000-index;
+ }else{ /*pozice e<index,4095> */
+ /*po smeru h.r. od indexu*/
+ dist=pos-index;
+ }
+ }
+
+ this->index_dist = dist;
+ return;
+
+ index_lost:
+ this->index_ok=0;
+ return;
+}
+
+
+/*
+ * \brief
+ * Computate speed.
+ */
+void compSpeed(struct rpi_state* this){
+ signed long int spd;
+ spd=this->spi_dat->pozice-this->old_pos[this->tf_count%OLD_POS_NUM];
+ this->speed=(int32_t)spd;
+}
--- /dev/null
+/**
+ * \brief
+ * Computatations of position, index, speed.
+ *
+ */
+#ifndef COMP
+#define COMP
+
+struct rpi_in;
+struct rpi_state;
+
+/**
+ * \brief
+ * Substact initial position.
+ */
+void substractOffset(struct rpi_in* data, struct rpi_in* offset);
+
+/**
+ * \brief
+ * Computation of distance to index.
+ *
+ * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
+ * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
+ * to je 3999 bodu
+ * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
+ * signalu indexu
+ */
+void comIndDist(struct rpi_state*);
+
+/*
+ * \brief
+ * Computates speed.
+ */
+void compSpeed(struct rpi_state*);
+
+#endif /*COMP*/
+
#include "cmd_proc.h"
#include "controllers.h"
#include "commutators.h"
+#include "comp.h"
printf("\nprogram bezpecne ukoncen\n");
}
-void substractOffset(struct rpi_in* data, struct rpi_in* offset){
- data->pozice=data->pozice_raw-offset->pozice_raw;
- return;
-}
-
-
-
/**
* Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
*/
}
return (void*)0;
}
-/*
- * \brief
- * Multiplication of 11 bit
- * Zaporne vysledky prvede na nulu.
- */
-inline uint16_t mult_cap(int32_t s,int d){
- int j;
- int res=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
- }
- return res;
-}
-
-
-/**
- * \brief
- * Computation of distance to index.
- *
- * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
- * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
- * to je 3999 bodu
- * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
- * signalu indexu
- */
-void comIndDist(){
- uint16_t pos = 0x0FFF & data.pozice_raw;
- uint16_t dist;
- uint16_t index = data.index_position;
-
- if (index<1999){ /*index e<0,1998> */
- if (pos<index){ /*pozice e<0,index-1> */
- /*proti smeru h.r. od indexu*/
- dist=pos+2000-index;
- }else if (pos<=index+1999){ /*pozice e<index,index+1999> */
- /*po smeru h.r. od indexu*/
- dist=pos-index;
- }else if (pos<index+2096){ /*pozice e<index+2000,index+2095> */
- goto index_lost;
- }else{ /*pozice e<index+2096,4095> */
- /*proti smeru h.r. od indexu - podtecena pozice*/
- dist=pos-index-2096;
- }
- }else if (index<=2096){ /*index e<1999,2096>*/
- if (pos<index-1999){ /*pozice e<0,index-2000> */
- goto index_lost;
- }else if (pos<index){ /*pozice e<index-1999,index-1> */
- /*proti smeru h.r. od indexu*/
- dist=pos+2000-index;
- }else if (pos<=index+1999){ /*pozice e<index,index+1999> */
- /*po smeru h.r. od indexu*/
- dist=pos-index;
- }else { /*pozice e<index+2000,4095> */
- goto index_lost;
- }
- }else{ /*index e<2097,4095> */
- if (pos<=index-2097){ /*pozice e<0,index-2097> */
- /*po smeru h.r. od indexu - pretecena pozice*/
- dist=4096+pos-index;
- }else if (pos<index-1999){ /*pozice e<index-2096,index-2000> */
- goto index_lost;
- }else if (pos<index){ /*pozice e<index-1999,index-1> */
- /*proti smeru h.r. od indexu*/
- dist=pos+2000-index;
- }else{ /*pozice e<index,4095> */
- /*po smeru h.r. od indexu*/
- dist=pos-index;
- }
- }
-
- rps.index_dist = dist;
- return;
-
- index_lost:
- rps.index_ok=0;
- return;
-}
-
-
-/*
- * \brief
- * Computate speed.
- */
-void compSpeed(){
- signed long int spd;
- spd=rps.spi_dat->pozice-rps.old_pos[rps.tf_count%OLD_POS_NUM];
- rps.speed=(int32_t)spd;
-}
-
-
/*
* \brief
/*subtract initiate postion */
rps.tf_count++;
substractOffset(&data,poc.spi_dat);
- compSpeed(); /*spocita rychlost*/
+ compSpeed(&rps); /*spocita rychlost*/
if (!rps.index_ok){
if (first){
first=0;
}else if (last_index!=data.index_position){
rps.index_ok=1;
- comIndDist(); /*vypocet vzdalenosti indexu*/
+ comIndDist(&rps); /*vypocet vzdalenosti indexu*/
}
}else{ /*index je v poradku*/
- comIndDist(); /*vypocet vzdalenosti indexu*/
+ comIndDist(&rps); /*vypocet vzdalenosti indexu*/
}
/* pocitame sirku plneni podle potreb rizeni*/