clear
Motor = 'amax22'
%Motor = 'Pittman14201'
%Motor = 'Pittman6214'
% Parameters for Maxxon Amax 22, 5 Watt, 6 Volts motor
switch Motor
 case 'amax22'
  R = 1.71; %motor resistance in ohms
  L = 0.11/1000; % motor inductance in Henris
  Kt = 5.9/1000; % torque constant in Newton Meters/Amps
  Kb = Kt; 
  M = 3.88/10000000;
  b = 17/100000000 ; % motor damping in New m/(rad/sec)
  CI = 0.840; % Max current for continuous operation Amps
  CT = CI*Kt; % Torque for continuous operation Newton Meters
  Vs = 6; % Reference Voltage in Volts
  
 case 'Pittman14201'
  % Parameters for Pittman 14201 DC servo motor 
  R = 0.72; %motor resistance in ohms
  L = 0.63/1000; % motor inductance in Henris
  Kt = 0.0263; % torque constant in Newton Meters/Amps
  Kb = Kt; 
  M = 1.1/100000; %moment of inertia of rotor in Kg m^2
  b = 1.1/100000 ; % motor viscous damping in New m/(rad/sec)
  CI = 4.15; % Max current for continuous operation Amps
  CT = CI*Kt; % Torque for continuous operation
  Vs = 12; % Reference Voltage in Volts

 case 'Pittman6214'
  % Parameters for Pittman 14201 DC servo motor 
  R = 1.08; %motor resistance in ohms
  L = 0.84/1000; % motor inductance in Henris
  Kt = 0.0189; % torque constant in Newton Meters/Amps
  Kb = Kt; 
  M = 3.2/1000000; %moment of inertia of rotor in Kg m^2
  b = 2.3/1000000 ; % motor viscous damping in New m/(rad/sec)
  CT = 0.033; % Torque for continuous operation
  CI = CT/Kt; % Max current for continuous operation Amps
  Vs = 12; % Reference Voltage in Volts

 otherwise
  R = 1.71; %motor resistance in ohms
  L = 0.11/1000; % motor inductance in Henris
  Kt = 5.9/1000; % torque constant in Newton Meters/Amps
  Kb = Kt; 
  M = 3.88/10000000;
  b = 17/100000000 ; % motor damping in New m/(rad/sec)
  Vs = 6; % Reference Voltage in Volts

end


taue = L/R;  % electrical time constant
taum = M/(Kb*Kt/R + b); % mechanical time constant 


dt = taue/100; % time step in seconds


T= 20*taum;; % simulation time in secs
s = ceil(T/dt);
V=Vs;
I=zeros(s,1);
I2=zeros(s,1);
Omega= zeros(s,1);
Omega2= zeros(s,1);
V= zeros(s,1);

I(1)=0;
Omega(1)=0;
Omega2(1)=0;
V(1)=0;
for t=1: s
  if(t>10000) V(t+1) = V(t) + 0.000009*(Vs - V(t)); end
  % if (t> 60000) V(t+1) = V(t) + 0.0001*(-Vs -V(t)); end 
  dI = (V(t) - R*I(t)  - Kb*Omega(t) )*dt/L;
  I2(t) = (V(t) - Kb*Omega(t))/R; % Approximation using equilibrium currrent
                                  % equations 
  
  tau = Kt *I(t);
  tau2 = Kt*I2(t);

  
  dOmega = (tau - b*Omega(t))*dt/M;
  dOmega2= (tau2 - b*Omega(t))*dt/M;
  Omega(t+1) = Omega(t) + dOmega;
  Omega2(t+1) = Omega2(t) + dOmega2;
  I(t+1) = I(t) + dI;
  
end



OmegaNL = Vs*Kt/(R*( b + (Kb*Kt)/R));
stallTorque = Kt*Vs/R;
stallCurrent = Vs/R;
maxPowerVelocity = 0.5*OmegaNL; 
maxPowerTorque = 0.5*stallTorque;
maxPowerCurrent= 0.5*stallCurrent;



maxMechanicalPower = maxPowerTorque*maxPowerVelocity; 

efficiencyAtMaxPower = maxMechanicalPower/(Vs*maxPowerCurrent);





do = OmegaNL/100000;
o = 0:do:OmegaNL;
a1 = stallTorque;
b1 = (b + Kt*Kb/R);
c1 = Vs^2/R;

efficiency = (a1*o - b1.*o.*o)./(c1 - a1*o);


mostEffVelocity = (b1*c1 - sqrt(b1*b1*c1*c1-a1*a1*b1*c1))/a1/b1;


maxEff= (a1*mostEffVelocity - b1*mostEffVelocity*mostEffVelocity)/(c1-a1*mostEffVelocity);






disp(sprintf('Stall Torque: %f milli Newton Meters', stallTorque*1000))
disp(sprintf('Stall Current: %f Amps', stallCurrent))
disp(sprintf('No Load Speed: %f RPM', OmegaNL*60/2/pi))
disp(sprintf('Max Power Torque: %f milli Newton Meters ', ...
             maxPowerTorque*1000))
disp(sprintf('Max Power Current: %f Amps ', ...
             maxPowerCurrent))
disp(sprintf('Max Power Velocity: %f RPM', maxPowerVelocity*60/2/pi))
disp(sprintf('Max Mechanical Power : %f Watts ', maxMechanicalPower))

disp(sprintf('Efficiency at Max Mechanical Power : %f Percent ', ...
             100*efficiencyAtMaxPower))

disp(sprintf('Max Motor Efficiency : %f Percent ', ...
             100*maxEff))

disp(sprintf('Most Efficienct Velocity : %f  RPMs', ...
             mostEffVelocity*60/2/pi))

disp(sprintf('Electrical Time: Constant %f msecs', 1000*taue))
disp(sprintf('Mechanical Time: Constant %f msecs', 1000*taum))
disp(sprintf('Start Up Current: %f Amps', V/R))
disp(sprintf('Reverse Current: %f Amps', 2*V/R))









figure
n = s;

[AX, H1,H2]= plotyy(1000*(1:n)*dt, I(1:n),1000*(1:n)*dt,Omega(1:n))
set(get(AX(1), 'Ylabel'), 'String', 'Current (Amps)/ Voltage (Volts)');
set(AX(1), 'YLim',[0 6.5])
set(AX(1), 'YTick',[0 1 2 3 4 5 6])
set(AX(1), 'YTickLabel',[0 1 2 3 4 5 6])
set(get(AX(2), 'Ylabel'), 'String', 'Speed (RPM)');
xlabel('Time (msecs)');
hold on
plot(1000*(1:n)*dt,V(1:n),'r--')
legend('Velocity', 'Current','Voltage')
keyboard


figure


do = OmegaNL/100000;

o = 0:do:OmegaNL;
torq = Kt*Vs/R - (Kt*Kb/R + b)*o;
torq= torq*1000;
Power =  Kt*Vs*o/R - (Kt*Kb/R +b)*(o.^2);
o = o*60/2/pi;
[AX, H1, H2] = plotyy(o,Power,o,torq);
set(get(AX(1), 'Ylabel'), 'String', 'Power (Watts)');
set(get(AX(2), 'Ylabel'), 'String', 'Torque milli N m');


%xlim([0 OmegaNL*60/2/pi])







figure


set( 0,'DefaultAxesFontSize',18,'DefaultTextFontSize',18)     
set( 0,'DefaultAxesFontName','Times','DefaultTextFontName','Times')
set( 0,'DefaultAxesLineWidth', 2, 'DefaultLineLineWidth', 2);
set( 0,'DefaultLineMarkerSize',2);


o = 0:do:OmegaNL;
torq = Kt*Vs/R - (Kt*Kb/R + b)*o;
torq= torq*1000;
Power =  Kt*Vs*o/R - (Kt*Kb/R +b)*(o.^2);
o = o*60/2/pi;
[AX, H1, H2] = plotyy(o,Power,o,torq);
set(H1,'LineWidth',2);
set(H2,'LineWidth',2);
set(get(AX(1), 'Ylabel'), 'String', 'Power (Watts)');
set(get(AX(2), 'Ylabel'), 'String', 'Torque (Milli Newton Meters)');
set(AX(1), 'XLim',[0 max(o)])
set(AX(2), 'XLim',[0 max(o)])
xlabel('Angular Velocity (RPM)');

figure
n = 5000

plot([1:n]*dt*1000, I(1:n))
hold on 
plot([1:n]*dt*1000, I2(1:n),'r--')
xlabel('Time (millisecs)');
ylabel('Current (Amps)');

figure
plot([1:n]*dt*1000, Omega(1:n)*60/2/pi)
hold on 
plot([1:n]*dt*1000, Omega2(1:n)*60/2/pi,'r--')
xlabel('Time (millisecs)');
ylabel('Velocity (RPM)');

figure
plot(o, efficiency)
xlabel('Efficiency');
ylabel('Velocity (RPM)');