www.pudn.com > Feedback-control-for-AUV.rar > REMUS_SIM.m, change:2015-11-18,size:5874b


% M-FILE INPUTS 
% ------------------------------- -------------------------------------- 
% + coeffs .  mat - generated by COEFFS .M, typically for each run 
% + vdata.mat - generated by COEFFS.M, typically for each run 
clear all 
clc ; 
% clear all variables 
disp (sprintf ( '\n \n REMUS DYNAMICS SIMULATOR')); 
disp (sprintf ('Timothy Prestero, MIT/WHOI\n\n')); 
load vehicle_type ; 
disp(sprintf (' NOTE: Model using %s REMS dimensions. \n \n', vehicle)); 
% 
% Check coeffs, initial conditions, and control input vector 
% 
choose_setup = input (' Run set-up (y In) : ) , ) s') ;  
if choose_setup == 'y' 
    sim_setup 
else 
    show all 
end 
% 
% Output flags 
% 
show_step = 1; 
show_speed = 0; 
show_pos = 0; 
run_savedata = 0 ;  
run_plots = 0; 
choose_int = 0; 
%choose_setup = 1 ; 
% 
% SET INTEGRATION METHOD 
% 
int_list = {'Basic Euler' 'Improved Euler' 'Fourth-Order Runge-Kutta'} ;  
int_methods = {'euler' 'imp_euler' 'rkutta' };  
if choose_int  
    disp(sprintf (' Integration Method: \n')) ; 
    for i = 1:size(int_list,2) ; 
    disp(sprintf(' %i - %s', i, char(int_list(i)))) 
    end 
    d = input ( '\n Enter a number: ') ; 
else 
    d = 3; 
end 
int_method = char  (int_methods (d)); 
 
% check working directory 
cd_outputs ; 
% create .mat files 
d = clock ; yy = d(l) ; mo = d(2) ; dd = d(3) ; hh = d(4) ;  
mm = d(5) ; ss = d(6) ; date_string = datestr (datenum(yy ,mo, dd) ,1); 
time_string = datestr(datenum(yy,mo,dd,hh,mm,ss) ,13) ; 
%% generate random filename 
% (dumy, file_string, dumy, dumyJ = fileparts(tempname) 
%disp(sprintf('\nCurrent simulator data files: ')); 
%ls * .  mat ; 
%file_string = input (sprintf (' \nEnter name for data file: '), 's') 
temp_str = datestr(now, 0) ;  
file_string = strcat (' sim-' , temp_str(l: 2) , temp_str(4: 6) , temp_str(10: 11) , '-' ,... 
    temp_str(13:14), temp_str(16:17)) ; 
disp(sprintf('\nData file saved as\n %s\\%s.mat', cd, file_string)); 
% EXPERIMENTAL/ASSIGNED VALUES: initial conditions, input vector 
% ------------------------------------------------------------------------------% loading model inputs, generated in SIM_SETUP. M 
load input_ vector; % data from FIN_INPUTS. M on mission files 
load time_step; 
load initial_state; % data from INITIAL_CONDITIONS.M on above 
 
pitch_max = 90 ; 
% RUN MODEL 
% ------------------------------------------------------------------------------ 
% Initialize number of steps and storage matrix 
if strcmp(int_method,' euler') 
    n_steps = size (ui, 2) ; 
else 
    n_steps = size (ui, 2)-1 
end 
output_table = zeros (n_steps, size (x, l)+size (ui, 1) +7) ; 
disp (sprintf ( '\n Simulator running...')); 
 
% MAIN PROGRAM 
for i = 1:n_steps, 
    % Print current step for error checking 
    if show_step == 1 
        if ~rem(i*10 ,n_steps) 
            disp( sprintf ( ' Steps Completed : %02d %%', i/n_steps*100)) ; 
        end 
    end 
 
    % Store current states x (n), inputs ui (n), and time in seconds 
    output_table(i,1:14) = [x' ui(:,i)']; 
    output_table (i ,21) = (i -1)*time_step; 
 
    % Calculate forces, accelerations 
    % 
    % * * CALLS REMS. M 
    % 
    % xdot(i) = f(x(i) ,u(i)) 
    [xdot , forces] = remus (x, ui(:,i)') ; 
 
    % Store forces at step n 
    output_table(i,15:20) = [forces']; 
 
    if strcmp(int_method, 'euler') 
        %% EULER INTEGRATION to calculate new states x(n+1) 
        %% x(i+1) = xCi) + dx/dt*delta_t 
        %% NOTE: overwriting old states with new states, saving back at the top of the loop 
        x = x + (xdot .* time_step) ; 
    elseif strcmp (int_method, 'imp_euler') 
        %% IMPROVED EULER INTEGRATION to calculate new states 
        k1_ vec = x + (xdot . * time_step) ; 
        k2_vec = remus(k1_vec, ui(:,(i+1))') ; 
        x = x + 0.5.*time_step.* (xdot + k2_vec); 
    elseif strcmp(int_method, 'rkutta') 
        %% RUNGE-KUTTA APPROXIMATION to calculate new states 
        %% NOTE: ideally, should be approximating ui values for k2,k3 
        %% ie (ui(: ,i)+ui(: ,i+1))/2 
        k1_vec = xdot; 
        k2_vec = remus(x+(0.5.*time_step.*k1_vec), ((ui(:,i)+ui(:,i+1))./2)'); 
        k3_vec = remus(x+(0.5.*time_step.*k2_vec), ((ui(:,i)+ui(:,i+1))./2)'); 
        k4_vec = remus(x+(time_step.*k3_vec) , ui(:,i+1)'); 
        x = x + time_step/6.*(k1_vec +2.*k2_vec +2.*k3_vec +k4_vec) ; 
        % k1_ vec xdot; 
        % k2_vec remus(x+(0.5.*time_step.*k1_vec), ((ui(: ,i)+ui(: ,i+1)) ./2)') 
        % k3_vec remus(x+(0.5.*time_step.*k2_vec), ui(: ,i)') ; 
        % k4 vec remus (x+ (time_step. *k3_ vec), ui (: , i) ') ; 
        % x = x + time_step/6.*(k1_vec +2.*k2_vec +2.*k3_vec +k4_vec) 
    end 
end 
 
% SAVE SIMULATOR OUTPUT 
% ------------------------------------------------------------------------------ 
% model coefficients and vehicle parameters loaded in REMUS.M 
load vdata ; load vehicle_type ; load inv_mass_matrix ;  
load vehicle_coeffs; 
save(file_string, 'output_table', 'file_string', 'date_string','time_string',... 
    'time_step', 'x', 'ni', ... 
    'W', 'Minv', 'B', 'm', 'g', 'rho', 'xg', 'yg', 'zg', 'xb', 'yb', 'zb', ... 
    'lxx', 'Iyy', 'Izz', 'delta_max',... 
    'Xuu', 'Xudot', 'Xwq', 'Xqq', 'Xvr', 'Xrr', 'Xprop',... 
    'Yvv', 'Yrr', 'Yuv', 'Yvdot', 'Yrdot', 'Yur', 'Ywp','Ypq','Yuudr', ... 
    'Zww', 'Zqq', 'Zuw', 'Zwdot', 'Zqdot' , 'Zuq', 'Zvp', 'Zrp' , 'Zuuds', ... 
    'Kpdot', 'Kprop', 'Kpp', ... 
    'Mww', 'Mqq', 'Muw', 'Mwdot' , 'Mqdot' , 'Muq' , 'Mvp' , 'Mrp', 'Muuds', ... 
    'Nvv', 'Nrr', 'Nuv', 'Nvdot', 'Nrdot' , 'Nur' , 'Nwp' , 'Npq' , 'Nuudr'); 
 
% return to working directory 
cd_model; 
 
% save text file of ismulator inputs 
if run_savedata 
    savedata 
end 
 
% 
% Plot output 
% 
 
figstart = input( ' \n Starting Figue Number : ' ) ;  
figstart = figstart -1; remus_plots; simplot;  
if run_plots 
    fmplot; 
end 
 
disp(sprintf(' \n')); 
return ;