%   Program to simulate an astronomical telescope being slewed to a new
%   position

close all ;
KA = 1 ;    %   Amplifier Gain (V/V)
Ktau = 2 ; %   Voltage to torque conversion constant (N-m/V)
I = 10000 ;    %   Telescope's rotational moment of inertia (kg-m^2)
Kf = 200 ;   %   Angular velocity to resisting torque conversion constant (N-m/rad/s)
wcm = 5 ;  %   Radian corner frequency of motor response
Kw = 5 ;    %   Position to voltage conversion factor (V/radian)
s = tf('s') ;
Hm = Ktau*wcm/(s+wcm) ;     %   Motor transfer function (N-m/V)
HT = (1/I)/(s*(s+Kf/I)) ;   %   Telescope transfer function (radians/N-M)
H1 = minreal(KA*Hm*HT*Kw) ;        %   Forward path transfer function of unity-gain feedback system
H = minreal(H1/(1+H1)) ;         %   Overall system transfer function
Vset = Kw/s ;           %   Set point forcing function
Vresp = Vset*H ;
E = Vset - Vresp ;
Tau = KA*Hm*E ;
Wpos = Tau*HT ;
figure('Position',[20,20,1500,800]) ;
[wpos,tw] = impulse(Wpos) ;
[e,te] = impulse(E) ;
[tau,ttau] = impulse(Tau) ;
subplot(1,2,1) ;
ptr = plot(tw,wpos,'k',te,e,'r',ttau,tau,'b') ; set(ptr,'LineWidth',2) ;
ptr = legend('Angular position (radians)','Error Signal (V)','Torque (N-m)') ;
set(ptr,'FontName','Times','FontSize',18) ;
xlabel('Time (s)','FontName','Times','FontSize',18) ;
grid on ;
ttl = ['{\itK}_{\itA} = ',num2str(KA),' V/V, {\itK}_{\tau} = ',num2str(Ktau),' N-m/V'] ;
ttl = str2mat(ttl,['{\itK}_{\itf} = ',num2str(Kf),' N-m/(rad/s), {\itK}_{\omega} = ',num2str(Kw),' V/rad']) ;
ttl = str2mat(ttl,'              Set Point is 1 radian') ;
title(ttl,'FontName','Times','FontSize',18) ;
ps = pole(H) ;
zs = zero(H) ;
subplot(1,2,2) ;
pzplot(zs,ps) ;