clear;
clc;
%MOTOR PARAMETER
Ra = 2.3; % Tahanan Jangkar [Ohm]
La = 0.0085; % Induktansi jangkar [Henry]
J = 0.00035; % Inersia [Nms2/rad]
Kb = 0.7452; % Konstanta generator [Vs/rad]
Km = 0.74; % Konstanta motor [Nm/A]
B = 0.002; % Konstanta beban (frction) [Nms/rad]
Vdc = 310; % Tegangan inverter [Volt]
Max_RPM = 5000; % RPM motor
Max_freq = Max_RPM/60; % Frekueansi maksimal [Hz]
P = 4; % Jumlah kutub

%CONTROLLER PARAMETER
Kp = 1.1;
Ki = 0.2;

% Tref = 2000*Ts/(P/2);
% Dref = 50; 
Vref = 5; % Tegangan referensi (maksimal) potensio (V)
Set_Point = 3; % Tegangan input potensio [V]
K = (Max_freq/Vref)*(P/2); % Gain Frekuensi pada frequency generator [Hz/V]
% RPM_ref = (Set_Point*K*60)/(P/2) % Referensi kecepatan
ErrorMax = 700;
UsahaMax = 2;
p = 1/ErrorMax;
q = 1/UsahaMax;

%MOTOR SIMULATION
SamplingTime = 1e-6;
Ts = 10*SamplingTime;
SimulationTime = 10;
sim('PLL_Control_BLDCM.slx')
N = length(Time);
RPMRef = (Refference*K*60)/(P/2);
T = zeros(1,N);
RPM = zeros(1,N);
RPM_Total = zeros(1,N);
RPM_rata2 = zeros(1,N);
Error = zeros(1,N);
Error_kuadrat = zeros(1,N);
RMSE = zeros(1,N);
Usaha_kuadrat = zeros(1,N);
RMSU = zeros(1,N);
T(1) = Time(1);
% RPM(1)= 1.9302e+03;
RPM(1) = 0;
RMSE(1)=0;
Error(1) = 0;
Error_kuadrat(1) = 0;
RMSU(1) = 0;
Usaha(1) = 0;
Usaha_kuadrat(1) = 0;
RPM_Total(1) = 0;
for I = 2:N
    T(I) = Time(I);
        if (Pulse(I)>0.5)&&(Pulse(I-1)<=0.5)
            PERIODE = T(I)-T(I-1);
            RPM(I)= (60/PERIODE)/(P/2);
        else
            RPM(I) = RPM(I-1);
            T(I) = T(I-1);
        end
    Error(I) = RPM(I) - RPMRef(I);
    Error_kuadrat(I) = Error_kuadrat(I-1) + Error(I)^2;
    RMSE(I) = sqrt(abs((Error_kuadrat(I)/I)));
    Usaha_kuadrat(I) = Usaha_kuadrat(I-1) + Usaha(I)^2;
    RMSU(I) = sqrt(abs((Usaha_kuadrat(I)/I)));
    RPM_Total(I) = (RPM_Total(I-1)+RPM(I));
    RPM_rata2(I) = RPM_Total(I)/I;
end
RPMakhir = RPM_rata2(I)
RMSEakhir = RMSE(I)
RMSUakhir = RMSU(I)
j = sqrt((((p*RMSEakhir).^2))+(((q*RMSUakhir).^2)))

subplot(411), plot(Time,Load), grid on, ylabel('Beban (Nms/rad)')
subplot(412), plot(Time,RPM), grid on, ylabel('Kecepatan (RPM)'); hold on; plot(Time,RPMRef), grid on; hold off;
subplot(413), plot(Time,Error), grid on, ylabel('Error (RPM)')
subplot(414), plot(Time,Usaha), grid on, ylabel('Usaha (Volt)'), xlabel('Waktu (detik)')

% plot(Time,RPM), grid on, ylabel ('Kecepatan (RPM)');