X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/2fb1fa82b48a8f915fce254bfe887b891f476b68..1a8d767b097d1536b264d8bae3e9d5c16617f835:/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 4991443..4ae245b 100644
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -24,8 +24,8 @@
 #include "pmsm_state.h"
 #include "cmd_proc.h"
 
-#define MAX_DUTY	128
-#define PID_P		0.1
+#define MAX_DUTY	170
+#define PID_P		0.3
 
 #define PRIOR_KERN	50
 #define PRIOR_HIGH	49
@@ -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
 };
 
 /**
@@ -113,10 +116,10 @@ void dq2alphabeta(int32_t *alpha, int32_t *beta, int d, int q, int32_t sin, int3
 	*beta=-sin*d+cos*q;
 	return;
 }
-void alphabeta2pwm3(int32_t * pwma, int32_t * pwmb, int32_t *pwmc,int32_t alpha, int32_t beta){
-	*pwma=alpha;
-	*pwmb=-alpha/2+beta*887/1024;
-	*pwmc=-alpha/2-beta*887/1024;
+void alphabeta2pwm3(int32_t * ia, int32_t * ib, int32_t *ic,int32_t alpha, int32_t beta){
+	*ia=alpha;
+	*ib=-alpha/2+beta*887/1024;
+	*ic=-alpha/2-beta*887/1024;
 }
 /*
  * \brief
@@ -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);*/