#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
*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
* ktera budou privedena na svorky motoru.
* Tedy na A(yel)-pwm1, B(red)-pwm2, C(blk)-pwm3
*/
-void compPhases(int32_t * pwm1, int32_t * pwm2, int32_t *pwm3, int32_t ua , int32_t ub){
- int32_t u1,u2,u3;
+void transDelta(int32_t * u1, int32_t * u2, int32_t *u3, int32_t ub , int32_t uc){
int32_t t;
/*vypocte napeti tak, aby odpovidaly rozdily*/
- u1=ua+ub;
- u2=ua;
- u3=0;
+ *u1=uc;
+ *u2=uc+ub;
+ *u3=0;
/*najde zaporne napeti*/
- t=min(u1,u2,u3);
-
- /*dorovna zaporna napeti napeti na nulu*/
- u1-=t;
- u2-=t;
- u3-=t;
-
- *pwm1=u1;
- *pwm2=u2;
- *pwm3=u3;
+ t=min(*u1,*u2,*u3);
+ /*dorovna zaporna napeti na nulu*/
+ *u1-=t;
+ *u2-=t;
+ *u3-=t;
}
void inv_trans_comm(int duty){
uint32_t pos;
uint32_t pos;
int32_t sin, cos;
int32_t alpha, beta;
- int32_t pwma,pwmb,pwmc;
- int32_t pwm1,pwm2,pwm3;
+ int32_t ua,ub,uc;
+ int32_t ia,ib,ic;
+ int32_t u1,u2,u3;
pos=rps.index_dist;
- /*melo by byt urceno co nejpresneji, aby faze 'a' splyvala s osou 'alpha'*/
- pos+=460;
- /*use it as cyclic 32-bit logic*/
+
+ pos+=960; /*zarovnani faze 'a' s osou 'alpha'*/
+
+ /*pro výpočet sin a cos je pouzita 32-bit cyklicka logika*/
pos*=4294967;
pxmc_sincos_fixed_inline(&sin, &cos, pos, 16);
+
dq2alphabeta(&alpha, &beta,0,duty, sin, cos);
alpha>>=16;
beta>>=16;
- alphabeta2pwm3(&pwma,&pwmb, &pwmc,alpha,beta);
- compPhases(&pwm1,&pwm2, &pwm3,pwma,pwmb);
+ alphabeta2pwm3(&ia,&ib, &ic,alpha,beta);
+
+ ua=ia;
+ ub=ib;
+ uc=ic;
+
+ transDelta(&u1,&u2, &u3,ub,uc);
- rps.pwm1=(uint16_t)pwm1;
- rps.pwm2=(uint16_t)pwm2;
- rps.pwm3=(uint16_t)pwm3;
+ rps.pwm1=(uint16_t)u1;
+ rps.pwm2=(uint16_t)u2;
+ rps.pwm3=(uint16_t)u3;
}
void prepare_tx(uint8_t * tx){