
close all

idx = find((t<5750) & (t>5720));

t = t(idx);
TargTension = TargTension(idx);
MeasTension = MeasTension(idx);
TorqueCmd = TorqueCmd(idx);
TorqueAct = TorqueAct(idx);
RPM_Cmd = RPM_Cmd(idx);
RPM_Act= RPM_Act(idx);
SD_TorqueAct = SD_TorqueAct(idx);
SD_RPM_Act = SD_RPM_Act(idx);
SD_Pos = SD_Pos(idx);
PID_FF = PID_FF(idx);
PID_P = PID_P(idx);
PID_I = PID_I(idx);
PID_D = PID_D(idx);
PID_Tf = PID_Tf(idx);
cmd_min = cmd_min(idx);
cmd_max = cmd_max(idx);


error = TargTension-MeasTension;
derror_dt = [diff(error)./diff(t) ; 0];

Cmd_P = PID_P.*error;
Cmd_D = PID_D.*derror_dt;

num_plots = 5;
figure;
ah1 = subplot(num_plots,1,1);
plot(t,TargTension);
hold all;
plot(t,MeasTension);
legend('Target Tension','Measured Tension');
ylabel('kg');
grid on;

ah2 = subplot(num_plots,1,2);
plot(t,TorqueCmd);
hold all;
plot(t,TorqueAct);
plot(t,Cmd_P);
plot(t,Cmd_D);
plot(t,PID_FF);
plot(t,(Cmd_P+Cmd_D+PID_FF));
%plot(t,cmd_min,'k--');
%plot(t,cmd_max,'k--');
legend('Cmd Torque','Actual Torque','Cmd_P','Cmd_D','PID_FF','CMD_P+CMD_D+PIDFF');
grid on;

ah3 = subplot(num_plots,1,3);
plot(t,RPM_Cmd);
hold all;
plot(t,RPM_Act);
legend('Target RPM','Actual RPM');
ylabel('kg');
grid on;

ah4 = subplot(num_plots,1,4);
plot(t,PID_P);
hold all;
plot(t,PID_I);
plot(t,PID_D);
plot(t,PID_Tf);
legend('P','I','D','Tf');
grid on;

ah5 = subplot(num_plots,1,5);
plot(t,PID_FF);
legend('PID_FF)');
grid on;

linkaxes([ah1 ah2 ah3 ah4 ah5],'x');
