Hello.
I need to use cosine and sine functions to calculate 3 duty cycles for some PWM signals. I need about 12x cos/sin calculations for each 3x duty cycles, and I need to calculate these as often and quick as possible. I need to do the calculations with atleast a frequency of 25kHz, and still have a lot of cpu time free for other calculations and interrupts.
First I tried with the standard cos and sin functions. These take around 300µS, so not near enough the speed I need. I then tried to make my own look up table in the hope that this should be faster. It was faster, around 140µS, but still to slow for my application. Anyone have an idea on how to make it faster? Any special variables, or tricks I can use?
Here is my code:
volatile int a=0;
volatile int b=0;
volatile int c=0;
volatile int da=0;
volatile int db=0;
volatile int dc=0;
volatile int iq=0;
volatile int id=0;
volatile int ia,ib,ic;
volatile int pos=0;
int idref=0;
volatile int iqref = 0;
int kpdq=0;
int kidq=0;
int kpspeed=0;
int kispeed=0;
int Ts=1;
volatile int vd,vq;
volatile int prevvd,prevvq,preverrd,preverrq,va,vb,vc,ysat,prevysat,ysatd,ysatq,prevysatd,prevysatq=0;
int kaw=1;
void setup() {
Serial.begin(9600);
int t=analogRead(0);
ADC->ADC_WPMR=0x00; //disable write protection on MR
ADC->ADC_MR |= 0x200080; //set ADC in free running mode and settling time is 9 times ADC clock
ADC->ADC_CR=2; //enable ADC clock
ADC->ADC_CHER=0xFF; //enable A0->A7+A8
}
void loop() {
int t=micros();
pos=0;
for(int i=0;i<100;i++){
pos++;
while((ADC->ADC_ISR & 0xFF)!=0xFF);
a=ADC->ADC_CDR[4]; //read data on A3 pin
b=ADC->ADC_CDR[3]; //read data on A4 pin
c=ADC->ADC_CDR[2]; //read data on A5 pin
//scale measured voltage into actual current
ia=25*a-24;
ib=25*b-24;
ic=25*c-24;
//Park transformation, from adc to dq0 ref system
id=1*(cosinus(pos)*ia+cosinus(pos-120)*ib+cosinus(pos+120)*ic);
iq=1*(cosinus(pos)*ia+cosinus(pos-120)*ib+cosinus(pos+120)*ic);
//PI controller with anti windup
vd=kpdq*((idref-id)-preverrd)+kidq*(Ts/2)*((idref-id)+preverrd)+prevvd;
if(vd>28){
ysatd=28;
}
else if(vd<-28){
ysatd=-28;
}
else{
ysatd=vd;
}
vd=kpdq*((idref-id)-preverrd)+kidq*(Ts/2)*((idref-id)+preverrd)+prevvd+((ysatd-vd)+prevysatd)*kaw;
prevysatd=(ysatd-vd)*kaw;
prevvd=vd;
preverrd=idref-id;
vq=kpdq*((iqref-iq)-preverrq)+kidq*(Ts/2)*((iqref-iq)+preverrq)+prevvq;
if(vq>28){
ysatq=28;
}
else if(vq<-28){
ysatq=-28;
}
else{
ysatq=vq;
}
vq=kpdq*((iqref-iq)-preverrq)+kidq*(Ts/2)*((iqref-iq)+preverrq)+prevvq+((ysatq-vq)+prevysatq)*kaw;
prevysatq=(ysatq-vq)*kaw;
prevvq=vq;
preverrq=iqref-iq;
//dq0 to abc (inverse park transformation)
va=cosinus(pos)*vd-cosinus(pos)*vq;
vb=cosinus(pos-120)*vd-cosinus(pos-120)*vq;
vc=cosinus(pos+120)*vd-cosinus(pos+120)*vq;
//convert voltage references to duty cycles
da=((va*1)+1)*127;
db=((vb*1)+1)*127;
dc=((vc*1)+1)*127;
}
t=micros()-t;
Serial.print("100 conversions in ");Serial.print(t);Serial.println(" micros");
}
//lookup table for cos
int cosinus(int posit){
int i=0;
int table[]={10000,9998,9994,9986,9976,9962,9945,9925,9903,9877,9848,9816,9781,9744,9703,9659,9613,9563,9511,9455,9397,9336,9272,9205,9135,9063,8988,8910,8829,8746,8660,8572,8480,8387,8290,8192,8090,7986,7880,7771,7660,7547,7431,7314,7194,7071,6947,6820,6691,6561,6428,6293,6157,6018,5878,5736,5592,5446,5299,5150,5000,4848,4695,4540,4384,4226,4067,3907,3746,3584,3420,3256,3090,2924,2756,2588,2419,2250,2079,1908,1736,1564,1392,1219,1045,872,698,523,349,175,0};
if(posit>360)
{
posit=posit-360;
}
if(posit<-360)
{
posit=posit+360;
}
if(posit<=90&posit>=0)
{
i=posit;
return table[i];
}
if(posit>90&posit<=180)
{
i=(((posit-90)/90)-1)*(-90);
return table[i]*(-1);
}
if(posit>180&posit<=270)
{
i=(posit-180);
return table[i]*(-1);
}
if(posit>270&posit<=360)
{
posit=posit-180;
i=(((posit-90)/90)-1)*(-1);
return table[i];
}
else
{
return -10;
}
}