clear all;

mround=@(x,n)bitshift(x+bitshift(int32((bitand(x,bitshift(1,n))~=0)|(bitand(x,bitshift(1,n)-1)~=bitshift(1,n-1))),n-1),-n);
k2=0.8;
f=randn(1,40);
f=resample(f,200,1);
%f=-ones(1,1000);
f=int32(3000+1500*f/max(abs(f)));%speed variation

mem1=0;
mem2=0;
k2=int32(k2*32768);% 1.15 (Q15)
k3=int32(1500/512*256);%k3- for accelerator - generator shaft imitation
k4=int32(32768/1500*256);%k4- speed to normalized speed coeff in 8.8 (Q8)
out=[];
repl=0;
for i=1:length(f)
    error=bitshift((f(i)-3000-repl)*k4,-8);% normalized speed error 2*(f-f0)/(fmax-fmin) in 1.15 (Q15) 16.0 * 8.8 -> 24.8 >> 8 -> 16.0 (1.15)
    out=[out double(error)/32768*1500];
    mem1=mem1+bitshift(error-mem1,-1);%lowpass filter
    mem2=mem2+bitshift(mem1*k2,-7); %integrator (1.15 * 1.15)-> 2.30 >> 7 -> 1.23
    control=mround(mem2,14)+512; %word for dac in 1.23 >> 14 -> 1.9 + 512 -> 0.10
    repl=bitshift((control-512)*k3,-8);%dac - accelerator - shaft speed correction imitation
end;

plot(f)
hold on
plot(out+3000,'r')
grid on