# Application    = Control Theory interactive solver - correction network
# Type    = Worksheet startup code
#
# Itamar Bocher,    20 - Dec - 2012

restart; 
with(plots); 
with(DynamicSystems); 
with(Student[Calculus1]); 
with(inttrans); 
with(LinearAlgebra); 
interface(typesetting = extended);

####################################################################################################################################

#Original system Phase and Gain Margins
Original_system := proc(G,H,K,val,upper,lower)
 
 #Building the control system
  KGH  := K*G*H;
  sys  := NewSystem(KGH);
  TF  := TransferFunction(KGH);
 
 #plot limits
 if GainMargin(TF)[2]=Float(undefined) then # the phase plot is asimptotic to -180
   p_min_lim := -180; #The lower limit for assimptotic phase plot is -180
   sys_GM  := `infinty`; #The system GM
   omega_phase := `infinty`; #The phase=-180  frequency
  else
   p_min_lim := evalf(180*argument(eval(KGH, s = I*10^upper))/Pi-360); #phase plot, lower point for the vertical line
   sys_GM  := GainMargin(sys)[1]; #The system GM
   omega_phase := GainMargin(TF)[2]; #The phase=-180  frequency
 end if;
 
  p_max_lim := evalf(180*argument(eval(KGH, s = I*10^(-lower)))/Pi); #phase plot, higher point for the vertical line 
  g_max_lim := evalf(20*log[10](abs(eval(KGH, s = I*10^(-lower))))); #gain plot, lower point for the vertical line
  g_min_lim := evalf(20*log[10](abs(eval(KGH, s = I*10^upper)))); #gain plot, higher point for the vertical line 
  
 #plots
  #vertical lines
   w_Phase  := plots[display](plottools[line]([PhaseMargin(TF)[2], g_min_lim], [PhaseMargin(TF)[2], g_max_lim]), color = blue,linestyle=dash); #vertical line on the gain graph - generate from phase=-180
   w_Phase_gap := plots[display](plottools[line]([GainMargin(TF)[2], g_min_lim], [GainMargin(TF)[2], g_max_lim]), color = black,linestyle=dash); #vertical line on the gain graph - generate from gain=0
   w_Gain  := plots[display](plottools[line]([GainMargin(TF)[2], p_min_lim], [GainMargin(TF)[2], p_max_lim]), color = black,linestyle=dash); #vertical line on the phase graph - generate from gain=0
   w_Gain_gap := plots[display](plottools[line]([PhaseMargin(TF)[2], p_min_lim], [PhaseMargin(TF)[2], p_max_lim]), color = blue,linestyle=dash); #vertical line on the phase graph - generate from phase=-180
  
  #horizontal lines
   if PhaseMargin(sys)[1]>0 then 
     phase_val := evalf(180*argument(eval(KGH, s = I*PhaseMargin(TF)[2]))/Pi);#The phase value for gain=0
    else
     phase_val := evalf(180*argument(eval(KGH, s = I*PhaseMargin(TF)[2]))/Pi-360);#The phase value for gain=0 and the PM is negative
   end if;
   
   Gain_val   := evalf(20*log[10](abs(eval(KGH, s = I*GainMargin(TF)[2]))));#The gain value for phase=0  
   
  #bode plot
   Mag   := MagnitudePlot(TF,range=10^(-lower)..10^upper,colour=red); #bode magnitude 
   phase  := PhasePlot(TF,range=10^(-lower)..10^upper,colour=red); #bode phase

 #output
  
  if flag_GuideLines[1]=1 then #if displaying gain=0 crossover is on
    Original_Gain_Mag  := display(Mag,w_Phase,plot([0],10^(-lower)..10^upper,color=[blue],linestyle=[dash],legend=["gain=0"])); #plot on the gain figure the crossover and the system bode
    Original_Gain_Phase  := display(phase,w_Gain_gap,plot([phase_val],10^(-lower)..10^upper,color=[blue],linestyle=[dash],legend=["gain=0"])); #plot on the phase figure the crossover and the system bode 
   else
    Original_Gain_Mag  := MagnitudePlot(TF,range=10^(-lower)..10^upper,colour=red); #plot on the gain figure only the system bode
    Original_Gain_Phase  := PhasePlot(TF,range=10^(-lower)..10^upper,colour=red); #plot on the phase figure only the system bode
  end if;
  
  if flag_GuideLines[2]=1 then #if displaying phase=-180 crossover is on
    Original_Phase_Mag  := display(Mag,w_Phase_gap,plot([Gain_val],10^(-lower)..10^upper,color=[black],linestyle=[dash],legend=["phase=-180"]));#plot on the gain figure the crossover and the system bode
    Original_Phase_Phase := display(phase,w_Gain,plot([-180],10^(-lower)..10^upper,color=[black],linestyle=[dash],legend=["phase=-180"])); #plot on the phase figure the crossover and the system bode 
   else
    Original_Phase_Mag  := MagnitudePlot(TF,range=10^(-lower)..10^upper,colour=red);#plot on the gain figure only the system bode
    Original_Phase_Phase := PhasePlot(TF,range=10^(-lower)..10^upper,colour=red);#plot on the phase figure only the system bode
  end if;
  
  Mag_sys  := display(Original_Gain_Mag,Original_Phase_Mag);
  Phase_sys := display(Original_Gain_Phase,Original_Phase_Phase);
  sys_PM  := PhaseMargin(sys)[1]; #The system PM
  omega_gain := PhaseMargin(TF)[2]; #The gain=0  frequency
  phase_gap := evalf[5](wantedPM-PhaseMargin(TF)[1]); #The gap from the wanted PM to the system PM

 return [Mag_sys,Phase_sys,sys_PM,sys_GM,omega_phase,omega_gain,phase_gap];
 
end proc:
####################################################################################################################################

#Original and Corrected systems with the phase and gain margins
Corrected_system := proc(G,H,K,Guess,wanted_PM,flag_NetworkType,flag_GuideLines_Network,upper,lower);
  
  KGH   := K*G*H;
  sys   := NewSystem(KGH); 
  
  if flag_NetworkType=1 then
   phi := Guess;
   c  := sin(phi*Pi/180); #constant
   A   := evalf(-(1+c)/(c-1)); #from the relation sin(phi*Pi/180)=(a-1)/(1+a)
   Wm   := abs(fsolve(evalc(abs(eval(KGH,s = I*omega))) = 1/sqrt(A))); #finding the frequency
   tau  := 1/(Wm*sqrt(A));  #from the relation Wm = 1/(tau*sqrt(A))
   Gc  := (1+tau*A*s)/(1+tau*s); #the correction network
  
  elif flag_NetworkType=0 then
   ##################phi1 := wanted_PM-PhaseMargin(sys)[1]-180;
   W   := Guess; ####max(solve(evalc(Im(eval(KGH,s = I*omega))/Re(eval(KGH,s = I*omega))) = evalf(tan((Pi*phi1/180))),omega)); #finding the crossover frequency for the wanted PM, using the real roots only
   A   := 2*evalc(abs(eval(KGH,s = I*W))); #the gain for this frequency 
   a_db  := 10^(-(1/20)*A); #The gain in db
   tau  := 10/(a_db*W);
   Gc   := (1+tau*a_db*s)/(1+tau*s); #the correction network
  
  end if;
  
  # Creating the transfer function of bode plot for the original and corrected network
  TF   := TransferFunction(KGH);
  TF_net  := TransferFunction(KGH*Gc);
  
  #plot limits
   if GainMargin(TF)[2]=Float(undefined) then # the phase plot is asimptotic to -180
    p_min_lim := -180; #The lower limit for assimptotic phase plot is -180
   else
    p_min_lim := evalf(180*argument(eval(KGH, s = I*10^upper))/Pi-360); #phase plot, lower point for the vertical line
   end if;
   
   #p_min_lim  := evalf(180*argument(eval(KGH, s = I*10^upper))/Pi-360); #phase plot, lower point for the vertical line
   p_max_lim  := evalf(180*argument(eval(KGH, s = I*10^(-lower)))/Pi); #phase plot, higher point for the vertical line 
   g_max_lim  := evalf(20*log[10](abs(eval(KGH, s = I*10^(-lower))))); #gain plot, lower point for the vertical line
   g_min_lim  := evalf(20*log[10](abs(eval(KGH, s = I*10^upper)))); #gain plot, higher point for the vertical line 
   
   if GainMargin(TF_net)[2]=Float(undefined) then # the phase plot is asimptotic to -180
    Gc_p_min_lim := -180; #The lower limit for assimptotic phase plot is -180
   else
    Gc_p_min_lim := evalf(180*argument(eval(KGH*Gc, s = I*10^upper))/Pi-360); #phase plot, lower point for the vertical line
   end if;
   
   #Gc_p_min_lim := evalf(180*argument(eval(KGH*Gc, s = I*10^upper))/Pi-360); #phase plot, lower point for the vertical line
   Gc_p_max_lim := evalf(180*argument(eval(KGH*Gc, s = I*10^(-lower)))/Pi); #phase plot, higher point for the vertical line 
   Gc_g_max_lim := evalf(20*log[10](abs(eval(KGH*Gc, s = I*10^(-lower))))); #gain plot, lower point for the vertical line
   Gc_g_min_lim := evalf(20*log[10](abs(eval(KGH*Gc, s = I*10^upper)))); #gain plot, higher point for the vertical line 
  
  
  #plots
   #vertical lines plots
    w_Phase   := plots[display](plottools[line]([PhaseMargin(TF)[2], g_min_lim], [PhaseMargin(TF)[2], g_max_lim]), color = blue,linestyle=dash); #Draw vertical line on the gain plot of the original system plot for gain=0
    w_Phase_gap  := plots[display](plottools[line]([GainMargin(TF)[2], g_min_lim], [GainMargin(TF)[2], g_max_lim]), color = black,linestyle=dash);  #Draw vertical line on the gain plot of the original system plot for phase=-180        
    w_Gain   := plots[display](plottools[line]([GainMargin(TF)[2], p_min_lim], [GainMargin(TF)[2], p_max_lim]), color = black,linestyle=dash); #Draw vertical line on the phase plot of the original system plot for phase=-180
    w_Gain_gap  := plots[display](plottools[line]([PhaseMargin(TF)[2], p_min_lim], [PhaseMargin(TF)[2], p_max_lim]), color = blue,linestyle=dash); #Draw vertical line on the phase plot of the original system plot for gain=0
    
    w_Phase_net  := plots[display](plottools[line]([PhaseMargin(TF_net)[2], Gc_g_min_lim], [PhaseMargin(TF_net)[2], Gc_g_max_lim]), color = "DarkBlue",linestyle=dashdot,thickness=2); #Draw vertical line on the gain plot of the corrected system plot for gain=0
    w_Phase_net_gap := plots[display](plottools[line]([GainMargin(TF_net)[2], Gc_g_min_lim], [GainMargin(TF_net)[2], Gc_g_max_lim]), color = "DarkCyan",linestyle=dashdot,thickness=2); #Draw vertical line on the gain plot of the corrected system plot for phase=-180
    w_Gain_net  := plots[display](plottools[line]([GainMargin(TF_net)[2], Gc_p_min_lim], [GainMargin(TF_net)[2], Gc_p_max_lim]), color = "DarkCyan",linestyle=dashdot,thickness=2); #Draw vertical line on the phase plot of the original system plot for phase=-180
    w_Gain_net_gap := plots[display](plottools[line]([PhaseMargin(TF_net)[2], Gc_p_min_lim], [PhaseMargin(TF_net)[2], Gc_p_max_lim]), color = "DarkBlue",linestyle=dashdot,thickness=2); #Draw vertical line on the phase plot of the original system plot for gain=0
   
   #horizontal lines plots
     
    if PhaseMargin(sys)[1]>0 then 
      phase_val  := evalf(180*argument(eval(KGH, s = I*PhaseMargin(TF)[2]))/Pi);#The phase value for gain=0
     else
      phase_val  := evalf(180*argument(eval(KGH, s = I*PhaseMargin(TF)[2]))/Pi-360);#The phase value for gain=0 #########################
    end if; 
   
    if PhaseMargin(NewSystem(KGH*Gc))[1]>0 then
      phase_val_net := evalf(180*argument(eval(KGH*Gc, s = I*PhaseMargin(TF_net)[2]))/Pi);#The phase value for gain=0
     else
      phase_val_net := evalf(180*argument(eval(KGH*Gc, s = I*PhaseMargin(TF_net)[2]))/Pi-360);#The phase value for gain=0 #########################
    end if; 
   
    Gain_val    := evalf(20*log[10](abs(eval(KGH, s = I*GainMargin(TF)[2]))));#The gain value of the original network for phase=0
    Gain_val_net   := evalf(20*log[10](abs(eval(KGH*Gc, s = I*GainMargin(TF_net)[2]))));#The gain value of the corrected network for phase=0
    

   
   #Original and corrected systems Bode plots (Magnitude and Phase)
    Mag   := MagnitudePlot(TF,colour=red,legend="Original System",range=10^(-lower)..10^upper);         
    Mag_net  := MagnitudePlot(TF_net,colour=green,legend="Corrected System",range=10^(-lower)..10^upper);         
    phase  := PhasePlot(TF,colour=red,legend="Original System",range=10^(-lower)..10^upper);
    phase_net := PhasePlot(TF_net,colour=green,legend="Corrected System",range=10^(-lower)..10^upper);
    
  #original system with Phase=-180 crossover
  if flag_GuideLines_Network[1]=1 then
   Original_Phase_Mag  := display(Mag,w_Phase_gap,plot([Gain_val],10^(-lower)..10^upper,color=[black],linestyle=[dash],legend=["phase=-180"])); #plot the crossovers and the bode magnitude plot for this case
   Original_Phase_Phase := display(phase,w_Gain,plot([-180],10^(-lower)..10^upper,color=[black],linestyle=[dash],legend=["phase=-180"]));#plot the crossovers and the bode phase plot for this case
  else
   Original_Phase_Mag  := MagnitudePlot(TF,colour=red,legend="Original System",range=10^(-lower)..10^upper);#plot the bode magnitude plot for this case
   Original_Phase_Phase := PhasePlot(TF,colour=red,legend="Original System",range=10^(-lower)..10^upper);#plot the bode phase plot for this case
  end if;
  
  #original system with gain=0 crossover
  if flag_GuideLines_Network[2]=1 then
   Original_Gain_Mag  := display(Mag,w_Phase,plot([0],10^(-lower)..10^upper,color=[blue],linestyle=[dash],legend=["gain=0"]));#plot the crossovers and the bode magnitude plot for this case
   Original_Gain_Phase  := display(phase,w_Gain_gap,plot([phase_val],10^(-lower)..10^upper,color=[blue],linestyle=[dash],legend=["gain=0"])); #plot the crossovers and the bode phase plot for this case
  else
   Original_Gain_Mag  := MagnitudePlot(TF,colour=red,legend="Original System",range=10^(-lower)..10^upper);#plot the bode magnitude plot for this case
   Original_Gain_Phase  := PhasePlot(TF,colour=red,legend="Original System",range=10^(-lower)..10^upper);#plot the bode phase plot for this case
  end if;        
  
  #corrected system with gain=0 crossover
  if flag_GuideLines_Network[3]=1 then
   Corrected_Phase_Mag  := display(Mag_net,w_Phase_net_gap,plot([Gain_val_net],10^(-lower)..10^upper,color=["DarkCyan"],linestyle=[dashdot],thickness=2,legend=["Correction phase=-180"]));#plot the crossovers and the bode magnitude plot for this case
   Corrected_Phase_Phase := display(phase_net,w_Gain_net,plot([-180],10^(-lower)..10^upper,color=["DarkCyan"],linestyle=[dashdot],thickness=2,legend=["Correction phase=-180"])); #plot the crossovers and the bode phase plot for this case
  else
   Corrected_Phase_Mag  := MagnitudePlot(TF_net,colour=green,legend="Corrected System",range=10^(-lower)..10^upper);#plot the bode magnitude plot for this case
   Corrected_Phase_Phase := PhasePlot(TF_net,colour=green,legend="Corrected System",range=10^(-lower)..10^upper); #plot the bode phase plot for this case
  end if;
  
  #corrected system with phase=-180 crossover
  if flag_GuideLines_Network[4]=1 then
   Corrected_Gain_Mag  := display(Mag_net,w_Phase_net,plot([0],10^(-lower)..10^upper,color=["DarkBlue"],linestyle=[dashdot],thickness=2,legend=["Correction gain=0"]));#plot the crossovers and the bode magnitude plot for this case
   Corrected_Gain_Phase := display(phase_net,w_Gain_net_gap,plot([phase_val_net],10^(-lower)..10^upper,color=["DarkBlue"],linestyle=[dashdot],thickness=2,legend=["Correction gain=0"]));#plot the crossovers and the bode phase plot for this case
  else
   Corrected_Gain_Mag  := MagnitudePlot(TF_net,colour=green,legend="Corrected System",range=10^(-lower)..10^upper);#plot the bode magnitude plot for this case
   Corrected_Gain_Phase := PhasePlot(TF_net,colour=green,legend="Corrected System",range=10^(-lower)..10^upper);#plot the bode phase plot for this case
  end if; 
       
  # Output
  Mag_Original_corrected  := display(Original_Phase_Mag,Original_Gain_Mag,Corrected_Phase_Mag,Corrected_Gain_Mag);
  Phase_Original_corrected := display(Original_Phase_Phase,Original_Gain_Phase,Corrected_Phase_Phase,Corrected_Gain_Phase);
  Correction_Network   := Gc; #the correction network
  New_PM      := PhaseMargin(TF_net)[1]; #the new PM for the corrected system
  
  return [Mag_Original_corrected,Phase_Original_corrected,Correction_Network,New_PM];

end proc:
####################################################################################################################################

#Interactive correction network solver
phasemargin := proc (KGH, PMwanted, Error) 
    local sys, e, i, PMsys, Gc, PMsys1, phi, A, Wm, T, phiM, tau, a, Wphi,NetType; 
    #This procedure calculate correction network for given control system and given phase margin demand with the demand error.    
    sys  := NewSystem(KGH);
    PMsys := PhaseMargin(sys)[1]; 
    Gc   := 1;
    PMsys1 := PMsys;
    e  := 0.5;
    
    phiM  := PMwanted-180;
    W   := fsolve(simplify(evalc(Im(eval(KGH,s = I*omega))/Re(eval(KGH,s = I*omega))) = evalf(tan((1/180)*Pi*phiM))),omega); #using the real roots only
    w1  := solve(simplify(evalc(Im(eval(KGH,s = I*omega))/Re(eval(KGH,s = I*omega))) = evalf(tan((1/180)*Pi*phiM))),omega); #Check for complex roots
    
    if W < 0 and type(w1[1],float)=true and type(w1[2],float)=true then 
        
     W := max(solve(simplify(evalc(Im(eval(KGH,s = I*omega))/Re(eval(KGH,s = I*omega))) = evalf(tan((1/180)*Pi*phiM))),omega));
        
    end if;
        
    if 0 <= PMsys then 
     case := 1:  #positive PhaseMargin
    elif 0 > PMsys and W<0 and (type(w1[1],complex)=true or type(w1[2],complex)=true) then
     case := 2:  #Negative PhaseMargin with Complex roots and real negative roots
    elif 0 > PMsys and W<=1 and W>0 then 
     case := 3:  #Nega4tive PhaseMargin with real roots between 0 to 1
    elif 0 > PMsys and W>1 then 
     case := 4:   #Nega4tive PhaseMargin with real roots greater then 1
    else
     case := 5:  #No correction network
    end if:
        
     while (PMwanted-Error)>PMsys1 or PMsys1>(PMwanted+ Error) do
           
      # Lead Network
      if case=1 then;
       
       if PMsys1>=(PMwanted-Error) then 
        e := e-e/2;
       else
        e  := e+e/2;
       end if;
          
       phi  := (PMwanted-PMsys)*(1+e); 
       c  := sin(phi*Pi/180); #constant
       A   := evalf(-(1+c)/(c-1)); #from the relation sin(phi*Pi/180)=(a-1)/(1+a)
       Wm   := abs(fsolve(evalc(abs(eval(KGH,s = I*omega))) = 1/sqrt(A))); #finding the frequency
       tau  := 1/(Wm*sqrt(A));  #from the relation Wm = 1/(tau*sqrt(A))
       Gc  := (1+tau*A*s)/(1+tau*s); #the correction network
       W_return:= Wm;
       NetType := `Lead Network`;
              
      elif case=2 then;
       
       if PMsys1>=(PMwanted-Error) then 
        e := e+e/2;
       else
        e := e-e/2;
       end if;
          
       phi  := (PMwanted-PMsys)*(1-e); 
       c  := sin(phi*Pi/180); #constant
       A   := evalf(-(1+c)/(c-1)); #from the relation sin(phi*Pi/180)=(a-1)/(1+a)
       Wm   := abs(fsolve(evalc(abs(eval(KGH,s = I*omega))) = 1/sqrt(A))); #finding the frequency
       tau  := 1/(Wm*sqrt(A));  #from the relation Wm = 1/(tau*sqrt(A))
       Gc  := (1+tau*A*s)/(1+tau*s); #the correction network
       W_return:= Wm;
       NetType := `Lead Network`;
       
      end if;
           
      #Lag Network
       
      if case=3 then    
       if PMsys1>=(PMwanted-Error) then 
        e := e+e/2;
       else
        e := e-e/2;
       end if;  
       Wphi := W*(1+e);
       W   := fsolve(evalc(Im(eval(KGH,s = I*omega))/Re(eval(KGH,s = I*omega))) = evalf(tan((Pi*phi/180))),omega); #finding the crossover frequency for the wnted PM, using the real roots only
       A   := 2*evalc(abs(eval(KGH,s = I*W))); #the gain for this frequency 
       a_db  := 10^(-(1/20)*A); #The gain in db
       tau  := 10/(a_db*W);
       Gc   := (1+tau*a_db*s)/(1+tau*s); #the correction network
       W_return:= Wphi;
       NetType := `Lag Network`;
       
      elif case=4 then
        
       if PMsys1>=(PMwanted-Error) then 
        e := e-e/2;
       else
        e := e+e/2;
       end if;  
       Wphi := W*(1-e);
       W   := fsolve(evalc(Im(eval(KGH,s = I*omega))/Re(eval(KGH,s = I*omega))) = evalf(tan((Pi*phi/180))),omega); #finding the crossover frequency for the wnted PM, using the real roots only
       A   := 2*evalc(abs(eval(KGH,s = I*W))); #the gain for this frequency 
       a_db  := 10^(-(1/20)*A); #The gain in db
       tau  := 10/(a_db*W);
       Gc   := (1+tau*a_db*s)/(1+tau*s); #the correction network
       W_return:= Wphi;
       NetType := `Lag Network`;
              
      end if;        
      
      #Error - There is no correcttion network
      
      if case=5 then 
       W_return:= 1;
       Gc  := 1;
       NetType := `There is no Correction Network`;
       break;
      end if;
      
      PMsys1   := PhaseMargin(NewSystem(KGH*Gc))[1]; 
           
     end do; 
    return [Gc, PMsys1, NetType,W_return];
    end proc;