clc
clear all
close all

p100=[333.7888 -97.6688];
p75=[273.3034 -96.7341];
p75_=[250.3416  -96.7341];
p50=[179.2602 -92.6042];
p50_=[166.8944  -92.6042];
p25=[89.9330 -90.7304];
p25_=[83.4472  -90.7304];
p10=[35.8928 -89.8646];
p10_=[33.3789  -89.8646];

speed=100;
add=strcat('.\6.0V\noLoad\noSpeedChange\Speed',num2str(speed),'\Position_Ascending');
S=load(add);

posreal=S.valNoSpeedChange.posreal;
time=S.valNoSpeedChange.time;
speeddes=S.valNoSpeedChange.speeddes;

pnew=[p100(1)*speed/100 p75(2)];

j=1;
for i=1:length(posreal)-1
    if(posreal(i)~=posreal(i+1))
        pos_fitted(j)=posreal(i);
        time_fitted(j)=time(i);        
        j=j+1;
    end
end
time_fitted=time_fitted-time_fitted(1);
p=polyfit(time_fitted,pos_fitted,1);

xlab_pos='Time (ms)';
ylab_pos='Position (º)';
leg_pos1='Real Position';
leg_pos2=strcat('p(x)=',num2str(p(1)),' * x + (',num2str(p(2)),')');
leg_pos3=strcat('p(x)=max.speed * ',num2str(speed),'% * x + (',num2str(p(2)),')');
title_pos=strcat('No Load / Speed = ',num2str(speeddes(1)),'%');
axis_pos=([0 ceil(time_fitted(end)*1e3) -100 100]);

%plotting
%figure
grid on
hold on
plot(time_fitted*1e3,pos_fitted,'bo',time_fitted*1e3,polyval(p,time_fitted),'r-',time_fitted*1e3,polyval(pnew,time_fitted),'g-')
title(title_pos);
xlabel(xlab_pos);
ylabel(ylab_pos);
legend(leg_pos1,leg_pos2,leg_pos3);
axis(axis_pos);
hold off