simulation.asv

Ultrasonor captor with Microchip dsPIC33F

clc;
clear all;
close all;

disp('*****************************************');
disp('***     MATLAB - SIMULATION V0.1      ***');
disp('*****************************************');

tic;
%% Constantes
largeur = 2.10;
Longueur = 3.00;
vitesse_son = 340;
f_ech = 123.077e3;
longueur_sbpa = 1023;
pas = 0.01;
seuil = 0.01;

load donnee.mat

        
%% Param鑤res
x_robot = 1.0;  %en m max 3.00    milieu de terrain : 1.50
y_robot = 1.0;  %en m max 2.10    milieu de terrain : 1.05

%% Coordonn閑s des balises
B1 = [0 largeur/2];
B2 = [Longueur 0];
B3 = [Longueur largeur];

%% Calcul des distances
dist(1) = sqrt((x_robot - B1(1))^2 + (y_robot - B1(2))^2);
dist(2) = sqrt((x_robot - B2(1))^2 + (y_robot - B2(2))^2);
dist(3) = sqrt((x_robot - B3(1))^2 + (y_robot - B3(2))^2);

%% Calcul d閜hasage sur les balises en 閏hantillons -> pour la cr閍tion des
%% signaux
deph_ech = round(dist/vitesse_son*f_ech);
if max(deph_ech) > 2*longueur_sbpa
deph_ech = deph_ech - min(deph_ech);
end
deph_ech(find(deph_ech > 2*longueur_sbpa - 1)) = 1022;

%% Calcul des d閏alages temporels -> pour comparaison apr鑣 corr閘ation
dec_temps(1) = (dist(1)/vitesse_son) - (dist(2)/vitesse_son);    % 1-2
dec_temps(2) = (dist(2)/vitesse_son) - (dist(3)/vitesse_son);    % 2-3
dec_temps(3) = (dist(3)/vitesse_son) - (dist(1)/vitesse_son);    % 3-1

%% Calcul des d閏alages en nombre d'閏hantillons -> pour comparaison apr鑣
%% corr閘ation
dec_ech = round(dec_temps*f_ech);

%% Calcul des diff閞ences de distances
dist_cal = dec_temps*vitesse_son;
temps_calcul(1) = toc;

%         %% Affichage
s = sprintf('Donn閑s : ');
disp(s);
s = sprintf('Coordonn閑s du robot : %g ; %g', x_robot,y_robot);
disp(s);
s = sprintf('Diff閞ences de distances : \n\t d1-d2 : %g m ; \n\t d2-d3 : %g m ; \n\t d3-d1 : %g m ;', dist_cal(1), dist_cal(2), dist_cal(3));
disp(s);
s = sprintf('Diff閞ences temporelles : \n\t t1-t2 = %g ms ; \n\t t2-t3 = %g ms ; \n\t t3-t1 = %g ms ;', dec_temps(1)*1e3,dec_temps(2)*1e3,dec_temps(3)*1e3);
disp(s);
s = sprintf('Diff閞ences d''閏hantillons : \n\t B1-B2 : %g 閏hantillons ; \n\t B2-B3 : %g 閏hantillons ; \n\t B3-B1 : %g 閏hantillons ;', dec_ech(1), dec_ech(2), dec_ech(3));
disp(s);
disp('.');
disp('*****************************************');
disp('SIMULATION');
disp('*****************************************');

%% Cr閍tion des signaux ultrasons
tic;
%sim ech 1023 points entre 0 et 1023 d'amplitude
ech=randn([2047 1])*1023;
ech = ech -mean(ech);

%ech = ech - 512;
modele = ech(1:2*longueur_sbpa)';
size_modele = length(modele);

signal(1,1:deph_ech(1)+1) = modele(size_modele-deph_ech(1):size_modele);
signal(1,deph_ech(1)+2:size_modele) = modele(1:size_modele-deph_ech(1)-1);

signal(2,1:deph_ech(2)+1) = modele(size_modele-deph_ech(2):size_modele);
signal(2,deph_ech(2)+2:size_modele) = modele(1:size_modele-deph_ech(2)-1);

signal(3,1:deph_ech(3)+1) = modele(size_modele-deph_ech(3):size_modele);
signal(3,deph_ech(3)+2:size_modele) = modele(1:size_modele-deph_ech(3)-1);

temps_calcul(2) = toc;

%% Affichage des signaux re鐄s par les balises :
figure('Name', 'Signaux ultrasons re鐄s par les balises', 'NumberTitle','off');
subplot(4,1,1);
plot(modele);
title('Signal 閙it par le robot');
subplot(4,1,2);
plot(signal(1,:));
title('Balise 1');
subplot(4,1,3);
plot(signal(2,:));
title('Balise 2');
subplot(4,1,4);
plot(signal(3,:));
title('Balise 3');

tic;
%% Corr閘ation des signaux
correlation(1,:)=xcorr(signal(1,:), signal(2,:));     %1 - 2
correlation(2,:)=xcorr(signal(2,:), signal(3,:));     %2 - 3
correlation(3,:)=xcorr(signal(3,:), signal(1,:));     %3 - 1

%% Recherche du maximum de corr閘ation
[m idx] = max(correlation(1,:));
dec_ech_corr(1) = idx - size_modele;
[m idx] = max(correlation(2,:));
dec_ech_corr(2) = idx - size_modele;
[m idx] = max(correlation(3,:));
dec_ech_corr(3) = idx - size_modele;

%% Calcul des diff閞ences de temps
temps_corr = dec_ech_corr/f_ech;

%% Calcul des diff閞ences de distances
dist_corr = temps_corr*vitesse_son;

temps_calcul(3) = toc;

%% Calcul d'erreur
err_temps = abs((temps_corr-dec_temps)./dec_temps)*100;

%% Calcul d'erreur
err_dist = abs((dist_corr-dist_cal)./dist_cal)*100;

tic;
A = dist_corr(1);
B = dist_corr(2);
C = dist_corr(3);

%% Cr閍tion de la table de coordonn閑s (A_table, B_table et C_table)
bar = waitbar(0,'Calcul position en cours...');
k=0;
for i=[0:pas:Longueur]
    k=k+1;
    l=0;
    for j=[0:pas:largeur]
        waitbar(((i)*largeur/pas+(j))/(Longueur*largeur/pas+largeur));
        l=l+1;
        A_table(k,l) = 1/C*(-2*(Longueur)*i-(largeur)*j+(Longueur)^2+3*(largeur)^2/4)-1/B*(2*(largeur)*j-(largeur)^2);
        B_table(k,l) = 1/A*(-(largeur)*j+(largeur)^2/4+2*(Longueur)*i-(Longueur)^2)-1/C*(-2*(Longueur)*i+(Longueur)^2+3*(largeur)^2/4-(largeur)*j);
        C_table(k,l) = 1/B*(2*(largeur)*j-(largeur)^2)-1/A*(-(largeur)*j+(largeur)^2/4+2*(Longueur)*i-(Longueur)^2);
    end
end
close(bar);
temps_calcul(4) = toc;

tic;
x_boucle_cal = 0;
y_boucle_cal = 0;

%% Calcul coordonn閑s simulation
err1=abs(A_table-A*ones(size(A_table)));
err2=abs(B_table-B*ones(size(B_table)));
err3=abs(C_table-C*ones(size(C_table)));
err=err1+err2+err3;
[x_boucle_cal, y_boucle_cal] = find(err == min(min(err)));
y_boucle_cal = (y_boucle_cal-1)*pas;
x_boucle_cal = (x_boucle_cal-1)*pas;

temps_calcul(5) = toc;


figure;
mesh(err);

mesh(log(err));

%% Affichage r閟ultat
disp('.');
s = sprintf('Calcul : ');
disp(s);
s = sprintf('Diff閞ences d''閏hantillons : \n\t B1-B2 : %g 閏hantillons ; \n\t B2-B3 : %g 閏hantillons ; \n\t B3-B1 : %g 閏hantillons ;', dec_ech_corr(1), dec_ech_corr(2), dec_ech_corr(3));
disp(s);
s = sprintf('Diff閞ences temporelles : \n\t t1-t2 = %g ms ; \n\t t2-t3 = %g ms ; \n\t t3-t1 = %g ms ;', temps_corr(1)*1e3,temps_corr(2)*1e3,temps_corr(3)*1e3);
disp(s);
s = sprintf('\t Erreur temporelle max : %g %% ; %g ms', max(err_temps), max(err_temps)*10);
disp(s);
s = sprintf('Diff閞ences de distances : \n\t d1-d2 : %g m ; \n\t d2-d3 : %g m ; \n\t d3-d1 : %g m ;', temps_corr(1)*vitesse_son, temps_corr(2)*vitesse_son, temps_corr(3)*vitesse_son);
disp(s);
s = sprintf('\t Erreur de distance max : %g %% ; %g mm', max(err_dist), max(err_dist)*10);
disp(s);
s = sprintf('Coordonn閑s du robot calcul閑s simul閑s: %g ; %g', x_boucle_cal, y_boucle_cal);
disp(s);

%% Affichage des corr閘ations
% x_corr = [-size_modele+1:size_modele-1];
% figure('Name', 'Convolutions', 'NumberTitle','off');
% subplot(3,1,1);
% plot(x_corr,correlation(1,:));
% title('Corr閘ation de B1 et B2');
% subplot(3,1,2);
% plot(x_corr,correlation(2,:));
% title('Corr閘ation de B2 et B3');
% subplot(3,1,3);
% plot(x_corr,correlation(3,:));
% title('Corr閘ation de B3 et B1');




%% %%%%%%%%%%%%%%%%%%%%%%%%%