00001 % Will load and plot test/line.cpp data file
00002
00003 %get input data from tab deliminated file
00004 data = dlmread('line.dat','\t');
00005 xpos = data(:,1);
00006 ypos = data(:,2);
00007 angle = data(:,3);
00008 encFR = data(:,4);
00009 encFL = data(:,5);
00010 encBR = data(:,6);
00011 encBL = data(:,7);
00012 irFR = data(:,8);
00013 irFL = data(:,9);
00014
00015 len = length(xpos);
00016 time = [0:len-1];
00017
00018 % Use matlab estimator to calculate truck states
00019 [mX, mY, mAngle, mAngleD, mSteeringAngle] = encEstimator(encFR,encFL,encBR,encBL);
00020
00021
00022 % Plot C estimator and matlab (x,y) data
00023 figure(1);
00024 hold off;
00025 plot(xpos,ypos,'ok',xpos,ypos,'-b');
00026 hold on;
00027 plot(mX,mY,'-r');
00028 title('Estimator postion output');
00029 legend('C estimator', 'matlab estimator');
00030 %quiver(xpos,ypos,cos(angle),sin(angle),'r');
00031
00032
00033 % Plot C estimator and matlab angle and steering angle
00034 figure(2);
00035 hold off;
00036 subplot(3,1,1);
00037 title('Truck angle');
00038 plot(time,angle,'r');
00039 hold on;
00040 plot(time,mAngle,'b');
00041 legend('C estimator', 'matlab estimator');
00042
00043 subplot(3,1,2);
00044 title('Differential truck angle');
00045 hold off;
00046 plot(time,mAngleD,'b');
00047 legend('matlab estimator');
00048
00049 subplot(3,1,3);
00050 title('Steering angle');
00051 hold off;
00052 plot(time,mSteeringAngle,'b');
00053 legend('matlab estimator');
00054
00055
00056
00057
00058 % Calculate differential encoder values
00059 encFR_D = encFR(2:len) - encFR(1:len-1);
00060 encFL_D = encFL(2:len) - encFL(1:len-1);
00061 encBR_D = encBR(2:len) - encBR(1:len-1);
00062 encBL_D = encBL(2:len) - encBL(1:len-1);
00063 time2 = [0:len-2];
00064
00065 % plot differential encoder forward movement
00066 % and differential encoder turning
00067 figure(3);
00068 subplot(3,1,1);
00069 hold off;
00070 plot(time2, encFR_D,'-r');
00071 hold on;
00072 plot(time2, encFL_D,'-g');
00073 plot(time2, encBR_D,'-b');
00074 plot(time2, encBL_D,'-c');
00075 title('Differential encoder data');
00076 legend('front right','front left','back right', 'back left');
00077
00078 subplot(3,1,2);
00079 hold off;
00080 plot(time2, encFR_D + encBR_D - encFL_D - encBL_D,'.b');
00081 legend('Turning differential encoder data (fr+br-fl-bl');
00082
00083 subplot(3,1,3);
00084 hold off;
00085 plot(time2, encFR_D + encBR_D + encFL_D + encBL_D,'-r');
00086 legend('Forward encoder data (fr+fr+br+bl)');
00087
00088
00089 %p plot cumulative encoder data
00090 figure(6);
00091 hold off;
00092 plot(time, encFR,'-r');
00093 hold on;
00094 plot(time, encFL,'-g');
00095 plot(time, encBR,'-b');
00096 plot(time, encBL,'-c');
00097 title('Encoder data');
00098 legend('front right','front left','back right', 'back left');
00099
00100
00101 % plot IR sensor data
00102 figure(7);
00103 hold off;
00104 plot(time, irFR,'-r');
00105 hold on;
00106 plot(time, irFL,'-g');
00107 title('IR sensor data');
00108 legend('front right','front left');
