% calculate motor speed
% input file: 
% timedifferences [us] fieldCurrent[LSB] rotorCurrent[LSB] encoder [LSB]
% 2024-07-10 ch

data=csvread("data.txt");
dt=data(:,1);
t=cumsum(dt)/1e6;
iField= data(:,2);
iRotor= data(:,3);
encoder= data(:,4);

subplot(2,1,1);
plot(t,iField);
hold on
plot(t,iRotor);
#title("motor speed supply 3.4V on/off")
xlabel("time [s]");
legend("field","rotor")
ylabel("current [LSB]");
#ylim([0,2000])
grid;
hold off

subplot(2,1,2);
plot(t,encoder);
ylim([-1000,5000])
ylabel("encoder [LSB]");
xlabel("time [s]");
grid;

# calculate position from encoder signals
#enc2=movmedian(encoder,5);
enc2=encoder
pos=enc2;
a=enc2(1);
offset=0;
for n=2:length(encoder),
  b=enc2(n);
  d=b-a;
  a=b;
  if(d<-2000),
    offset=offset+4096;
  end
  if(d>2000),
    offset=offset-4096;
  end
  pos(n)=enc2(n)+offset;
end

figure
subplot(2,1,1)
turns=pos/4096;
plot(t,turns);
ylabel("position [turns]");
xlabel("time [s]");
grid on

subplot(2,1,2)
de=diff(turns);
de=[de' 0]; # append one value because diff is one value shorter
speed=de'./dt;
wndw = 100;  %# sliding window size
turns_per_us= filter(ones(wndw,1)/wndw, 1, speed);

RPM=turns_per_us*1e6*60;
plot(t,RPM);
grid on
ylabel("speed [RPM]");
xlabel("time [s]");
ylim([-2000,200]);