% ACI3-4 system (Sensorless Direct Field Oriented Control of Induction Motor)
% 
% Required following function m-files:
% 1. "aci.m" - induction model
% 2. "aci_se.m" - Open-loop speed estimator
% 3. "aci_fe.m" - Flux estimator
% 4. "pid_reg3.m" - PID controller
% 5. "park.m" - Park transformation
% 6. "inv_park.m" - Inverse park tranformation
% 7. "ramp_gen.m" - Ramp generator

clear all
close all

T = 5e-04;               % Sampling time for simulation (sec)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Incremental build %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
phase1_inc_build = 0;   % Estimated flux/speed tests (ACI_FE/ACI_SE tests)
phase2_inc_build = 1;   % Closed-loop speed loop test
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Incremental build %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%% Machine parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 1-hp WEG Induction Motor
Rs = 1.723;                  % stator resistance (ohm) 
Rr = 2.011;                  % rotor resistance referred to stator (ohm)
Ls = (7.387+159.232)*1e-03;  % stator inductance (H)
Lr = (9.732+159.232)*1e-03;  % rotor inductance referred to stator (H)
Lm = 159.232*1e-03;          % magnetizing inductance (H)
P = 4;                       % number of poles
J = 0.001;                   % moment of inertia of the rotor mass (kg.m^2)
B = 0.0001;                  % damping coefficient (N.m.sec/rad) - always ignored
Tl = 0;                      % load torque (N.m)
np = P/2;                    % number of pole pairs
%%%%%%%%%%%%%%%%%%%%%%%%%% Machine parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Base quantities %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fb = 60;                   % Base supplied frequency (Hz)
Wb = 2*pi*fb;               % Base electrically angular velocity (rad/sec)
Ib = 5;                     % Base phase current (amp)
Vb = 320/sqrt(3);           % Base phase voltage (volt)
%Lb = 220*sqrt(2/3)/(2*pi*60);     % Base flux linkage (volt.sec/rad)
Lb = Lm*Ib;                        % Base flux linkage (volt.sec/rad)
Tb = (3*Vb*Ib/2)*(np/(2*pi*60));   % Base torque (N.m)
SPb = 120*fb/P;                    % Base synchronous speed (rpm)  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Base quantities %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%% Initial conditions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Induction motor
X = [0;0;0;0];       % X = [psi_rq; psi_rd; i_sq; i_sd] 
Wr = 0;              % Rotor electrically angular velocity (rad/sec)
Iq = 0;
Id = 0;
% Parameter vector of induction motor
p_im = [T; Rs; Rr; Ls; Lr; Lm; np; J; B; Tl/Tb; Wb; Ib; Vb; Lb; Tb];  

% Flux estimator
h_fe = [0; 0; 0; 0; 0; 0; 0; 0];       % [theta_psi_r; psi_rD_i; psi_sd_v; 
                                       % psi_sq_v; ui_sd; ui_sq; e_sd; e_sq]
theta_psi_r = 0;                       % Rotor flux angle (pu)
p_fe = [T; Rs; Rr; Ls; Lr; Lm; 0.055; 1e-06; Ib; Vb];  % [T; Rs; Rr; Ls; Lr; Lm; Kp; Ti; Ib; Vb]

% Open-loop speed estimator
h_se = [0;0];         % [theta_r_se; wr_psi_r]
p_se = [T; Rr; Lr; Lm; 200; 0.97; 0.03; fb];       
% [T; Rr; Lr; Lm; fc; diff_max_limit; diff_min_limit; fb]
wr_hat_se = 0;

% PID - IQ control
h_iq = [0;0;0];            % h_pid  = [up_reg3; ui_reg3; ud_reg3]  
p_iq = [T; 2*Ib/Vb; 0.001; 0.0001; 0.1; 0.71; -0.71]; % [T; Kp; Ti; Td; Kc; Umax; Umin];  


% PID - ID control
h_id = [0;0;0];            % h_pid  = [up_reg3; ui_reg3; ud_reg3]  
p_id = [T; 2*Ib/Vb; 0.001; 0.0001; 0.1; 0.71; -0.71]; % [T; Kp; Ti; Td; Kc; Umax; Umin]; 


% PID - Speed control
h_sp = [0;0;0];            % h_pid  = [up_reg3; ui_reg3; ud_reg3]  
p_sp = [T; 0.00334*SPb/Ib; 0.01; 0.0001; 0.9; 1; -1]; % [T; Kp; Ti; Td; Kc; Umax; Umin];  

%%%%%%%%%%%%%%%%%%%%%%%%%% Initial conditions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%% Simulation inputs  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Tt = 0.5120;                 % Total time for simulation (sec) 
%Tt = 29*T;                 % Total time for simulation (sec) 
t = 0:T:Tt;               % Time for simulation (sec) 

speed_ref = 900/SPb;     % Reference speed (pu)
%speed_ref = 1;
Id_ref = 2/Ib;            % Reference syn. rotating d-axis current (pu)   
%Id_ref = 1;
Iq_ref = 0/Ib;            % Reference syn. rotating q-axis current (pu)  
%%%%%%%%%%%%%%%%%%%%%%%%%% Simulation inputs  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%% Simulation section %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:length(t),

   if phase1_inc_build==1         
      % Phase 1 incremental build
     [rmp_out] = ramp_gen(2*pi/(2*pi),0,speed_ref*fb,t(i));
     [Uq_ref,Y,h_iq] = pid_reg3(Iq_ref,Iq,h_iq,p_iq);
     [Ud_ref,Y,h_id] = pid_reg3(Id_ref,Id,h_id,p_id);
     
     [uq_ref,ud_ref] = inv_park(Uq_ref,Ud_ref,rmp_out);
   
     [Te,Wr,X] = aci(Wr,X,[uq_ref; ud_ref],p_im);
   
     [Iq,Id] = park(X(3),X(4),rmp_out);
   
     [psi_rq,psi_rd,theta_psi_r,h_fe] = aci_fe(X(3),X(4),uq_ref,ud_ref,h_fe,p_fe);
   
     [wr_hat_se,h_se] = aci_se(X(3),X(4),psi_rq,psi_rd,theta_psi_r,h_se,p_se);  

     rmp(i) = rmp_out;
     
    elseif phase2_inc_build==1         
      % Phase 2 incremental build

     %[Iq_ref,Y,h_sp] = pid_reg3(speed_ref,Wr,h_sp,p_sp);  % sensored
     [Iq_ref,Y,h_sp] = pid_reg3(speed_ref,wr_hat_se,h_sp,p_sp); % sensorless    

     [Uq_ref,Y,h_iq] = pid_reg3(Iq_ref,Iq,h_iq,p_iq);
     [Ud_ref,Y,h_id] = pid_reg3(Id_ref,Id,h_id,p_id);
  
     [uq_ref,ud_ref] = inv_park(Uq_ref,Ud_ref,theta_psi_r);
   
     [Te,Wr,X] = aci(Wr,X,[uq_ref; ud_ref],p_im);
   
     [psi_rq,psi_rd,theta_psi_r,h_fe] = aci_fe(X(3),X(4),uq_ref,ud_ref,h_fe,p_fe);
   
     [wr_hat_se,h_se] = aci_se(X(3),X(4),psi_rq,psi_rd,theta_psi_r,h_se,p_se);  
    
     [Iq,Id] = park(X(3),X(4),theta_psi_r);
      
 end
   
   i_qe(i) = Iq;
   i_de(i) = Id;
   v_qe(i) = Uq_ref;
   v_de(i) = Ud_ref;

   ibeta(i) = X(3);
   ialfa(i) = X(4);
   vbeta(i) = uq_ref;
   valfa(i) = ud_ref;
     
   psi_r_beta(i) = X(1);
   psi_r_alfa(i) = X(2);

   psi_r_beta_hat(i) = psi_rq;
   psi_r_alfa_hat(i) = psi_rd;

   theta_r(i) = theta_psi_r;
   
   torque(i) = Te;
   wr(i) = Wr; 
   wr_hat(i) = wr_hat_se;
   
   t(i)
     
 end   
%%%%%%%%%%%%%%%%%%%%%%%%%% Simulation section %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(1)
subplot(221)
plot(t,valfa,t,vbeta);
legend('v_{s\alpha}','v_{s\beta}',0);
title('Induction Motor Responses');
ylabel('stator voltages (pu)');
xlabel('time (sec)');
grid
subplot(222)
plot(t,v_de,t,v_qe);
legend('v_{de}','v_{qe}',0);
title('Induction Motor Responses');
xlabel('time (sec)');
grid
subplot(223)
plot(t,ialfa,t,ibeta);
legend('i_{s\alpha}','i_{s\beta}',0);
ylabel('stator currents (pu)');
xlabel('time (sec)');
grid
subplot(224)
plot(t,i_de,t,i_qe);
legend('i_{de}','i_{qe}',0);
xlabel('time (sec)');
grid

figure(2)
subplot(211)
plot(t,psi_r_alfa,t,psi_r_beta);
legend('\psi_{r\alpha}','\psi_{r\beta}',0);
title('Rotor Flux Linkages (pu)');
ylabel('Measured');
xlabel('time (sec)');
grid
subplot(212)
plot(t,psi_r_alfa_hat,t,psi_r_beta_hat);
legend('\psi_{r\alpha}','\psi_{r\beta}',0);
ylabel('Estimated');
xlabel('time (sec)');
grid

if phase1_inc_build==1 
figure(3)
subplot(211);
plot(t,rmp);
ylabel('\theta_{ramp} (pu)');
xlabel('time (sec)');
grid
subplot(212);
plot(t,theta_r);
ylabel('\theta_{\Psi_r} (pu)');
xlabel('time (sec)');
grid
end

if phase2_inc_build==1 
figure(3)
plot(t,theta_r);
title('Estimated rotor flux');
ylabel('\theta_{\Psi_r} (pu)');
xlabel('time (sec)');
grid
end

figure(4)
plot(t,speed_ref*ones(1,length(t)),':',t,wr,t,wr_hat);
legend('\omega^*_r','\omega_r','\omega_{rhat}',0);
title('Induction Motor Responses');
ylabel('rotor electrically angular velocity (pu)');
xlabel('time (sec)');
grid

figure(5)
plot(t,torque);
title('Induction Motor Responses');
ylabel('T_e (pu)');
xlabel('time (sec)');
grid

figure(6)
subplot(211)
plot(t,wr);
axis([0 Tt -1 1]);
title('Induction Motor Responses');
ylabel('Measured Rotor Speed (pu)');
xlabel('time (sec)');
grid
subplot(212)
plot(t,wr_hat);
axis([0 Tt -1 1]);
ylabel('Estimated Rotor Speed (pu)');
xlabel('time (sec)');
grid

figure(7)
subplot(211)
plot(t,ialfa);
axis([0 Tt -2 2]);
title('MATLAB Version');
ylabel('Alpha-axis stator current (pu)');
xlabel('time (sec)');
grid
subplot(212)
plot(t,wr_hat);
axis([0 Tt -1 1]);
ylabel('Estimated speed (pu)');
xlabel('time (sec)');
grid