% Creacion de los perfiles
%
%1era prueba
to = 0; % [s]: Simulation starting time
Kv = 1; % max speed of profile
Tn = 0.05; % Tn = null time
Ta = 0.45; % Ta = ascending time
Ts = 0.05; % Ts = slew time
Td = 0.15; % Ta = descending time
tf = 2*Tn+Ta+Ts+Td; % [s]: Simulation ending time
%2da prueba
to = 0; % [s]: Simulation starting time
Kv = 1; % max speed of profile
Tn = 0.00; % Tn = null time
tf = 1;
%3ra prueba
to = 0; % [s]: Simulation starting time
Kv = 1; % max speed of profile
Tn = 0.00; % Tn = null time
tf = (10000-500)/4500; % me aseguro la misma aceleracion
%4ra prueba (para el paper)
to = 0; % [s]: Simulation starting time
Kv = 1; % max speed of profile
Tn = 0.00; % Tn = null time
tf = (10000-500)/10000; % me aseguro la aceleracion de 10000
%5ra prueba (para el paper)
to = 0; % [s]: Simulation starting time
Kv = 1; % max speed of profile
Tn = 0.00; % Tn = null time
Ta = 0.95; % Ta = ascending time= (10000-500)/10000
% me aseguro una aceleracion de 10000
Ts = 0.20; % Ts = slew time
Td = 0.95; % Ta = descending time
tf = 2*Tn+Ta+Ts+Td;
% (2*Tn+Ta+Ts+Td)/step
% round(2*SR/(Td)^2)
% Resultados
% -----------
% Max aceleracione en la bajada: 20978 p/s^2
% Con los siguientes parametros:
% Tn = 0.05; % Tn = null time
% Ta = 0.45; % Ta = ascending time
% Ts = 0.05; % Ts = slew time
% Td = 0.15; % Ta = descending time
% SR = 236; %ok max verdader p=200
% Constants definitions
%
% Time and Frecuency definitions
% 20s dividido a 10ms = 2000 pts.
% 40s dividido a 10ms = 4000 pts.
%
%SR = 96; % Step rate, maximun speed for the profiles
%SR = 236-50; %ok max verdader p=200
%SR = 400; %max verdadero p=50
%SR = 146; %max verdadero p=50
%SR = 750; % Step rate, maximun speed for the profiles
%SR = 1410; % Step rate, maximun speed for the profiles
%SR = 2280; % Step rate, maximun speed for the profiles
%SR = 4860; % Step rate, maximun speed for the profiles
%SR = 5000; % Step rate, maximun speed for the profiles
%SR = 6000; % Step rate, maximun speed for the profiles
SR = 10000; % Step rate, maximun speed for the profiles
%SR = 21276.5; % Step rate, maximun speed for the profiles
T = tf-to; % s : Period of periodic profiles
f = 2*1/T; % Hz : Frecuency of periodic profiles
% Ts = 1/SR; [s] : Periodo minimo en memoria de 1 paso.
Q = 8; % Quantization level
step = 1/(SR*Q); % s: the simulation step.
% 1 de cada 8 unidades de velocidad maxima
pts = fix(tf/step); % 4s dividido a 1ms = 4000 pts.
%Tvs = fix(Ts/(Q*step)); % Tvs : Period of the stepper voltage pulse
%Tvs = fix(Ts/step); % Tvs : Period of the stepper voltage pulse
Tvs = 1;
% Voltage definitions
V = 5;
Vh = 5;
Vl = 0;
% Stepper constructive definitions
M = 0.0002;
Tp = .002;
fp = 1/Tp;
g = 500; % [pps] : velocidad minima
k1 = g/SR; % [pps] : velocidad minima
[prof] = profiles(pts,to,tf,f,Tvs,step,V,Kv,Ta,Tn,Ts,Td,k1,'trapezoidal');
%[prof] = profiles(pts,to,tf,f,Tvs,step,V,Kv,Ta,Tn,Ts,Td,k1,'lin-inc');
Ti = 120e-6;
%Ti = 20e-6;
Ti = 100e-6;
[fo, velpro] = srgenerator(prof,pts,SR,step,Vl,Vh,Ti,to,tf,Tn);
%figure(2);
%plot([0:step:tf-step], velpro,'g-', [0:step:tf-step], SR*prof,'r-');
%plot([0:Ti:tf-Ti], velpro,'g-', [0:Ti:tf-Ti], SR*prof,'r-');
% Analisis of velocity profiles :
%
tiempo = [tf/length(velpro):tf/length(velpro):tf];
t_orig = [tf/length(prof):tf/length(prof):tf];
title(['Perfil de velocidad, V_{mx} = ', num2str(SR),' p/s, V_{mn} = ', num2str(g),'p/s, T_{i} = ', num2str(Ti), ' s.']);
hold on
figure('DefaultAxesColorOrder', [0 0 0], 'DefaultAxesLineStyleOrder', '-|:|--|-.');
plot(tiempo, velpro, t_orig, SR*prof), grid on;
axis([to tf+.02*tf -.05*max(SR*prof) 1.05*max(SR*prof)]), ylabel('Pasos/s'), xlabel('Tiempo, [s]');
hold off
% Analisis of motion profiles :
%
pos = cumsum(velpro); % CUMSUM = Cumulative sum
plot(tiempo, Ti*pos, '--'), grid on;
hold
% Analisis of profiles derivatives:
%
derivadas(SR*prof,to,tf,SR,step);
derivadas(velpro,to,tf,SR,Ti);
%
% Generacion de la forma de onda de corriente/tension de salida.
% Va y Vb es la tension aplicadad directamente a los bobinados del motor.
%
[Va, Vb] = powerwaves(fo,Vh,Vl,Tvs,V);
%
% Stepper motor constants :
%
%km=.38; % km/c = d.c. gain.
%D= .90; % parameter related to the rotational inertia and viscous
% breaking coeficcinet.
%J= 10; % magnetic and electric constant.
%L=1.11*10^(-3);
%r=1.5;
% Coeficientes con que entro en la funcion Stepper
% p,n,Flux,lambda
%
%p=1;
%n=20;
%Flux=0.18;
%lambda=1.12;
%
% Simulacion:
% New Code
% 2*pi/(4*p)
p = 200; % numero de pasos del moot
%t = [0:step:tf-step]; % time vector
%t = [0:step:tf-2*step]; % time vector
tol = 1e-4;
t = [0:Ti:tf-2*Ti]; % time vector
%t = [0:Ti:tf-Ti]; % time vector
%tol = 1e-2;
minstep = step;
maxstep = step;
Timespan = [to tf];
x = [t' Va' Vb']; % Vector de variables de entrada
%y = [th' w' ia' ib'];
x0 = [0 0]; % se definen las condiciones iniciales:
Options = SIMSET('MaxOrder', 5,'RelTol',tol,'AbsTol', tol,...
'MaxStep', maxstep,'InitialStep',step, 'InitialState', x0);
[t,x,y] = sim('stepperi', Timespan, Options);
%save stepper
plot(t,th*4*p/(2*pi), '-'), grid on;
hold
title(['Posicin. V_{mx} = ', num2str(SR),' sps. T_{i} = ', num2str(Ti), ' s.']);
ylabel('Pasos'), xlabel('Tiempo, [s]');
% resamplear para la resta
% thetha([1:1:length(tiempo)]) = th([1:(length(t)/length(tiempo)):length(t)])*4*p/(2*pi);
% resta([1:1:length(tiempo)]) = Ti*pos([1:1:length(tiempo)]) - thetha([1:1:length(tiempo)])';
thetemp = resamcon(th,t,length(tiempo)/(2*pi*110*length(th))); % Resampleo
thetha([1:1:length(tiempo)]) = thetemp([1:(length(thetemp)/length(tiempo)):length(thetemp)])*4*p/(2*pi);
resta = Ti*pos - thetha;
plot(tiempo, resta, '-.'), grid on;
% Pruebas con Ti = 20 us
Ti = 20e-6;
[fo, velpro] = srgenerator(prof,pts+1,SR,step,Vl,Vh,Ti,to,tf,Tn);
% Analisis of motion profiles :
%
tiempo = [tf/length(velpro):tf/length(velpro):tf];
title(['Perfil de velocidad, V_{mx} = ', num2str(SR),' p/s, V_{mn} = ', num2str(g),'p/s, T_{i} = ', num2str(Ti), ' s.']);
hold on
figure('DefaultAxesColorOrder', [0 0 0], 'DefaultAxesLineStyleOrder', '-|:|--|-.');
axis([to tf+.02*tf -.05*max(SR*prof) 1.05*max(SR*prof)]), ylabel('Pasos/s'), xlabel('Tiempo, [s]');
hold off
% Analisis of motion profiles :
%
pos = cumsum(velpro); % CUMSUM = Cumulative sum
plot(tiempo, Ti*pos, '--'), grid on;
hold
% Analisis of profiles derivatives:
%
derivadas(SR*prof,to,tf,SR,step);
derivadas(velpro,to,tf,SR,step);
%
% Generacion de la forma de onda de corriente/tension de salida.
% Va y Vb es la tension aplicadad directamente a los bobinados del motor.
%
[Va, Vb] = powerwaves(fo,Vh,Vl,Tvs,V);
%
% Stepper motor constants :
%
% 2*pi/(4*p)
p = 200; % numero de pasos del moot
%
% Simulacion:
% New Code
%t = [0:step:tf-step]; % time vector
%t = [0:step:tf-2*step]; % time vector
tol = 1e-4;
t = [0:Ti:tf-2*Ti]; % time vector
%t = [0:Ti:tf-Ti]; % time vector
%tol = 1e-2;
minstep = step;
maxstep = step;
Timespan = [to tf];
x = [t' Va' Vb']; % Vector de variables de entrada
%y = [th' w' ia' ib'];
x0 = [0 0]; % se definen las condiciones iniciales:
Options = SIMSET('MaxOrder', 5,'RelTol',tol,'AbsTol', tol,...
'MaxStep', maxstep,'InitialStep',step, 'InitialState', x0);
[t,x,y] = sim('stepperi', Timespan, Options);
%save stepper
plot(t,th*4*p/(2*pi), '-'), grid on;
hold
title(['Posicin. V_{mx} = ', num2str(SR),' sps. T_{i} = ', num2str(Ti), ' s.']);
ylabel('Pasos'), xlabel('Tiempo, [s]');
% resamplear para la resta
thetha = zeros(1, length(tiempo));
resta = zeros(1, length(tiempo));
%thetha([1:1:length(tiempo)]) = th([1:(length(t)/length(tiempo)):length(t)])*4*p/(2*pi);
%resta([1:1:length(tiempo)]) = Ti*pos([1:1:length(tiempo)]) - thetha([1:1:length(tiempo)])';
thetemp = resamcon(th,t,length(tiempo)/(2*pi*110*length(th))); % Resampleo
thetha([1:1:length(tiempo)-1]) = thetemp([1:(length(thetemp)/length(tiempo)):length(thetemp)])*4*p/(2*pi);
resta = Ti*pos - thetha;
plot(tiempo, resta, '-.'), grid on;
plot(resta, '-.'), grid on;
% fin del paper