00001 function [phases, amplitudes] = wheelPLL(encData,steps,amplitudeMax)
00002 % function [frP,flP,brP,blP] = trucksim(fr,fl,br,bl)
00003 %
00004 % determines phase and amplitudes on fr,fl,br,bl
00005 % returns phases of wheels and amplitudes of wheels
00006 % such that :
00007 % fr - amplitudes(1) * cos(2*pi*fr/1620 + phases(1))
00008 % should not have much periodic disturbances due to
00009 % the offcentered wheels
00010
00011 % encoder ticks per wheel revolution
00012 ticksPerRevolution = 4*540;
00013
00014 % make sure cols are data for each encoder
00015 [rows,cols] = size(encData)
00016
00017 if rows < cols
00018 encData = encData';
00019 [rows,cols] = size(encData)
00020 end
00021
00022 %
00023 encDot = zeros(rows,cols);
00024 encDot(2:rows,:) = encData(2:rows,:) - encData(1:rows-1,:);
00025
00026 %
00027 n = 100;
00028 f = [0, 0.03, 0.12, 1];
00029 a = [0,1,0];
00030 up = [ 0.01, 1.1, 0.1];
00031 lo = [-0.01, 0.9, -0.1];
00032 h1 = fircls(n,f,a,up,lo);
00033
00034
00035 %n = 50;
00036 %f = 0.10;
00037 %h2 = fir1(n,f);
00038 %fvtool(h2,1)
00039
00040 % filter encDot
00041 b = ones(1,8) / 8;
00042 for i = [1:cols]
00043 encDot(:,i) = filter(h1,1,encDot(:,i));
00044 %x = filter(h2,1,encData(:,i));
00045 figure(i+cols*2)
00046 hold off;
00047 plot(encData(:,i),encDot(:,i),'-r');
00048 hold on;
00049 end
00050
00051
00052
00053
00054
00055
00056 % make a avg of each row of data
00057 encAvg = sum(encData,2) / cols;
00058
00059
00060 % for each cols (each encoder) :
00061 % - first determine phase
00062 % - then determine amplitude
00063
00064 phases = zeros(cols,1);
00065 amplitudes = ones(cols,1);
00066 results = zeros(cols,steps);
00067 results2 = zeros(cols,steps);
00068
00069 for i = [1:cols]
00070 for j = [1:steps]
00071 phase = 2*pi * j/steps;
00072 y = encDot(:,i) - cos(2*pi*encData(:,i)/ticksPerRevolution + phase);
00073 results(i,j) = sum(y .* y);
00074 end
00075 figure(i)
00076 hold off;
00077 plot(2*pi*[1:steps]/steps,results(i,:));
00078 end
00079
00080 [value,index] = min(results');
00081 phases = 2*pi * index/steps;
00082
00083 figure(9);
00084 hold off;
00085 y = cos(2*pi*encData(:,1)/ticksPerRevolution + phases(1));
00086 plot(encData(:,1),encDot(:,1),'-r');
00087 hold on;
00088 plot(encData(:,1),y,'-b');
00089 plot(encData(:,1),encDot(:,1)-y,'-k');
00090 %plot([1:rows],encDot(:,1) .* y,'-k');
00091
00092
00093 for i = [1:cols]
00094 for j = [1:steps]
00095 amp = amplitudeMax * j/steps;
00096 y = encDot(:,i) - amp * cos(2*pi*encData(:,i)/ticksPerRevolution + phases(i) + pi/2);
00097 results2(i,j) = sum(y .* y);
00098 end
00099 figure(i+cols)
00100 hold off;
00101 plot(amplitudeMax*[1:steps]/steps,results2(i,:));
00102 end
00103
