robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions
servo/App/FOC/PWM_algorithms.c
on4ip84 526b689693 Project changes 
1/ Added Spi that used to read data from AS5045b position sensor
2/ Added Base FOC code to project
3/ Added Standart DSP lib with optimazes sincos cals Need for FOC
2023-08-23 09:02:09 +03:00

221 lines
4.9 KiB
C
Raw Blame History

/*
* PWM_algorithms.c
*
* Created on: 07 <20><><EFBFBD>. 2017 <20>.
* Author: Andrey
*/
#include "PlatformMath.h"
#include "PWM_algorithms.h"
#include "math.h"
#define GENERIC_MAX(x,y) ((x)>(y) ?(x) : (y))
#define GENERIC_MIN(x,y) ((x)<(y) ?(x) : (y))
typedef enum
{
SV_PWM = 0x1,
DPWMMAX,
DPWMMIN,
DPWM30,
DPWM60
}PWM_MODE;
PWM_struct MY_PWM = {0};
FAST_RAM unsigned int PWM_proc(PWM_struct * myPWM);
unsigned int PWM_proc(PWM_struct * myPWM)
{
float VrefA=0,VrefB=0,VrefC=0;
//double Va=0,Vb=0,Vc=0;
float X=0,Y=0,Z=0;
unsigned int A=0,B=0,C=0;
float t1=0,t2=0;
float Ta=0,Tb=0,Tc=0;
float Ualfa_norm=0,Ubeta_norm=0;//modulation index
//Vdc_pu=(sqrt(myPWM->Ualfa*myPWM->Ualfa+my_PWM.Ubeta*my_PWM.Ubeta))*sqrt(3.)/myPWM->Vm_max;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//Vdc_T=myPWM->Trpd_pwm;///myPWM->Vdc;// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> Udc
Ualfa_norm=myPWM->Ualfa;//*1.73205/myPWM->Vdc;
if(Ualfa_norm>1) Ualfa_norm=1.f;
if(Ualfa_norm<-1) Ualfa_norm=-1.f;
Ubeta_norm=myPWM->Ubeta;//*1.73205/myPWM->Vdc;
if(Ubeta_norm>1) Ubeta_norm=1.f;
if(Ubeta_norm<-1) Ubeta_norm=-1.f;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
VrefA=Ubeta_norm;
VrefB=-0.5f*Ubeta_norm+0.8665025f*Ualfa_norm;
VrefC=-0.5f*Ubeta_norm-0.8665025f*Ualfa_norm;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
if(VrefA>0) A=1;
else A=0;
if(VrefB>0) B=1;
else B=0;
if(VrefC>0) C=1;
else C=0;
myPWM->sector=A+2*B+4*C;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// X=Ubeta_norm;
// Y=0.5f*(1.732f*Ualfa_norm+Ubeta_norm);
// Z=0.5f*(-1.732f*Ualfa_norm+Ubeta_norm);
X=VrefA;
Y=-VrefC;
Z=-VrefB;
//
//X=2*0.57735*Ubeta_norm;
//Y=(0.57735*Ualfa_norm+Ubeta_norm);
//Z=(-0.57735*Ualfa_norm+Ubeta_norm);
// <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
switch(myPWM->sector)
{
case 1:
{
t1=Z;
t2=Y;
Tb =(1.f-t1-t2)*0.5f; // tbon = (1-t1-t2)/2
Ta = Tb+t1; // taon = tbon+t1
Tc = Ta+t2; // tcon = taon+t2
break;
}
case 2:
{
t1=Y;
t2=-X;
Ta =0.5f*(1.f-t1-t2); // taon = (1-t1-t2)/2
Tc = Ta+t1; // tcon = taon+t1
Tb = Tc+t2; // tbon = tcon+t2
break;
}
case 3:
{
t1=-Z;
t2=X;
Ta = 0.5f*(1.f-t1-t2); // taon = (1-t1-t2)/2
Tb = Ta+t1; // tbon = taon+t1
Tc = Tb+t2; // tcon = tbon+t2
break;
}
case 4:
{
t1=-X;
t2=Z;
Tc = 0.5f*(1.f-t1-t2); // tcon = (1-t1-t2)/2
Tb = Tc+t1; // tbon = tcon+t1
Ta = Tb+t2; // taon = tbon+t2
break;
}
case 5:
{
t1=X;
t2=-Y;
Tb = 0.5f*(1.f-t1-t2); // tbon = (1-t1-t2)/2
Tc = Tb+t1; // tcon = tbon+t1
Ta = Tc+t2; // taon = tcon+t2
break;
}
case 6:
{
t1=-Y;
t2=-Z;
Tc = 0.5*(1.f-t1-t2); // tcon = (1-t1-t2)/2
Ta = Tc+t1; // taon = tcon+t1
Tb = Ta+t2; // tbon = taon+t2
break;
}
case 0:
{
t1=0.5f;
t2=0.5f;
Ta=0.5f;
Tb=0.5f;
Tc=0.5f;
break;
}
default:
{
break;
}
}//end of switch
static float Tas = 0,Tbs=0,Tcs=0;
static float Toffset = 0;
static float Tmin = 0, Tmax = 0;
Tmax = GENERIC_MAX(Ta, Tb);
Tmax = GENERIC_MAX(Tmax, Tc);
Tmin = GENERIC_MIN(Ta, Tb);
Tmin = GENERIC_MIN(Tmin, Tc);
PWM_MODE pwm_mod = SV_PWM;
switch (pwm_mod)
{
case SV_PWM:
{
Toffset = (1.f-(Tmax-Tmin)) / 2.f - Tmin;
break;
}
case DPWMMAX:
{
Toffset = 1.f - Tmax;
break;
}
case DPWMMIN:
{
Toffset = -Tmin;
break;
}
case DPWM30:
{
if ((2.f*(Tmin + Tmax)-2.f) >= 0)
Toffset = -(Tmin*2.f-1.f);
else
{
Toffset = 1.0f - (Tmax*2.f-1.f);
}
break;
}
case DPWM60:
{
break;
}
default:
break;
}
////D<>+
//Toffset = 1 - Tmax;
////DC-
//Toffset = -Tmin;
////Generic
//static float sigma = toPI/8.f;
//float mu = 1 - 0.5f * (1 + my_sign(cos(BLDC_drive.Teta_rot_el+sigma),100000));
//mu = 0.5f;
//Toffset = (1 - mu) + (mu - 1) * Tmax - mu * Tmin;
Tas = Ta + Toffset;
Tbs = Tb + Toffset;
Tcs = Tc + Toffset;
//
if (Tas > 1.0f) Tas = 1.0f;
if (Tbs > 1.0f) Tbs = 1.0f;
if (Tcs > 1.0f) Tcs = 1.0f;
if (Tas < 0.0f) Tas = 0.0f;
if (Tbs < 0.0f) Tbs = 0.0f;
if (Tcs < 0.0f) Tcs = 0.0f;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>
myPWM->Ta=Tas*myPWM->Trpd_pwm;
myPWM->Tb=Tbs*myPWM->Trpd_pwm;
myPWM->Tc=Tcs*myPWM->Trpd_pwm;
return 1;
}
Reference in a new issue View git blame Copy permalink