
% This function/script/whatever-it-is will generate a trajectory
% file for the ArbTrajectory Module in McGillLib/Modules (recently
% moved to McGillLib/controllers)

% first thing to do is get the desired cycle time
%cycles = floor(input( 'Enter Desired Number of steps in cycle: '));
cycles = 1300;
disp(['Cycle time is ', int2str(cycles), ' ms']);

frontKp = 6;
frontKd =  0.06;
midKp = 15;
midKd =  0.15;
rearKp = 22;
rearKd =  0.22;

% build up the arrays
front_angles = zeros(1, cycles);
mid_angles   = zeros(1, cycles);
rear_angles  = zeros(1, cycles);

time = 0:0.001:(cycles-1)/1000;

%% for testing purposes, try a sinusoid trajectory
%front_angles = 4*sin (2*pi/max(time)*time);
%mid_angles = 4*sin (2*pi/max(time)*time+1/3*pi);
%rear_angles = 4*sin (2*pi/max(time)*time+2/3*pi);

% Existing downstairs controller
key_times = [1 201 351 551 701 1001];
front_angles(key_times) = ...
    [45 45 45 45 45 45] ./ 180 .* pi;
mid_angles(key_times) = ...
    [-10 33.75 42.5 95 -40+360 -20+360] ./ 180 .* pi;
rear_angles(key_times) = ...
    [20 -145+360 -28.2+360 -11+360 -0.7+360 15+360] ./ 180 .* pi;

% interpolate between points
timp = length(key_times);
for k=1:timp-1
  angle_d_f = (front_angles(key_times(k+1)) - front_angles(key_times(k))) / ...
      ( key_times(k+1) - key_times(k) );
  angle_d_m = (mid_angles(key_times(k+1)) - mid_angles(key_times(k))) / ...
      ( key_times(k+1) - key_times(k) );
  angle_d_r = (rear_angles(key_times(k+1)) - rear_angles(key_times(k))) / ...
      ( key_times(k+1) - key_times(k) );
      
  for kk=(key_times(k)+1):(key_times(k+1)-1)
    front_angles(kk) = front_angles(kk-1) + angle_d_f;
    mid_angles(kk)   = mid_angles(kk-1) + angle_d_m;
    rear_angles(kk)  = rear_angles(kk-1) + angle_d_r;
  end
end

angle_d_f = (front_angles(1) - front_angles(key_times(timp))) / ...
    ( cycles + 1 - key_times(timp) );
angle_d_m = (mid_angles(1)+(2*pi) - mid_angles(key_times(timp))) / ...
    ( cycles + 1 - key_times(timp) );
angle_d_r = (rear_angles(1)+(2*pi) - rear_angles(key_times(timp))) / ...
    ( cycles + 1 - key_times(timp) );
  
for kk=(key_times(timp)+1):cycles
    front_angles(kk) = front_angles(kk-1) + angle_d_f;
    mid_angles(kk)   = mid_angles(kk-1) + angle_d_m;
    rear_angles(kk)  = rear_angles(kk-1) + angle_d_r;
end

% plot the trajectories
close all

figure; h1 = gcf;
plot(time, front_angles);
hold on;plot(time(key_times), front_angles(key_times), '*');
%axis([0 max(time) -pi pi]);

figure; h2 = gcf;
plot(time, mid_angles);
hold on;plot(time(key_times), mid_angles(key_times), '*');
%axis([0 max(time) -pi pi]);

figure; h3 = gcf;
plot(time, rear_angles);
hold on;plot(time(key_times), rear_angles(key_times), '*');
%axis([0 max(time) -pi pi]);

figure; h4 = gcf;
hold on;
plot(time, front_angles, 'b');
plot(time, mid_angles, 'r');
plot(time, rear_angles, 'm');
%axis([0 max(time) -pi pi]);
hold off;

%%%% File output
output_file = 'ArbTrajectory.rc';

fd = fopen( output_file, 'w');
fprintf(fd, '%s\n', '## configuration file generated by MATLAB');
fprintf(fd, '%s\n\n', '###### DO NOT MODIFY #####');

fprintf(fd, '%s\n', '# Gains');
fprintf(fd, '%s %.2f%c\n', 'float arb_frontKp =', frontKp, ';');
fprintf(fd, '%s %.2f%c\n', 'float arb_frontKd =', frontKd, ';');
fprintf(fd, '%s %.2f%c\n', 'float arb_midKp =', midKp, ';');
fprintf(fd, '%s %.2f%c\n', 'float arb_midKd =', midKd, ';');
fprintf(fd, '%s %.2f%c\n', 'float arb_rearKp =', rearKp, ';');
fprintf(fd, '%s %.2f%c\n\n', 'float arb_rearKd =', rearKd, ';');

fprintf(fd, '%s %d%c\n\n', 'float arb_points =', cycles, ';');

%% output with 3 decimals gives 0.06 deg resolution
fprintf(fd, '%s', 'float arb_front_angles =');
fprintf(fd, ' %.3f', front_angles);
fprintf(fd, '%c\n\n', ';');

fprintf(fd, '%s', 'float arb_mid_angles =');
fprintf(fd, ' %.3f', mid_angles);
fprintf(fd, '%c\n\n', ';');

fprintf(fd, '%s', 'float arb_rear_angles =');
fprintf(fd, ' %.3f', rear_angles);
fprintf(fd, '%c\n\n', ';');

fclose(fd);