clear all
close all
clc

S=load('noLoadSpeedChange70Position');

posreal=S.valSpeedChange.posreal;
pw=S.valSpeedChange.pw;
v=S.valSpeedChange.v;
time=S.valSpeedChange.time;
speeddes=S.valSpeedChange.speeddes;

maxspeed=60/0.14;
N=length(posreal);

posimag=zeros(1,N);
speedreal=zeros(1,N);
delta=zeros(1,N);
diftime=zeros(1,N);
difpos=zeros(1,N);

j=1;
n=1;
posimag(1)=posreal(1);
speedreal(1)=0;
delta(1)=0;
for i=1:N-1
    if(i==1)
        dir=posreal(i+1)-posreal(i);
    else
        dir=posreal(i)-posreal(i-1);
    end
    delta(i+1)=time(i+1)-time(i);
    m_speed=speeddes(i)*maxspeed/100;
    if (dir > 0)
        posimag(i+1)=posreal(i)+m_speed*delta(i+1);
    else if(dir < 0)
        posimag(i+1)=posreal(i)-m_speed*delta(i+1);
        else
            posimag(i+1)=posreal(i);
        end
    end
    speedreal(i+1)=(posreal(i+1)-posreal(i))/delta(i+1);
end
posimag(2)=posimag(1);

xlab_pos='Time (ms)';
ylab_pos='Position (º)';
leg_pos1='Real Position';
leg_pos2='Desired Position';
title_pos='Real Position vs. Desired Position';
axis_pos=([0 ceil(time(end)*1e3) -100 100]);

xlab_speed='Time (ms)';
ylab_speed='Speed (%)';
leg_speed1='Real Speed';
leg_speed2='Desired Speed';
title_speed='Real Speed vs. Desired Speed';
axis_speed=([0 ceil(time(end)*1e3) 0 100]);

xlab_pw='Time (ms)';
ylab_pw='Pulse Width (%)';
leg_pw='Real Speed';
title_pw='Pulse Width';
axis_pw=([0 ceil(time(end)*1e3) 0 100]);

xlab_v='Time (ms)';
ylab_v='Voltage (V)';
leg_v='Voltage';
title_v='Voltage';
axis_v=([0 ceil(time(end)*1e3) 0 8]);

%plotting
figure
subplot(211)
grid on
hold on
plot(time*1e3,posreal,'bo',time*1e3,posimag,'r*-')
title(title_pos);
xlabel(xlab_pos);
ylabel(ylab_pos);
legend(leg_pos1,leg_pos2);
axis(axis_pos);
hold off

subplot(212)
grid on
hold on

plot(time*1e3,abs(speedreal)/m_speed*100,'bo-',time*1e3,speeddes,'r*-')
title(title_speed)
xlabel(xlab_speed);
ylabel(ylab_speed);
legend(leg_speed1,leg_speed2);
%axis(axis_speed);
hold off

figure
subplot(211)
grid on
hold on
plot(time*1e3,v,'bo-')
title(title_v)
xlabel(xlab_v);
ylabel(ylab_v);
legend(leg_v);
axis(axis_v);
hold off

subplot(212)
grid on
hold on
plot(time*1e3,pw,'bo-')
title(title_pw)
xlabel(xlab_pw);
ylabel(ylab_pw);
legend(leg_pw);
axis(axis_pw);
hold off