Tải bản đầy đủ - 0 (trang)
PHỤ LỤC 6: Code Arduino di chuyển đến điểm cần đo độ cao mặt đồng.

PHỤ LỤC 6: Code Arduino di chuyển đến điểm cần đo độ cao mặt đồng.

Tải bản đầy đủ - 0trang

bool QuaytraiOK;

long newLeft, newRight;



void Dithang()

{

DithangOK = false;

bool step1OK = false;

bool step2OK = false;

analogWrite(INE1,speedDC);

analogWrite(INE2,0);

analogWrite(INE3,speedDC);

analogWrite(INE4,0);

knobLeft.write(0);

knobRight.write(0);

while(!DithangOK)

{

newLeft = knobLeft.read();

newRight = knobRight.read();

if(abs(newLeft)>=(xungpermeter*meter))

{

analogWrite(INE1,0);

analogWrite(INE2,0);

step1OK = true;

}

if(abs(newRight)>=(xungpermeter*meter))

67



{

analogWrite(INE3,0);

analogWrite(INE4,0);

step2OK = true;

}

if((step1OK&step2OK) == true)

{

DithangOK = true;

Serial.println(newLeft);

Serial.println(newRight);



} }}



void Quayphai()

{ QuayphaiOK = false;

bool step1OK = false;

bool step2OK = false;

analogWrite(INE1,speedDC);

analogWrite(INE2,0);

analogWrite(INE3,0);

analogWrite(INE4,speedDC);

knobLeft.write(0);

knobRight.write(0);

while(!QuayphaiOK)

{ newLeft = knobLeft.read();

newRight = knobRight.read();

if(abs(newLeft)>=xungperAngle)

{ analogWrite(INE1,0);

68



analogWrite(INE2,0);

step1OK = true;

}

if(abs(newRight)>=xungperAngle)

{

analogWrite(INE3,0);

analogWrite(INE4,0);

step2OK = true;

}

if((step1OK&step2OK) == true)

{

Serial.println(newLeft);

Serial.println(newRight);

QuayphaiOK = true; } }}

void Quaytrai()

{ QuaytraiOK = false;

bool step1OK = false;

bool step2OK = false;

analogWrite(INE1,0);

analogWrite(INE2,speedDC);

analogWrite(INE3,speedDC);

analogWrite(INE4,0);

knobLeft.write(0);

knobRight.write(0);

while(!QuaytraiOK)

69



{

newLeft = knobLeft.read();

newRight = knobRight.read();

if(abs(newLeft)>=xungperAngle)

{

analogWrite(INE1,0);

analogWrite(INE2,0);

step1OK = true;

}

if(abs(newRight)>=xungperAngle)

{

analogWrite(INE3,0);

analogWrite(INE4,0);

step2OK = true;

}

if((step1OK&step2OK) == true)

{

Serial.println(newLeft);

Serial.println(newRight);

QuaytraiOK = true;

}

}

}



70



PHỤ LỤC 7: Code Arduino xử lý điểm cần đo.

void ProcessPoint()

{ switch (point)

{



case 0:

MPU6050();

Laser_Processing();

Serial2.print("P00");

Serial2.print(numlaser);

Serial2.print(",");

break;

case 1:

Dithang();

MPU6050();

Laser_Processing();

Serial2.print("P01");

Serial2.print(numlaser);

Serial2.print(",");

break;

case 2:

Dithang();

MPU6050();

Laser_Processing();

Serial2.print("P02");

Serial2.print(numlaser);

71



Serial2.print(",");

break;

case 3:

Dithang();

MPU6050();

Laser_Processing();

Serial2.print("P03");

Serial2.print(numlaser);

Serial2.print(",");

break;

case 4:

Dithang();

MPU6050();

Laser_Processing();

Serial2.print("P04");

Serial2.print(numlaser);

Serial2.print(",");

break;

case 5:

Dithang();

delay(1000);

Quayphai();

MPU6050();

Laser_Processing();

Serial2.print("P05");

72



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

PHỤ LỤC 6: Code Arduino di chuyển đến điểm cần đo độ cao mặt đồng.

Tải bản đầy đủ ngay(0 tr)

×