close all
clc

%%%%%%%%%%%%%%%%%%%%%%%%% 可修改定义区 %%%%%%%%%%%%%%%%%%%%%%%%% 特别注意！！！电流都应该用幅值
I_max = 495;              % 这里是幅值,扫描电流的最大值
I_min = 1;                % 这里是幅值,扫描电流的最小值
U_max = 30.022;           % 这是 RMS
p = 3;                    % 极对数
Rs = 0;                   % 定子相电阻 0.0191238
kw = 0.99493;                   % 斜槽引起的转矩削弱

%%%%%%%%%% 调整转速、电流的步长和下面插值的k可以修复map图右下角的马赛克 %%%%%%%%%% 
speed = [200:400:3400 3400:10:8000];   % 计算速度
current =(I_min:10:I_max);               % 计算的电流
k = 200;                                  % 总的插值次数， 这里多插值可以改善map图右下角马赛克

%%%%%%%%%%%%%%%%%%%%%% 读取文件，都是原始值 %%%%%%%%%%%%%%%%%%%%%%
data = load('dq_results.mat');
id_init = data.id;
iq_init = data.iq;
Psid_init = data.psid;
Psiq_init = data.psiq;
Psim_init = data.psim;  % 永磁同步电机需要Psim, 同步磁阻电机不需要
Is_init = sqrt(id_init .*id_init +iq_init .*iq_init);        % 峰值电流

ID=zeros(size(speed, 2));
IQ=zeros(size(speed, 2));
US=zeros(size(speed, 2));
TORQUE=zeros(size(speed, 2));
Torque_all=zeros(size(speed, 2));
ID_all=zeros(size(speed, 2));
IQ_all=zeros(size(speed, 2));
US_all=zeros(size(speed, 2));

n_current = 0;                 %定义最大电流循环步数
for I_max = current
n_current = n_current+1;

n_step = 0;                    %定义最大转速循环步数
for n = speed

n_step = n_step + 1 ;      %当前最大转速循环次数
f = n*p/60;                % 计算供电频率
Omega = 2*pi*f;            % 计算电角频率

%%%%%%%%%%%%% 对数据进行进行插值%%%%%%%%%%%%%%%% !!!必须放在循环中间是因为每次dq轴参数需要重置
[M,N] =size(id_init);
[X,Y] =meshgrid(1:N,1:M);

N_step = (N-1)/k;                      % N的插值步长
M_step = (M-1)/k;                      % M的插值步长

[Xq,Yq] =meshgrid((1:N_step:N),(1:M_step:M)); % 对横纵坐标进行插值

id = interp2(X,Y,id_init,Xq,Yq);
iq = interp2(X,Y,iq_init,Xq,Yq);
Psid = interp2(X,Y,Psid_init,Xq,Yq);
Psiq = interp2(X,Y,Psiq_init,Xq,Yq);
Is = interp2(X,Y,Is_init,Xq,Yq);

%%%%%%%%%%%%%%%%%%%% 计算转矩和端电压 %%%%%%%%%%%%%%%%%%%%%%%% 
Torque = 3/2*p*(Psid.*iq-Psiq.*id)*kw;                                   % 永磁同步电机公式    

%%%%%%%%%%%%%%%%%%%  峰值电流计算公式 %%%%%%%%%%%%%%%%%%% 
%Torque = 3/2*p*(Psid.*iq-Psiq.*id)*kw;                                    % 同步磁阻电机公式  
Us = sqrt(Psid.^2+Psiq.^2)*Omega/sqrt(2)+sqrt(iq.^2+id.^2)/sqrt(2)*Rs;    % 这是RMS因为除以了根号2

%%%%%%%%%%%%%%%%%%%  有效值电流计算公式 %%%%%%%%%%%%%%%%%%%
%Torque = 3/2*p*(Psid.*iq-Psiq.*id)*kw*sqrt(2);                                      % 同步磁阻电机公式，需要将电流换为峰值电流 
%Us = sqrt(Psid.^2+Psiq.^2)*Omega/sqrt(2)+sqrt(iq.^2+id.^2)*Rs;                      % 这里的Psi和i都是有效值

Is(Is>I_max)=NaN;         % 剔除超过电流幅值的点
Us(Us>U_max)=NaN;         % 剔除超过电压幅值的点 
Torque = Torque.*(Is./Is).*(Us./Us); % 剔除超过电压电流幅值的转矩点
%%%%%%%%%%%%%%% 查找转矩最大值，以及对应的行数、列数 %%%%%%%%%%%%%%%
Torque_max  = max(max(Torque));
[x,y] = find(Torque == Torque_max);


if ~isnan(Torque_max)           
   ID(n_step) = id(x,y);
   IQ(n_step) = iq(x,y);
   US(n_step) = Us(x,y);
   TORQUE(n_step) = Torque(x,y);
else                       %% 如果转矩为NaN则说明电压电流超了，此时都赋为NaN然后继续往下扫描
   ID(n_step) = NaN;
   IQ(n_step) = NaN;
   US(n_step) = NaN;
   TORQUE(n_step) = NaN;
end

end
Torque_all(n_current,:) = TORQUE;
ID_all(n_current,:) = ID;
IQ_all(n_current,:) = IQ;
US_all(n_current,:) = US;
end

%%%%%%%%%%%%%%% 转矩图 %%%%%%%%%%%%%%% 

figure(1);
plot(speed,Torque_all,'MarkerSize',15,'Marker','.');
grid on;
set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1);
set(gcf,'color','w');
set(gca,'FontSize',14,'FontName','Times New Roman');
xlabel('Speed, [rpm]');
ylabel('Torque, [Nm]');
%title('Torque');
set(gca,'Xlim',[200,8000]);
set(gca,'XTick',(200:400:8000));
% set(gca,'Ylim',[0,200]);
% set(gca,'YTick',[0:20:200]);

%%%%%%%%%%%%%%% 画转矩、电流、电压相互关联的图 %%%%%%%%%%%%%%% 

[M,N] = size(Torque_all);
[X,Y] = meshgrid(1:1:N,1:1:M);

figure(2);
stem3(X,Y,Torque_all);
grid on;
set(gcf,'color','w');
set(gca,'FontSize',16,'FontName','Times New Roman');
xlabel('X');
ylabel('Y');
title('Torque');
view(2);

figure(3);
stem3(X,Y,ID_all);
grid on;
set(gcf,'color','w');
set(gca,'FontSize',16,'FontName','Times New Roman');
xlabel('x position');
ylabel('y position');
title('Id');
view(2);

figure(4);
stem3(X,Y,IQ_all);
grid on;
set(gcf,'color','w');
set(gca,'FontSize',16,'FontName','Times New Roman');
xlabel('x position');
ylabel('y position');
title('Iq');
view(2);


figure(5);
stem3(X,Y,US_all);
grid on;
set(gcf,'color','w');
set(gca,'FontSize',16,'FontName','Times New Roman');
xlabel('x position');
ylabel('y position');
title('Us');
view(2);

Gama_all = atand(abs(IQ_all)./ID_all)+180;        % 对永磁同步电机为：Gama,同步磁阻电机为Kappa
figure(6);
stem3(X,Y,Gama_all);
grid on;
set(gcf,'color','w');
set(gca,'FontSize',16,'FontName','Times New Roman');
xlabel('x position');
ylabel('y position');
title('Gama');
view(2);

%%%%%%%%%%%%%%%%%%%  永磁同步电机角度输出 %%%%%%%%%%%%%%%%%%% 
% Gama_all = atand(abs(ID_all)./IQ_all)+90;
% figure(6);
% stem3(X,Y,Gama_all);
% grid on;
% set(gcf,'color','w');
% set(gca,'FontSize',16,'FontName','Times New Roman');
% xlabel('x position');
% ylabel('y position');
% title('Gama');
% view(2);

%%%%%%%%%%%%%%%%%%%  转速输出 %%%%%%%%%%%%%%%%%%% 
Speed_all = repelem(speed,M,1);
figure(7);
stem3(X,Y,Speed_all);
grid on;
set(gcf,'color','w');
set(gca,'FontSize',16,'FontName','Times New Roman');
xlabel('x position');
ylabel('y position');
title('Speed');
view(2);

%%%%%%%%%%%%% 写入参数  %%%%%%%%%%%%%
% xlswrite('Torque_Id_Iq',Torque_all,'Torque');% xlswrite('Torque_Id_Iq',ID_all,'Id');
% xlswrite('Torque_Id_Iq',IQ_all,'Iq');
% xlswrite('Torque_Id_Iq',Speed_all,'Speed_all');
%save('Torque_Id_Iq', 'Torque_all', 'ID_all', 'IQ_all','Speed_all');