%=============Federal University of Santa Catarina========================%
%Author: Tiago Davi Curi Busarello
%Date: April 2021
%Project: Design of the controllers for a PMSM drive

format long
clear all
close all
clc

opts = bodeoptions('cstprefs');
opts.FreqUnits = 'Hz';
s = tf('s');

%Parameters of the system
Rs = 1.3;
Ld = 0.0089;
Lq = 0.0172;
P = 6; %number of poles
lambda_af = 0.1819; %shoot
B = 0.01;
J = 0.0206;
fsw = 10e3;
Hw = 1; %speed sensor gain
Hi = 1; %current sensor gain
Vdc = 500;
Ke = 99; %Voltage Constant

%Kr=0.65*Vdc/Vcm; %Inverter gain, to be added later
%Tr=1/(2*fsw); %Inverter time delay,

Ks=1/Rs; %Gain of the machine's stator
Ts=Lq/Rs; %Time constant of the machine's stator

Kte=(3/4)*P*lambda_af; % Gain of the speed transfer function

%Current Controller design
Gid = Ks/(1 + s*(Lq/Rs)); %Current Plant for d-axis. The 0.65*Vdc is based on Krishnan's book
Giq = Ks/(1 + s*(Ld/Rs)); %Current Plant for q-axis
tau = 0.5e-3; %Desired time constant (any value between 0.5-2ms)10 times slower that 1/(2fsw)

Kpd = Ld/tau; %proportioanl gain of PI d-axis
Kid = Rs/tau; %Integral gain of PI d-axis
tau_d = Kpd/Kid; %PI d-axis time constant
%Kpd = Kpd/(Vdc/2);

Kpq = Lq/tau; %Proportional gain of PI q-axis
Kiq = Kid; %Integral gain of PI q-axis
tau_q = Kpq/Kiq; %PI q-axis time constant
%Kpq = Kpq/(Vdc/2); %The same analysis for the d-axis

PId = (Kpd*s+Kid)/s; %PI d-axis
PIq = (Kpq*s+Kiq)/s; %PI q-axis

%Speed Controller
Ka = 0.75*P*lambda_af/B;
Gs = Ka/(1+s*(J/B));

inst = evalfr(Gs, 2*pi*200*j);
ganho_fc = mag2db(abs(inst));
ganho_real = 10^(abs(ganho_fc/20));

tau_s = tau_q*10; %Used according to the PSIM example
Kps=ganho_real; %Used according to the PSIM example
Kis=Kps/tau_s;

PIs = (Kps*s+Kis)/s; %PI of the speed controller

CLTFd = (PId*Gid)/(1+PId*Gid); %Closed-loop transfer function of d-axis
CLTFq = (PIq*Giq)/(1+PIq*Giq); %Closed-loop transfer function of q-axis
CLTFs = (PIs*Gs)/(1+PIs*Gs); %Closed-loop transfer function of speed mesh

figure(1)
bode(Gs,opts)

figure(2)
bode(CLTFq,opts)
hold
%bode(CLTFq,opts)
bode(CLTFs,opts)
legend
grid