Приложение б

Листинг 1

#include <SPI.h>

const int slaveSelectPin = 8;

const int resetPin = 4;

const int START = 7;

byte res[2];

byte test;

float r[2];

float ToF;

float Result;

int n=0;

int s[4];

int i=0;

int j=0;

int m=0;

int k=0;

int l=0;

int n=0;

float sred=0;

int shagtime=50;

int pos=0;

int flag=0;

unsigned long time1=0;

unsigned long time1_p=0;

unsigned long time2=0;

unsigned long time2_p=0;

boolean pulse=LOW;

int iteration=0;

float data[4][5000];

float scan[5000];

void RESET()

{

digitalWrite(resetPin, LOW);

delay(0.001);

digitalWrite(resetPin, HIGH);

}

void INIT()

{

digitalWrite(slaveSelectPin, LOW);

SPI.transfer(0x70);

digitalWrite(slaveSelectPin, HIGH);

}

void writeReg0()

{

digitalWrite(slaveSelectPin, LOW);

SPI.transfer(0x80);

SPI.transfer(0x00);

SPI.transfer(0x24);

SPI.transfer(0x10);

digitalWrite(slaveSelectPin, HIGH);

}

void writeReg1()

{

digitalWrite(slaveSelectPin, LOW);

SPI.transfer(0x81);

SPI.transfer(0x19);

SPI.transfer(0x49);

SPI.transfer(0x00);

digitalWrite(slaveSelectPin, HIGH);

}

void writeReg2()

{

digitalWrite(slaveSelectPin, LOW);

SPI.transfer(0x82);

SPI.transfer(0xE0);

SPI.transfer(0x00);

SPI.transfer(0x00);

digitalWrite(slaveSelectPin, HIGH);

}

void writeReg3()

{

digitalWrite(slaveSelectPin, LOW);

SPI.transfer(0x83);

SPI.transfer(0x00);

SPI.transfer(0x00);

SPI.transfer(0x00);

digitalWrite(slaveSelectPin, HIGH);

}

void writeReg4()

{

digitalWrite(slaveSelectPin, LOW);

SPI.transfer(0x84);

SPI.transfer(0x20);

SPI.transfer(0x00);

SPI.transfer(0x00);

digitalWrite(slaveSelectPin, HIGH);

}

void writeReg5()

{

digitalWrite(slaveSelectPin, LOW);

SPI.transfer(0x85);

SPI.transfer(0x10);

SPI.transfer(0x00);

SPI.transfer(0x00);

digitalWrite(slaveSelectPin, HIGH);

}

void writeReg6()

{

digitalWrite(slaveSelectPin, LOW);

SPI.transfer(0x86);

SPI.transfer(0x00);

SPI.transfer(0x00);

SPI.transfer(0x00);

digitalWrite(slaveSelectPin, HIGH);

}

void ReadDATA()

{

digitalWrite(slaveSelectPin, LOW);

SPI.transfer(0xB0);

res[1] = SPI.transfer(0x00);

res[0] = SPI.transfer(0x00);

digitalWrite(slaveSelectPin, HIGH);

r[0]=float(res[0]);

r[1]=float(res[1]);

ToF=(r[0]+256*r[1])*0.09;

Result=ToF*0.299792458/2;

data[iteration][k]=Result;

k++;

}

void setup()

{

pinMode (slaveSelectPin, OUTPUT);

pinMode (resetPin, OUTPUT);

pinMode (START, OUTPUT);

digitalWrite(slaveSelectPin, HIGH);

digitalWrite(START, LOW);

RESET();

SPI.begin();

SPI.setDataMode(SPI_MODE1);

SPI.setClockDivider(SPI_CLOCK_DIV8);

SPI.setBitOrder(MSBFIRST);

writeReg0();

writeReg1();

writeReg2();

writeReg3();

writeReg4();

writeReg5();

writeReg6();

attachInterrupt(0, ReadDATA, LOW);

Serial.begin(9600);

s[0]=10;

s[1]=9;

s[2]=6;

s[3]=5;

pinMode(s[0], OUTPUT);

pinMode(s[1], OUTPUT);

pinMode(s[2], OUTPUT);

pinMode(s[3], OUTPUT);

pinMode(3, INPUT);

digitalWrite(s[0], LOW);

digitalWrite(s[1], LOW);

digitalWrite(s[2], LOW);

digitalWrite(s[3], LOW);

delay(1000);

attachInterrupt(1, STARTSHAG, FALLING);

}

void STARTSHAG()

{

s[0]=5;

s[1]=6;

s[2]=9;

s[3]=10;

flag=1;

pos=0;

}

void ROTATE1()

{

time1=millis();

if(time1-time1_p>=shagtime)

{

time1_p=time1;

digitalWrite(s[i], HIGH);

digitalWrite(s[j], HIGH);

for (m=0; m<4; m++)

{

if(m!=i && m!=j)

{

digitalWrite(s[m], LOW);

}

}

if(j==i)

{

j=(i+1)%4;

}

else

{

i=(i+1)%4;

}

if (flag==1)

{

pos++;

}

}

}

void LaserPulse()

{

time2=micros();

if(time2-time2_p>=125)

{

time2_p=time2;

pulse=~pulse;

digitalWrite(START, pulse);

}

}

void loop()

{

if(iteration<4)

{

ROTATE1();

if (pos==156)

{

s[0]=10;

s[1]=9;

s[2]=6;

s[3]=5;

flag=0;

iteration++;

}

if(pos>=28&pos<=128)

{

LaserPulse();

}

}

else

{

for(l=0, l<5000, l++)

{

for(n=0, n<4, n++)

{

sred+=(data[n][l])/4;

}

scan[l]=sred;

sred=0;

}

for(l=0, l<5000, l++)

{

Serial.println(scan[l]);

}

}

}

Листинг 2

clear;

clc;

A1=zeros(800,800);

A2=zeros(800,800);

alfa=pi/4:pi/2000:3*pi/4;

r1=[load('scandata1.txt')];

r2=[load('scandata2.txt')];

for i=1:1000

x1(i)=round(r1(i).*cos(alfa(i)))+400;

y1(i)=round(r1(i).*sin(alfa(i)));

x2(i)=round(r2(i).*cos(alfa(i)))+400;

y2(i)=round(r2(i).*sin(alfa(i)));

end

for j=1:1000

A1(y1(1, j),x1(1, j))=1;

A2(y2(1, j),x2(1, j))=1;

end

Image1=im2bw(100*A1);

Image2=im2bw(100*A2);

Image1=~Image1;

Image2=~Image2;

imwrite(Image1, '1scan.jpg');

imwrite(Image2, '2scan.jpg');

I1 = imread('1scan.jpg');

BW1 = edge(I1,'canny');

[H1,T1,R1] = hough(BW1);

P1= houghpeaks(H1,2,'threshold',ceil(0.1*max(H1(:))));

p1 = T1(P1(:,2)); theta1 = R1(P1(:,1));

I2 = imread('2scan.jpg');

BW2 = edge(I2,'canny');

[H2,T2,R2] = hough(BW2);

P2= houghpeaks(H2,2,'threshold',ceil(0.1*max(H2(:))));

p2 = T2(P2(:,2)); theta2 = R2(P2(:,1));

L(1,1)=p1(1);

L(1,2)=p1(2);

L(2,1)=p2(1);

L(2,2)=p2(2);

F(1,1)=(pi*theta1(1)/180);

F(1,2)=(pi*theta1(2)/180);

F(2,1)=(pi*theta2(1)/180);

F(2,2)=(pi*theta2(2)/180);

fi(1)=atan((L(1,1)*cos(F(1,2))-L(1,2)*cos(F(1,1)))/(L(1,2)*sin(F(1,1))-L(1,1)*sin(F(1,2))));

fi(2)=atan((L(2,1)*cos(F(2,2))-L(2,2)*cos(F(2,1)))/(L(2,2)*sin(F(2,1))-L(2,1)*sin(F(2,2))));

ro(1)=((L(1,1)/cos(F(1,1)))+(L(1,2)/cos(F(1,2))))/2;

ro(2)=((L(2,1)/cos(F(2,1)))+(L(2,2)/cos(F(2,2))))/2;

dx=ro(2)*cos(fi(2))-ro(1)*cos(fi(1));

dy=ro(1)*sin(fi(1))-ro(2)*sin(fi(2));

dfi=(((F(2,1)-F(1,1))+(F(2,2)-F(1,2)))/2)*180/pi;

140