%% this script computes the transforamtion matrices for piezo to pseudo coordinates

fjpur_111 = [-140; -67.5; 0]*0.001;
fjpuh_111 = [-140; (220-67.5); 0]*0.001;
fjpd_111 = [140; (110-67.5); 0]*0.001;

fjpur_311 = [-140; -(220-67.5); 0]*0.001;
fjpuh_311 = [-140; 67.5; 0]*0.001;
fjpd_311 = [140; -(110-67.5); 0]*0.001;

A_p2r_111 = [1 -1*fjpur_111(1) fjpur_111(2); 1 -1*fjpuh_111(1) fjpuh_111(2); 1 -1*fjpd_111(1) fjpd_111(2)];
A_p2r_311 = [1 -1*fjpur_311(1) fjpur_311(2); 1 -1*fjpuh_311(1) fjpuh_311(2); 1 -1*fjpd_311(1) fjpd_311(2)];

A_r2p_111 = inv(A_p2r_111);
A_r2p_311 = inv(A_p2r_311);

A_x2p = [-1 0 0; 0 1 0; 0 0 1];
B_x2p = [0 0 0];

motor_pos2mv = 0.6667; %modified by Ludovic 12/07/2021
% % no calibr
calibr_fjpur = (1);
calibr_fjpuh = (1);
calibr_fjpd =  (1);

%calibr
% calibr_fjpur = (1-0.016);
% calibr_fjpuh = (1+0.052);
% calibr_fjpd =  (1+0.0065);

calibr_mat = [calibr_fjpur 0 0;0 calibr_fjpuh 0;0 0 calibr_fjpd];