matlab/wheelFFT.m

Go to the documentation of this file.

function [phases, amplitudes] = wheelFFT(encData,steps,amplitudeMax)
% function [phases, amplitudes] = trucksim(fencData,steps,amplitudeMax)
%
% determines phase and amplitudes on fr,fl,br,bl
% returns phases of wheels and amplitudes of wheels
% such that :
%   fr - amplitudes(1) * cos(2*pi*fr/1620 + phases(1)) 
% should not have much periodic disturbances due to 
% the offcentered wheels

% encoder ticks per wheel revolution
ticksPerRevolution = 4*540;

% make sure cols are data for each encoder
[rows,cols] = size(encData)

if rows < cols 
    encData = encData';
    [rows,cols] = size(encData)
end

% make a avg of each row of data
encAvg = sum(encData,2) / cols;


% for each cols (each encoder) :
%  - first determine phase
%  - then determine amplitude

phases = zeros(cols,1);
amplitudes = 2 * ones(cols,1);
results = zeros(cols,steps);


for i = [1:cols]
    for j = [1:steps]
        phase = 2*pi * j/steps;
        y = encData(:,i) - amplitudes(i) * cos(2*pi*encData(:,i)/ticksPerRevolution + phase);        
        results(i,j) = sum(y .* y);
    end
    figure(i)
    hold off;
    plot(2*pi*[1:steps]/steps,results(i,:));
end

[value,index] = min(results');
phases = 2*pi * index/steps;

results2 = zeros(cols,steps);

for i = [1:cols]
    for j = [1:steps]
        amp = amplitudeMax * j/steps;
        y = encData(:,i) - amp * cos(2*pi*encData(:,i)/ticksPerRevolution + phases(i));        
        results2(i,j) = sum(y .* y);
    end
    figure(i+cols)
    hold off;
    plot(amplitudeMax*[1:steps]/steps,results2(i,:));
end

---