%
% Test the triangle code
%
  t=[0:500]'/10;
  cmd_prd = 25;
  Mag = 100;

  for i = 1:501,
     modTime = mod(t(i),cmd_prd);
     if(modTime <  cmd_prd/4.) 
         thrust(i) = (4.0 * Mag * modTime)/cmd_prd;
         disp 'First Quarter'; 
     elseif ( (modTime >=  cmd_prd/4.) & (modTime < cmd_prd*3./4.) )
         thrust(i) =  (-4.0 * Mag * modTime)/cmd_prd + 2.0*Mag;
         disp 'Middle half'
     else 
         thrust(i) =  ( 4.0 * Mag * modTime)/cmd_prd - 4.0*Mag;
         disp 'Last quarter'
     end
  end

  figure(1)
  clf
  plot(t,thrust)
