%This file generates a .pmc file to be uploaded onto the tank control
%software (Delta Tau, PMAC)

%(This example generates the oscillatory motion used in Heather's whisker 
%experiments. Here, 90 different motions are generated, covering a range 
%of frequencies and amplitudes)



clear all; close all;

% Name files to be generated
filename = ['motion_example.pmc']; % output file name to use with PMACS
fid = fopen(filename,'w');
filename = ['motion_key_example.asc']; %write file with pos and vel points
motkeyfile=fopen(filename,'w');


% Start file
fprintf(fid,'CLOSE\n');
fprintf(fid,'END GATHER\n');
fprintf(fid,'DELETE GATHER\n');
fprintf(fid,'DELETE TRACE\n\n');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%PROGRAM 1- HOMING%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Write the header with program information

fprintf(fid,'//*********************************************\n');
fprintf(fid,'// prog 1 Homing Routine\n');

% Write the homing program routine

fprintf(fid,'\n\n//Homing Routine\n');
fprintf(fid,'open prog1\n');
fprintf(fid,'clear\n');
fprintf(fid,'// Homing X-axis\n');
fprintf(fid,'ta 50              // set acc time to 50 ms\n');
fprintf(fid,'ts 50              // set s-curve time to 50 ms\n');
fprintf(fid,'f .25              // set feedrate to 0.25 units/s\n');
fprintf(fid,'inc                // incremental mode\n');
fprintf(fid,'While (m141 = 0)   // axis 1 warning following error bit\n');
fprintf(fid,'x +.05             // move X-axis specified distance\n');
fprintf(fid,'EndWhile\n');
fprintf(fid,'dwell 0            // do not blend into next move\n');
fprintf(fid,'HOMEZ1             // set encoder position to 0\n');
fprintf(fid,'dwell 200          // wait 200 ms\n');
fprintf(fid,'f 1                // set feedrate to 1\n');
fprintf(fid,'x -2              // move to 2,725 units\n');
fprintf(fid,'dwell 0            // do not blend into next move\n');
fprintf(fid,'HOMEZ1             // set encoder position to 0\n');

fprintf(fid,'// Homing Y-axis\n');
fprintf(fid,'ta 50\n');
fprintf(fid,'ts 50\n');
fprintf(fid,'f .25\n');
fprintf(fid,'inc\n');
fprintf(fid,'While (m241 = 0)\n');
fprintf(fid,'y -.05\n');
fprintf(fid,'EndWhile\n');
fprintf(fid,'dwell 0\n');
fprintf(fid,'HOMEZ2\n')
fprintf(fid,'dwell 200\n');
fprintf(fid,'f 1\n');
fprintf(fid,'y 3.9\n'); % was 3.7 on original version
fprintf(fid,'dwell 0\n');
fprintf(fid,'HOMEZ2\n');
fprintf(fid,'close\n\n\n');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%PROGRAM 2-? MOTION%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------------------Set experiment parameters--------------------------

d = 0.0265; % avg spanwise diameter [m] 
U = 0.2; %[m/s]

A_ratio_array = [0.025:0.025:0.15]; %ratio value to give amplitude of motion
A_array = A_ratio_array.*d/.0254; %amplitude of motion [in]

Vr_array=[2.5:0.25:6]; %reduced velocity
f_array=U./(d*Vr_array); %frequency [Hz]

phi = pi/2; %want to make the position in cos form to avoid max vel being
    %at start (would jerk the system)

%length of traveling distance in meters
lengthTank = 9; %[m]

%time of run
tRun = lengthTank/U;


%-----Write the equations that describe the desired motion in each axis----
counter=1;
for f = f_array
    for A = A_array
        
        counter=counter+1;
        motfilename=['motion_' num2str(counter) '.asc'];
        motfile=fopen(motfilename,'w');
      
        num_points=100;
        
        thetao=0;
        T = 1/f;
              
        numloops = round(tRun/T) + 2; %Prev used +(2/T) to give startup time
            %at beginning when motion hasn't started
        timestep=(T/num_points)*1000;   
        timestep = round(timestep);
        
        num_points = (1000/timestep)*T;
        
        w=2*pi*f;
        t=0:timestep/1000:(1/f - (timestep/1000)); 
        
        time_step=timestep;
    
%       %(MIT Small Tank - x axis is controlled separately through LabVIEW. Don't command motion for it here.)
%         x=0*U*t; 
%         vx=0*U*ones(size(x));
        %(Singapore Tank - all axes are controlled through same box. Do command motion for it here.)
        x=1*U*t; 
        vx=1*U*ones(size(x));
        
        y=A*sin(w*t + phi); 
        vy=A*w*cos(w*t + phi);
        
        z=0*U*t;
        vz=0*U*ones(size(z));
        
        lead_in_time = 1; %second

        %%%%%%%%%%%%%%%%%%

        fprintf(fid,'//******************************************\n');
        fprintf(fid,'open prog%2.0f\n',counter);
        fprintf(fid,'clear\n');
        fprintf(fid,'linear             // set to linear positioning mode\n');
        fprintf(fid,'abs                // absolute positioning mode\n');
        fprintf(fid,'ta 500             // set acc time to 500 ms\n');
        fprintf(fid,'ts 500             // set s-curve time to 500 ms\n\n');
        fprintf(fid,'f 1\n');
        fprintf(fid,'//Attention!!! Following points must be same as starting points of motion loop\n');
        fprintf(fid,'X %6.5f Y %6.5f\n',x(1),y(1));
        fprintf(fid,'dwell 0\n\n');
        fprintf(fid,'PVT %5.3f\n',lead_in_time*1000);
        fprintf(fid,'   X   %6.5f   :   %6.5f   Y   %6.5f   :   %6.5f\n',x(1),0,y(1),0);
        fprintf(fid,'   X   %6.5f   :   %6.5f   Y   %6.5f   :   %6.5f\n',x(1),0,y(1),0);
        fprintf(fid,'PVT %5.3f\n',time_step);
        fprintf(fid,'p%2.0f = %2.0f\n',i+10,numloops);
        fprintf(fid,'cmd"endgather"\n');
        fprintf(fid,'cmd"delete gather"\n');
        fprintf(fid,'cmd"define gather"\n');
        fprintf(fid,'cmd"gather"\n');
        fprintf(fid,'m414=1\n');
        fprintf(fid,'While (p%2.0f > 0)\n',i+10);
        fprintf(fid,'   p%2.0f=p%2.0f-1\n',i+10,i+10);
        fprintf(fid,'   // Attention!!! Last point must differ from first point!!!\n');

        for j = 1:length(t)
            fprintf(fid,'   Y   %6.5f   :   %6.5f\n',y(j),vy(j));
            fprintf(motfile,'%6.5f %6.5f\n',y(j),vy(j));
        end;

        fprintf(fid,'EndWhile\n');
        fprintf(fid,'p%2.0f=p100\n',i+10);
        fprintf(fid,'cmd"endgather"\n');
        fprintf(fid,'m414=0\n');
        fprintf(fid,'close\n\n\n');
        fclose(motfile);
        fprintf(motkeyfile,'%3d %6.5f %6.5f %6.5f %6.5f \n', counter,f,A,U,time_step/1000);
    end
end

        
fclose(motkeyfile);
fclose(fid);
%==============================================


%--------------------Plot the generated position and velocity--------------
plot(t,y)
ylim([-1.5 1.5])
title('Y','fontsize', 16)
xlabel('Time (t)', 'fontsize', 16); ylabel('Position (in)', 'fontsize', 16);

figure
plot(t,vy)
ylim([-6, 6])
title('V_y','fontsize', 16)
xlabel('Time (t)','fontsize', 16); ylabel('Velocity (in/s)','fontsize', 16);




