I think slow convergence is due to floating point variables. Tried using Fixed point arithmetic. But the output seems to be oscillating. The code is given below. All variables are Q15 format. Should I operate on Q32?
D=.84;
w=50;
kf=_Q15ftoi((w*w)*.0004);
kp=_Q15ftoi((2*D)/w);
ki=_Q15ftoi(1.0);
pi=0;
i=0;
input=_Q15ftoi(0.5);
while(1)
{
// error goes through first stage integrator. The output of integrator goes through PI compensator
error=_Q15sub(input,Output);
i=_Q15add(i,Q15_MUL(error,kf)); //Integrator stage;add,sub are microchip in-buit functions
pi=_Q15add(pi,Q15_MUL(i,ki)); //PI stage
Output=_Q15add(pi,Q15_MUL(i,kp));
printf("%f\n",(double)_itofQ15(Output)); // output oscillating- should settle at 0.5
}
_Q15 Q15_MUL(_Q15 x, _Q15 y)
{
long temp,test1,test2;
test1=x;
test2=y;
if ((test1==0x8000)&& (test2==0x8000))
{
temp=0x7fff;
}
else
{
temp=(test1*test2)>>15;
}
return(temp);
}