close all

NORM=1/53;

data1=load('traject=up_intMaxTorque#T=50#M=m2#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
data2=load('traject=up_intMaxTorque#T=50#M=m42#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');

posTh=data1(:,5);
time1=data1(:,1);
pos1 =data1(:,2);
curr1=data1(:,4);

time2=data2(:,1);
pos2 =data2(:,2);
curr2=data2(:,4);

figure('name','-45º => +45º (Max. torque at 0º)');
axes('fontsize',14);
plot(time1,posTh,'k',time1,pos1,'b',time2,pos2,'r','linewidth',2);
grid on
xlabel('time (s)');
ylabel('Position (degrees)');
legend('Requested trajectory','463g load','1129g load');

figure('name','-45º => +45º (Max. torque at 0º)');
axes('fontsize',14);
plot(pos1,curr1*NORM,'xb',pos2,curr2*NORM,'^r','linewidth',2);
%title('-45º => +45º (Max. torque at 0º)');
xlabel('Position (degrees)');
ylabel('Current (normalized)');
legend('463g load','1129g load');

grid on

figure('name','-45º => +45º (Max. torque at 0º)');
axes('fontsize',14);
plot(time1,curr1*NORM,'b',time2,curr2*NORM,'r','linewidth',2);
axis([0 2 0 1]);
%title('-45º => +45º (Max. torque at 0º)');
xlabel('time (s)');
ylabel('Current (normalized)');
legend('463g load','1129g load');
grid on

    
data=load('traject=up_max2minTorque#T=50#M=m42#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
    
posTh=data(:,5);
velTh=data(:,6);
time=data(:,1);
pos =data(:,2);
curr=data(:,4);

figure('name','-90º => 0º (Max => Min torque)');
axes('fontsize',14);
plot(time,posTh,'k',time,pos,'b','linewidth',2);
grid on
xlabel('time (s)');
ylabel('Position (degrees)');
legend('Requested trajectory','1129g load');

figure('name','-90º => 0º (Max => Min torque)');
axes('fontsize',14);
plot(pos,curr*NORM,'sb','linewidth',2);
%title('0º => +90º (Max => Min torque)');
xlabel('Position (degrees)');
ylabel('Current (normalized)');
grid on

figure('name','-90º => 0º (Max => Min torque)');
axes('fontsize',14);
plot(time,abs(velTh)/max(abs(velTh)),'g',time,curr*NORM,'r','linewidth',2);
axis([0 2 0 1]);
xlabel('time (s)');
ylabel('Current (normalized)');
legend('Mean velocity','Current drained');
grid on


data=load('traject=up_min2maxTorque#T=50#M=m42#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
    
posTh=data(:,5);
velTh=data(:,6);
time=data(:,1);
pos =data(:,2);
curr=data(:,4);

figure('name','+90º => 0º (Min => Max torque)');
axes('fontsize',14);
plot(time,posTh,'k',time,pos,'b','linewidth',2);
grid on
xlabel('time (s)');
ylabel('Position (degrees)');
legend('Requested trajectory','1129g load');

figure('name','+90º => 0º (Min => Max torque)');
axes('fontsize',14);
plot(pos,curr*NORM,'sb','linewidth',2);
%title('0º => +90º (Max => Min torque)');
xlabel('Position (degrees)');
ylabel('Current (normalized)');
grid on

figure('name','+90º => 0º (Min => Max torque)');
axes('fontsize',14);
plot(time,abs(velTh)/max(abs(velTh)),'g',time,curr*NORM,'r','linewidth',2);
axis([0 2 0 1]);
xlabel('time (s)');
ylabel('Current (normalized)');
legend('Mean velocity','Current drained');
grid on
