|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%% Initialize GenoM stuff and connect the ports %%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
%% %%%%%%%%%%%% SETUP %%%%%%%%%%%%%%%%%%%%%%%
|
|
filename_imuCalibration = 'QR7_imu_calibration';
|
|
quad_num = '7';
|
|
|
|
switch flag_simulationReal
|
|
case 1 % Simulation
|
|
disp('Initialize genom components for simulation');
|
|
host_base = 'localhost';
|
|
host_remote = 'localhost';
|
|
case 2 % Real experiment
|
|
disp('Initialize genom components for real experiment');
|
|
host_base = 'localhost';
|
|
host_remote = 'fritillary-wifi';
|
|
end
|
|
|
|
openrobots_dir = getenv('ROBOTPKG_BASE');
|
|
% Load genomix client
|
|
client = genomix.client(host_remote);
|
|
client.rpath([openrobots_dir,'/lib/genom/pocolibs/plugins']);
|
|
|
|
%% %%%%%%%%%%%% MRSIM %%%%%%%%%%%%%%%%%%%%%%%
|
|
if flag_simulationReal == 1
|
|
disp('* Initialize Mrsim');
|
|
mrsim = client.load('mrsim');
|
|
string = 'Connecting to mrsim: done';
|
|
disp(string);
|
|
mrsim.set_pty('/tmp/mrsim');
|
|
end
|
|
|
|
%% %%%%%%%%%%%% ROTORCRAFT %%%%%%%%%%%%%%%%%
|
|
disp('* Initialize Rotorcraft');
|
|
rotorcraft = client.load('rotorcraft');
|
|
pause(1);
|
|
|
|
switch flag_simulationReal
|
|
case 1 % Simulation
|
|
result_mk = rotorcraft.connect('/tmp/mrsim', 500000);
|
|
case 2 % Real experiment
|
|
result_mk = rotorcraft.connect('/dev/ttyUSB0', 500000);
|
|
end
|
|
string = ['Connecting to rotorcraft: ',result_mk.status];
|
|
disp(string);
|
|
|
|
% calibration
|
|
if flag_simulationReal == 2
|
|
load(strcat(pwd, filesep, 'calibration', filesep, filename_imuCalibration));
|
|
eval('calibration');
|
|
rotorcraft.set_imu_calibration(calibration);
|
|
end
|
|
|
|
% log of files
|
|
rotorcraft.set_sensor_rate(1000,16,50,1); % IMU - mag - motor - battery
|
|
|
|
% connection to nhfc
|
|
result_nhfc = rotorcraft.connect_port('rotor_input' , 'nhfc/rotor_input');
|
|
string = ['Initializing connection of Nhfc: ',result_nhfc.status];
|
|
disp(string);
|
|
|
|
%% %%%%%%%%%%%%% OPTITRACK %%%%%%%%%%%%%%%%%%
|
|
if flag_simulationReal == 2
|
|
disp('* Initialize Optitrack');
|
|
opti = client.load('optitrack');
|
|
pause(1);
|
|
|
|
result = opti.connect('marey', '1510', '239.192.168.30', '1511');
|
|
string = ['Connecting to MoCap: ', result.status];
|
|
disp(string);
|
|
end
|
|
|
|
%% %%%%%%%%%%%%% POM %%%%%%%%%%%%%%%%%%%%%%%%
|
|
disp('* Initialize Pom');
|
|
pom = client.load('pom');
|
|
pause(1);
|
|
|
|
result_pom = pom.connect_port('measure/imu', 'rotorcraft/imu');
|
|
string = ['Initializing 1st connection of POM (imu): ', result_pom.status];
|
|
pom.add_measurement('imu');
|
|
disp(string);
|
|
|
|
switch flag_simulationReal
|
|
case 1 % Simulation
|
|
result = pom.connect_port('measure/gps', 'mrsim/gps');
|
|
string = ['Initializing 2nd connection of POM (mrsim/gps): ',result.status];
|
|
pom.add_measurement('gps');
|
|
disp(string);
|
|
case 2 % Real experiment
|
|
result = pom.connect_port('measure/mocap', strcat('optitrack/bodies/QR_',quad_num));
|
|
string = ['Initializing 2nd connection of POM (optitrack): ',result.status];
|
|
pom.add_measurement('mocap');
|
|
disp(string);
|
|
end
|
|
|
|
%% %%%%%%%%%%%%% NHFC %%%%%%%%%%%%%%%%%%%%%%%
|
|
disp('* Initialize Nhfc');
|
|
nhfc = client.load('nhfc');
|
|
nhfc.set_mass(mR);
|
|
nhfc.set_geom(G_UAV,J_UAV);
|
|
nhfc.set_servo_gain(Kp_p(1,1) , Kp_p(3,3) , Kp_R(1,1) , Kp_R(3,3) , Kd_p(1,1) , Kd_p(3,3) , Kd_R(1,1) , Kd_R(3,3), 0, 0); % Kpxy, Kpz, Kqxy, Kqz, Kvxy, Kvz, Kwxy, Kwz,
|
|
nhfc.set_emerg(1, 0.03, 1, 0.03, 1); % descent acc, uncer pos (3*std), uncer orien, uncer vel, unc ang vel
|
|
result = nhfc.connect_port('state', 'pom/frame/robot');
|
|
string = ['Conncting to pom: ',result.status];
|
|
disp(string);
|
|
result = nhfc.connect_port('reference', 'maneuver/desired');
|
|
string = ['Conncting to maneuver: ',result.status];
|
|
disp(string);
|
|
|
|
%% %%%%%%%%%%%%% MANEUVER %%%%%%%%%%%%%%%%%%%%%%%
|
|
disp('* Initialize Maneuver');
|
|
maneuver = client.load('maneuver');
|
|
result = maneuver.connect_port('state', 'pom/frame/robot');
|
|
string = ['Initializing maneuver: ',result.status];
|
|
disp(string);
|
|
|
|
%% %%%%%%%%%%%%% BATTERY %%%%%%%%%%%%%%%%%%%%
|
|
batteryLevel = rotorcraft.get_battery();
|
|
disp(strcat('Battery level: ', num2str(batteryLevel.result.battery.level)));
|
