%% SAMPLE_MEAS_ESS
% Plots the position and input voltage response found in the data_pos and
% data_vm variables. This script then measureds the associate steady-state
% error of the saved position ramp response.
%
%
%% Load data
% Comment these lines out if you want to use the data recently stored in
% the data_pos and data_vm variables from after running q_srv02_pos.mdl.
% (A) PV Ramp Response
% load('data_ramp_rsp_pv_q3_theta.mat');
% load('data_ramp_rsp_pv_q3_Im.mat');
% (B) PIV Ramp Response
% load('data_ramp_rsp_piv_q3_theta.mat');
% load('data_ramp_rsp_piv_q3_Im.mat');
%
%% Setup variables
% Load from variables set in workspace after running a Simulink model or
% from the previously saved response saved in the MAT files above.
t = data_pos(:,1);
yd = data_pos(:,2);
y = data_pos(:,3);
u = data_Im(:,2);
%
%% Plot response
subplot(2,1,1);
plot(t,yd,'b:',t,y,'r-');
ylabel('\theta_l (rad)');
subplot(2,1,2);
plot(t,u,'r-');
ylabel('I_m (V)');
xlabel('time (s)');
%
%% Measured steady-state error
% Time where to measure error (s)
t = 1.1;
% Sample time used in Simulink model
Ts = qc_get_step_size;
% Index to measure error
i = t/Ts;
% Measured steady-state error (rad)
e_ss_m = yd(i) - y(i);
%
%% Calculated PV steady-state error
% Frequency of triangle wave (Hz)
f = 0.8;
% Amplitude of triangle wave (rad)
A0 = pi/3;
% Ramp slope (rad/s)
rise = 2*A0;
run = 1/f/2;
R0 = rise / run;
% Calculate error (rad)
e_ss = R0*kv / kp;
%
%% Design integral control 
% Calculate the integral control gain using the expected time.
% Integration time (s)
ti = 0.5;
% Maximum voltage from integral control (V)
SRV02_IMAX = 1.6;
% Integral gain (V/rad/s)
ki_des = ( SRV02_IMAX - kp * e_ss ) / ( e_ss * ti );
%
%% Display Results
disp( ' ' );
disp( 'Expected steady-state error with PV control: ' );
disp( [ '   e_ss = ' num2str( e_ss, 3 ) ' rad' ] );
disp( 'Measured steady-state error: ' );
disp( [ '   e_ss = ' num2str( e_ss_m, 3 ) ' rad' ] );
disp( 'Integral gain: ' );
disp( [ '   ki = ' num2str( ki_des, 3 ) ' A/rad/s' ] );