%% Calibrate the conversion constants from raw DAQ data to physical units

% Voltage, current, and string pot: the constants are determined using a mixture of experimental and design data.
% Encoder: the DAQ sends data in degrees assuming 96 tks per 360 degrees. The encoder itself is 128 tks per motor rev. The conversion tks to mm is based on manufacturer data 
     % encoder_tks = (96/360) * encoder_daq;      % convert daq to tks
     % encoder_rad = (2*pi)/(4*32) * encoder_tks; % convert tks to to radians
     % encoder_mm = (1/10901.811)*encoder_tks;    % convert tks to mm of battery travel 

     
%% Compute current and voltage offsets

fprintf('current and voltage offsets ... \n')
load('2019-03-28-calibration/rad0.0104-idle/data-2019-03-28-14-43-29.mat')
figure(1)
plot(time,current), grid, ylabel('current'), xlabel('time')
figure(2)
plot(time,voltage), grid, ylabel('voltage'), xlabel('time')

offset_current = mean(current);
offset_voltage = mean(voltage);
pause

%% Check the polarity during forward motion

fprintf('polarity check ... \n')
load('2019-03-28-calibration/run6-0-3.7-m2-2/data-2019-03-28-14-07-30.mat')
figure(1)
plot(time,encoder), grid, ylabel('encoder'), xlabel('time')
figure(2)
plot(time,current), grid, ylabel('current'), xlabel('time')
figure(3)
plot(time,voltage), grid, ylabel('voltage'), xlabel('time')
pause

%% Calibrate the sting pot

fprintf('string pot ... ')
encoder_rad = - (96/128*pi/180) * encoder;
encoder_mm = 1/(84.3*2*pi)*encoder_rad;
C = polyfit(str_pot,encoder_mm,1);
A = C(1);   offset_strpot = -C(2)/C(1);
fprintf('done \n')
pause

%% Show the calibration
fprintf('Result of calibration: \n')
fprintf("voltage(volts)   : slope = %f  offset  = %f \n", -2.15, offset_voltage)
fprintf("current(amps)    : slope = %f  offset  = %f \n", -0.1075, offset_current)
fprintf("encoder(rad)     : slope = %f  offset  = %f \n", - (96/128*pi/180) , 0)
fprintf("strpot(mm)       : slope = %f  offset  = %f \n", A, offset_strpot)
pause

%% Nominal current
fprintf('pitch correction ... \n')
filename = '2019-03-28-calibration/run6-0-3.7-m2-2/fdi-2019-03-28-14-07-30.hdf5';
[time,voltage, current, strpot, encoder, ~,~,~,~,~,~,~,~,~,pitch] = read_hdf_file(filename);
[voltage_V,current_A,strpot_mm,encoder_rad] = convert_to_physical_units(voltage,current,strpot,encoder);

plot(pitch,current_A,'.'), grid, xlabel('pitch (rad)'), ylabel('current (A)')

k = find(current_A > 0); pitch_f = pitch(k); current_f = current_A(k);
k = find(current_A < 0); pitch_r = pitch(k); current_r = current_A(k);

C_f = polyfit(pitch_f(pitch_f > 0), current_f(pitch_f > 0),1);
cutoff_f = mean(current_f(pitch_f < -0.1));

C_r = polyfit(pitch_r(pitch_r < 0), current_r(pitch_r < 0),1);
cutoff_r = mean(current_r(pitch_r > 0.1));

mean_slope = (C_f(1) + C_r(1))/2;
mean_intercept = (C_f(2) - C_r(2))/2;
mean_cutoff = (cutoff_f - cutoff_r)/2; 

fprintf("pitch correction(A)  : slope = %f A/rad  intercept  = %f A cutoff  = %f A\n", mean_slope, mean_intercept, mean_cutoff)
pause

%% Km
load('2019-03-28-calibration/run6-0-3.7-m2-2/data-2019-03-28-14-07-30.mat')
[voltage_V,current_A,strpot_mm,encoder_rad] = convert_to_physical_units(voltage,current,str_pot,encoder);

k = find(time > 5 & time < 200);
omega_mech = (encoder_rad(k+5) - encoder_rad(k-5))*1612/10;
Rm = 20.2; Km = 0.0212;

N = 21;
error = zeros(N,1);
Km_ = Km + [-10:10]*0.0001;
for m = 1:N
omega_el = (voltage_V - Rm*current_A) / Km_(m);
error(m) = norm(omega_mech - omega_el(k+40));
end

plot(Km_,error,'.')
axis([0.02 0.0225 0 2e4]), grid, xlabel('Km'), ylabel('error')
[~,m] = min(error);
fprintf('best Km = %f Nm/A \n',Km_(m));
pause

%% Nominal speed
filename = '2019-03-28-calibration/run6-0-3.7-m2-2/fdi-2019-03-28-14-07-30.hdf5';
[time,voltage, current, strpot, encoder, ~,~,~,~,~,~,~,~,~,pitch] = read_hdf_file(filename);
[voltage_V,current_A,strpot_mm,encoder_rad] = convert_to_physical_units(voltage,current,strpot,encoder);

omega_mech = [diff(encoder_rad) ./ diff(time)];
k1 = find(time < 60);
k2 = find(time > 60 & time < 150);
k3 = find(time > 150);

w1 = mean(omega_mech(k1(1:end-1)));
w2 = mean(omega_mech(k2(1:end-1)));
w3 = mean(omega_mech(k3(1:end-1)));
omega_nominal = mean(abs([w1 w2 w3]));

fprintf('omega nominal = %f rad/s \n',omega_nominal);
