function find_cmd_and_vel( the_data )
% find static commands and corresponding steady state velocities.

% for converting encoder speeds to radians/second
full_revolution = 11*4*64*30;

% spikes in the derivative of the command will tell us when the command changes
diff_command = diff(the_data(:,4));

% low pass filter the velocities to get rid of PicoEncoder noise artifact
[B,A] = butter(2,0.01);
% convert to radians/second
left_vel = filtfilt(B,A,the_data(:,3))/full_revolution;
right_vel = filtfilt(B,A,the_data(:,6))/full_revolution;

% plot stuff
figure(2);
hold off
plot(left_vel);
title( 'left velocity (filtered and radians/second)' );
xlabel( 'samples' )
ylabel( 'radians/second' )
figure(3);
hold off
plot(right_vel);
title( 'right velocity (filtered radians/second)' );
xlabel( 'samples' )
ylabel( 'radians/second' )
figure(4);
hold off
plot(the_data(:,3));
title( 'left velocity (raw)' );
xlabel( 'samples' )
ylabel( 'encoder_counts/second' )
figure(5);
hold off
plot(the_data(:,6));
title( 'right velocity (raw)' );
xlabel( 'samples' )
ylabel( 'encoder_counts/second' )

% back up from the command change - this avoids filter ringing artifact
% https://en.wikipedia.org/wiki/Ringing_artifacts
shift = 250;

% set up a variable that helps us from being fooled by weird stuff
last_jump_index = 0;

% open the file to save the data in (zzz, rename it as you wish)
fid = fopen( 'zzz', 'w' );

for i = 1:length( diff_command )
if abs( diff_command( i ) ) > 0.1 % test for command discontinuity
if ( i - shift ) < 1 % if the shift backwards goes off the start of the
continue; % data
end
if ( i - shift - last_jump_index ) < 900 % real command changes happen
continue; % at least 1 second apart
end
% print data
fprintf( 1, "%8d %4d %14.6f %14.6f\n", i - shift, ...
the_data( i - shift, 4 ), ...
left_vel( i - shift ), right_vel( i - shift ) );
% save data
fprintf( fid, "%8d %4d %14.6f %14.6f\n", i - shift, ...
the_data( i - shift, 4 ), ...
left_vel( i - shift ), right_vel( i - shift ) );
% update this variable
last_jump_index = i - shift;
end
end
if ( i - shift - last_jump_index ) > 900 % real command changes happen
% at least 1 second apart
% print data
fprintf( 1, "%8d %4d %14.6f %14.6f\n", i - shift, ...
the_data( i - shift, 4 ), ...
left_vel( i - shift ), right_vel( i - shift ) );
% save data
fprintf( fid, "%8d %4d %14.6f %14.6f\n", i - shift, ...
the_data( i - shift, 4 ), ...
left_vel( i - shift ), right_vel( i - shift ) );
end
% close the file
fclose( fid );
end