% Data from YongLae's strain gage calibration tests.

% X-axis displacement test (along leg axis) - TRIAL 1
% Displacement (in), Weight (g), Force (N), V_s, V_c
TrialX1Data = [0	0	0	2.105	2.082
0.01	19	0.186371	2.065	2.091
0.02	41	0.402169	2.015	2.102
0.03	63	0.617967	1.966	2.11
0.04	83	0.814147	1.916	2.123
0.05	107	1.049563	1.864	2.133
0.06	128	1.255552	1.815	2.145
0.07	149	1.461541	1.767	2.159
0.08	173	1.696957	1.715	2.172
0.09	194	1.902946	1.665	2.185
0.1	214	2.099126	1.615	2.203
0.11	237	2.324733	1.56	2.215
0.12	260	2.55034	1.51	2.228
0.13	282	2.766138	1.459	2.241
0.14	304	2.981936	1.409	2.251
0.15	324	3.178116	1.358	2.268
0.16	346	3.393914	1.308	2.281
0.17	369	3.619521	1.255	2.293
0.18	391	3.835319	1.205	2.305
0.19	413	4.051117	1.148	2.321
0.2	435	4.266915	1.098	2.334
0.21	459	4.502331	1.046	2.346
0.22	481	4.718129	0.993	2.359
0.23	505	4.953545	0.943	2.369
0.24	528	5.179152	0.892	2.383
0.25	549	5.385141	0.84	2.398
0.26	571	5.600939	0.788	2.411
0.27	598	5.865782	0.735	2.421
0.28	621	6.091389	0.681	2.434
0.29	645	6.326805	0.629	2.446
0.3	670	6.57203	0.577	2.459
0.31	692	6.787828	0.524	2.474
0.32	719	7.052671	0.468	2.484
0.33	745	7.307705	0.417	2.495
0.34	770	7.55293	0.363	2.506
0.35	797	7.817773	0.312	2.516
0.36	822	8.062998	0.259	2.526
0.37	847	8.308223	0.201	2.539
0.38	876	8.592684	0.15	2.547
0.39	904	8.867336	0.096	2.556
0.4	930	9.12237	0.043	2.565];

TrialX1.Displacement = [TrialX1Data(:,1)*0.0254, zeros(size(TrialX1Data,1),1)]; % Convert inches to meters
TrialX1.Mass = TrialX1Data(:,2)/1000;  %Convert grams to kilograms
TrialX1.Force = [TrialX1Data(:,3), zeros(size(TrialX1Data,1),1)];
TrialX1.Vs = TrialX1Data(:,4);
TrialX1.Vc = TrialX1Data(:,5);
TrialX1.VsOffset = TrialX1.Vs(1);
TrialX1.VcOffset = TrialX1.Vc(1);
TrialX1.Vs = TrialX1.Vs-TrialX1.VsOffset;  % Start at (0,0)
TrialX1.Vc = TrialX1.Vc-TrialX1.VcOffset;  % Start at (0,0)

% X-axis displacement test (along leg axis) - TRIAL 2
% Displacement (in), Weight (g), Force (N), V_s, V_c
TrialX2Data = [0	0	0	2.103	2.076
0.01	22	0.215798	2.056	2.084
0.02	43	0.421787	2.008	2.094
0.03	64	0.627776	1.96	2.105
0.04	85	0.833765	1.911	2.117
0.05	107	1.049563	1.863	2.128
0.06	129	1.265361	1.812	2.14
0.07	151	1.481159	1.763	2.152
0.08	173	1.696957	1.712	2.164
0.09	195	1.912755	1.661	2.177
0.1	216	2.118744	1.611	2.192
0.11	238	2.334542	1.56	2.201
0.12	260	2.55034	1.509	2.216
0.13	282	2.766138	1.459	2.228
0.14	304	2.981936	1.409	2.242
0.15	325	3.187925	1.358	2.255
0.16	347	3.403723	1.309	2.269
0.17	369	3.619521	1.258	2.282
0.18	389	3.815701	1.205	2.297
0.19	414	4.060926	1.153	2.309
0.2	434	4.257106	1.104	2.325
0.21	456	4.472904	1.053	2.339
0.22	479	4.698511	1	2.353
0.23	502	4.924118	0.951	2.367
0.24	525	5.149725	0.899	2.379
0.25	548	5.375332	0.848	2.392
0.26	569	5.581321	0.799	2.406
0.27	593	5.816737	0.744	2.419
0.28	618	6.061962	0.694	2.43
0.29	641	6.287569	0.642	2.445
0.3	667	6.542603	0.592	2.456
0.31	692	6.787828	0.537	2.469
0.32	715	7.013435	0.486	2.48
0.33	742	7.278278	0.434	2.491
0.34	767	7.523503	0.38	2.501
0.35	793	7.778537	0.328	2.511
0.36	819	8.033571	0.274	2.521
0.37	841	8.249369	0.221	2.535
0.38	873	8.563257	0.166	2.541
0.39	901	8.837909	0.167	2.551
0.4	929	9.112561	0.167	2.559];

TrialX2.Displacement = [TrialX2Data(:,1)*0.0254, zeros(size(TrialX2Data,1),1)]; % Convert inches to meters
TrialX2.Mass = TrialX2Data(:,2)/1000;  %Convert grams to kilograms
TrialX2.Force = [TrialX2Data(:,3), zeros(size(TrialX2Data,1),1)];
TrialX2.Vs = TrialX2Data(:,4);
TrialX2.Vc = TrialX2Data(:,5);
TrialX2.VsOffset = TrialX2.Vs(1);
TrialX2.VcOffset = TrialX2.Vc(1);
TrialX2.Vs = TrialX2.Vs-TrialX2.VsOffset;  % Start at (0,0)
TrialX2.Vc = TrialX2.Vc-TrialX2.VcOffset;  % Start at (0,0)



% Y-axis displacement test (along leg axis) - TRIAL 1
% Displacement (in), Weight (g), Force (N), V_s, V_c
TrialY1Data = [0	0	0	2.123	2.025
0.01	17	0.166753	2.096	1.994
0.02	27	0.264843	2.084	1.965
0.03	31	0.304079	2.088	1.935
0.04	46	0.451214	2.057	1.904
0.05	45	0.441405	2.079	1.871
0.06	59	0.578731	2.049	1.838
0.07	68	0.667012	2.038	1.809
0.08	68	0.667012	2.067	1.775
0.09	73	0.716057	2.073	1.744
0.1	77	0.755293	2.073	1.715
0.11	82	0.804338	2.072	1.685
0.12	86	0.843574	2.07	1.658
0.13	86	0.843574	2.075	1.633
0.14	92	0.902428	2.069	1.605
0.15	105	1.029945	2.041	1.577
0.16	111	1.088799	2.035	1.549
0.17	111	1.088799	2.037	1.526
0.18	122	1.196698	2.011	1.496
0.19	132	1.294788	1.981	1.47
0.2	115	1.128035	2.039	1.454
0.21	132	1.294788	2.009	1.417
0.22	141	1.383069	1.98	1.393
0.23	129	1.265361	2.02	1.378
0.24	143	1.402687	1.992	1.342
0.25	148	1.451732	1.974	1.321
0.26	150	1.47135	1.971	1.299
0.27	163	1.598867	1.943	1.268
0.28	171	1.677339	1.913	1.244
0.29	178	1.746002	1.891	1.221
0.3	186	1.824474	1.867	1.197
0.31	176	1.726384	1.892	1.186
0.32	191	1.873519	1.865	1.149
0.33	199	1.951991	1.838	1.127
0.34	204	2.001036	1.813	1.107
0.35	212	2.079508	1.791	1.082
0.36	219	2.148171	1.766	1.058
0.37	225	2.207025	1.744	1.037
0.38	231	2.265879	1.722	1.016
0.39	236	2.314924	1.698	0.993
0.4	241	2.363969	1.675	0.972
0.41	246	2.413014	1.654	0.95
0.42	251	2.462059	1.631	0.927
0.43	256	2.511104	1.611	0.907
0.44	261	2.560149	1.592	0.886
0.45	266	2.609194	1.574	0.864
0.46	273	2.677857	1.554	0.836
0.47	287	2.815183	1.54	0.783
0.48	296	2.903464	1.527	0.737
0.49	307	3.011363	1.517	0.691
0.5	315	3.089835	1.511	0.647];

TrialY1.Displacement = [zeros(size(TrialY1Data,1),1), TrialY1Data(:,1)*0.0254]; % Convert inches to meters
TrialY1.Mass = TrialY1Data(:,2)/1000;  %Convert grams to kilograms
TrialY1.Force = [zeros(size(TrialY1Data,1),1), TrialY1Data(:,3)];
TrialY1.Vs = TrialY1Data(:,4);
TrialY1.Vc = TrialY1Data(:,5);
TrialY1.VsOffset = TrialY1.Vs(1);
TrialY1.VcOffset = TrialY1.Vc(1);
TrialY1.Vs = TrialY1.Vs-TrialY1.VsOffset;  % Start at (0,0)
TrialY1.Vc = TrialY1.Vc-TrialY1.VcOffset;  % Start at (0,0)


% Y-axis displacement test (along leg axis) - TRIAL 2
% Displacement (in), Weight (g), Force (N), V_s, V_c
TrialY2Data = [0	0	0	2.128	2.038
0.01	16	0.156944	2.103	2
0.02	26	0.255034	2.086	1.967
0.03	34	0.333506	2.084	1.936
0.04	40	0.39236	2.083	1.901
0.05	47	0.461023	2.077	1.863
0.06	58	0.568922	2.055	1.832
0.07	67	0.657203	2.044	1.803
0.08	74	0.725866	2.035	1.775
0.09	74	0.725866	2.061	1.745
0.1	76	0.745484	2.075	1.717
0.11	81	0.794529	2.075	1.687
0.12	87	0.853383	2.068	1.659
0.13	92	0.902428	2.067	1.631
0.14	94	0.922046	2.071	1.604
0.15	100	0.9809	2.059	1.577
0.16	110	1.07899	2.04	1.55
0.17	113	1.108417	2.035	1.526
0.18	117	1.147653	2.032	1.498
0.19	122	1.196698	2.02	1.472
0.2	132	1.294788	1.99	1.447
0.21	143	1.402687	1.96	1.423
0.22	123	1.206507	2.024	1.407
0.23	141	1.383069	1.996	1.369
0.24	150	1.47135	1.966	1.345
0.25	137	1.343833	2.005	1.332
0.26	152	1.490968	1.979	1.294
0.27	152	1.490968	1.975	1.278
0.28	164	1.608676	1.947	1.246
0.29	173	1.696957	1.918	1.222
0.3	179	1.755811	1.894	1.201
0.31	188	1.844092	1.868	1.177
0.32	193	1.893137	1.844	1.155
0.33	181	1.775429	1.882	1.144
0.34	196	1.922564	1.858	1.106
0.35	202	1.981418	1.832	1.085
0.36	209	2.050081	1.807	1.062
0.37	215	2.108935	1.783	1.039
0.38	220	2.15798	1.765	1.02
0.39	226	2.216834	1.742	0.997
0.4	232	2.275688	1.721	0.976
0.41	238	2.334542	1.696	0.953
0.42	243	2.383587	1.674	0.933
0.43	248	2.432632	1.652	0.911
0.44	252	2.471868	1.63	0.888
0.45	257	2.520913	1.609	0.868
0.46	263	2.579767	1.588	0.845
0.47	274	2.687666	1.567	0.806
0.48	288	2.824992	1.549	0.751
0.49	297	2.913273	1.535	0.711
0.5	309	3.030981	1.521	0.66];

TrialY2.Displacement = [zeros(size(TrialY2Data,1),1), TrialY2Data(:,1)*0.0254]; % Convert inches to meters
TrialY2.Mass = TrialY2Data(:,2)/1000;  %Convert grams to kilograms
TrialY2.Force = [zeros(size(TrialY2Data,1),1), TrialY2Data(:,3)];
TrialY2.Vs = TrialY2Data(:,4);
TrialY2.Vc = TrialY2Data(:,5);
TrialY2.Offset = TrialY2.Force(1);
TrialY2.Displacement = TrialY2.Displacement-TrialY2.Offset;  % Start at (0,0)
TrialY2.VsOffset = TrialY2.Vs(1);
TrialY2.VcOffset = TrialY2.Vc(1);
TrialY2.Vs = TrialY2.Vs-TrialY2.VsOffset;  % Start at (0,0)
TrialY2.Vc = TrialY2.Vc-TrialY2.VcOffset;  % Start at (0,0)


GageCalib.Displacement = [TrialX1.Displacement; TrialX2.Displacement; TrialY1.Displacement; TrialY2.Displacement];
GageCalib.Mass =         [TrialX1.Mass;         TrialX2.Mass;         TrialY1.Mass;         TrialY2.Mass];
GageCalib.Force =        [TrialX1.Force;        TrialX2.Force;        TrialY1.Force;        TrialY2.Force];
GageCalib.Vs =           [TrialX1.Vs;           TrialX2.Vs;           TrialY1.Vs;           TrialY2.Vs];
GageCalib.Vc =           [TrialX1.Vc;           TrialX2.Vc;           TrialY1.Vc;           TrialY2.Vc];
GageCalib.VsOffset =     [TrialX1.VsOffset;     TrialX2.VsOffset;     TrialY1.VsOffset;     TrialY2.VsOffset];
GageCalib.VcOffset =     [TrialX1.VcOffset;     TrialX2.VcOffset;     TrialY1.VcOffset;     TrialY2.VcOffset];

clear Trial*

%Plot Data

figure(1)
clf
subplot(3,1,1), plot3(GageCalib.Force(:,1),GageCalib.Force(:,2),GageCalib.Vs,'bx',GageCalib.Force(:,1),GageCalib.Force(:,2),GageCalib.Vc,'rx'), ...
    legend('Vs','Vc', 0), title('X Direction'), grid on, xlabel('Force X'), ylabel('Force Y'), zlabel('Voltage'), ...
    view(0,0);
subplot(3,1,2), plot3(GageCalib.Force(:,1),GageCalib.Force(:,2),GageCalib.Vs,'bx',GageCalib.Force(:,1),GageCalib.Force(:,2),GageCalib.Vc,'rx'), ...
    legend('Vs','Vc', 0), title('Y Direction'), grid on, xlabel('Force X'), ylabel('Force Y'), zlabel('Voltage'), ...
    view(90,0);
subplot(3,1,3), plot3(GageCalib.Force(:,1),GageCalib.Force(:,2),GageCalib.Vs,'bx',GageCalib.Force(:,1),GageCalib.Force(:,2),GageCalib.Vc,'rx'), ...
    legend('Vs','Vc', 0), title('3D View'), grid on, xlabel('Force X'), ylabel('Force Y'), zlabel('Voltage');

%subplot(2,1,2), plot3(GageCalib.Force(:,2),GageCalib.Vs,'bx',GageCalib.Force(:,2),GageCalib.Vc,'rx'), legend('Vs','Vc'), title('Y Direction'), grid on;

%Pseudo Inverse Method
%C (2x2) = F (2xn) * pinv(V) (nx2)
C = GageCalib.Force' * pinv([GageCalib.Vs, GageCalib.Vc]');
% Go back from F=CV
ForceOut = [C*[GageCalib.Vs, GageCalib.Vc]']';
figure(2)
clf
subplot(3,1,1), plot3(GageCalib.Force(:,1),GageCalib.Force(:,2),GageCalib.Vs,'bx',GageCalib.Force(:,1),GageCalib.Force(:,2),...
    GageCalib.Vc,'rx',ForceOut(:,1),ForceOut(:,2),GageCalib.Vs,'go',ForceOut(:,1),ForceOut(:,2),GageCalib.Vc,'co'), ...
    legend('Vs','Vc','CalibVs','CalibVc', 0), title('X Direction'), grid on, xlabel('Force X'), ylabel('Force Y'), zlabel('Voltage'), ...
    view(0,0);
subplot(3,1,2), plot3(GageCalib.Force(:,1),GageCalib.Force(:,2),GageCalib.Vs,'bx',GageCalib.Force(:,1),GageCalib.Force(:,2),...
    GageCalib.Vc,'rx',ForceOut(:,1),ForceOut(:,2),GageCalib.Vs,'go',ForceOut(:,1),ForceOut(:,2),GageCalib.Vc,'co'), ...
    legend('Vs','Vc','CalibVs','CalibVc', 0), title('Y Direction'), grid on, xlabel('Force X'), ylabel('Force Y'), zlabel('Voltage'), ...
    view(90,0);
subplot(3,1,3), plot3(GageCalib.Force(:,1),GageCalib.Force(:,2),GageCalib.Vs,'bx',GageCalib.Force(:,1),GageCalib.Force(:,2),...
    GageCalib.Vc,'rx',ForceOut(:,1),ForceOut(:,2),GageCalib.Vs,'go',ForceOut(:,1),ForceOut(:,2),GageCalib.Vc,'co'), ...
    legend('Vs','Vc','CalibVs','CalibVc', 0), title('3D View'), grid on, xlabel('Force X'), ylabel('Force Y'), zlabel('Voltage');

%MATLAB Slash Method
% (2x2)    (2xN) /  (2xN)
Cslash = GageCalib.Force' / [GageCalib.Vs, GageCalib.Vc]';
% Should be same result... it is.