โปรเจค หุ่นยนต์หลบสิ่งกีดขวาง Arduino UNO + HY-SRF05

const unsigned int TRIG_PIN=7;

const unsigned int ECHO_PIN=6;

const unsigned int BAUD_RATE=9600;



void setup() {

  pinMode(TRIG_PIN, OUTPUT);

  pinMode(ECHO_PIN, INPUT);

  Serial.begin(BAUD_RATE);

}



void loop() {

  digitalWrite(TRIG_PIN, LOW);

  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);

  delayMicroseconds(10);

  digitalWrite(TRIG_PIN, LOW);

  

 const unsigned long duration= pulseIn(ECHO_PIN, HIGH);

 int distance= duration/29/2;

 if(duration==0){

   Serial.println("Warning: no pulse from sensor");

   } 

  else{

      Serial.print("distance to nearest object:");

      Serial.println(distance);

      Serial.println(" cm");

  }

 delay(100);


 }




ไปที่ Tools > Board เลือกเป็น Arduino/Genuino UNO

ไปที่ Tools > Port แล้วเลือกพอร์ตที่ปรากฏ (กรณีใช้เครื่องคอมพิวเตอร์ที่มี COM Port มากกว่าหนึ่ง  ให้เลือกตัวอื่นที่ไม่ใช่ COM1)

ในตัวอย่างเลือกเป็น "COM14"

(ถ้ายังว่าง หรือ เป็น COM1 ให้ตรวจสอบการติดตั้งไดร์เวอร์ การต่อสาย USB ของ Arduino UNO)

(ถ้าไม่พบให้ทำการติดตั้งไดร์เวอร์ก่อน) 
การติดตั้งโปรแกรม Arduino (IDE) และ การติดตั้งไดร์เวอร์




หากสามารถอัพโหลดโปรแกรมลงบอร์ดได้สำเร็จ จะแสดงคำว่า Done uploading. ที่แถบด้านล่าง

เปิดสวิตช์ไฟ จากรางถ่านเข้าสู่หุ่นยนต์

แล้วไปที่ Tools -> Serial Monitor


Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3




/*

    Obstacle Avoiding Robot with Arduino UNO + HY-SRF05
    For more details visit:
    https://robotsiam.blogspot.com/2017/12/arduino-uno-hy-srf05.html

*/

/*-------definning SRF05------*/

const unsigned int TRIG_PIN = 7;
const unsigned int ECHO_PIN = 6;
const unsigned int BAUD_RATE = 9600;

/*-------definning Outputs------*/

int MA1 = 12;     // Motor A1
int MA2 =  3;     // Motor A2
int PWM_A =  10;   // Speed Motor A

int MB1 =  13;     // Motor B1
int MB2 =  8;     // Motor B2
int PWM_B =  11;  // Speed Motor B

int SPEED = 200;  // Speed PWM สามารถปรับความเร็วได้ถึง 0 - 255

void setup() {

  pinMode(4, OUTPUT); //Buzzer

  //Setup Channel A
  pinMode(12, OUTPUT); //Motor A1
  pinMode(3, OUTPUT); //Motor A2
  pinMode(10, OUTPUT); //Speed PWM Motor A

  //Setup Channel B
  pinMode(13, OUTPUT);  //Motor B1
  pinMode(8, OUTPUT);  //Motor B2
  pinMode(11, OUTPUT); //Speed PWM Motor B

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  Serial.begin(BAUD_RATE);

}

void loop() {

  digitalWrite(TRIG_PIN, LOW);

  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);

  delayMicroseconds(10);

  digitalWrite(TRIG_PIN, LOW);



  const unsigned long duration = pulseIn(ECHO_PIN, HIGH);

  int distance = duration / 29 / 2;

  if ((distance > 1) && (distance < 20))  {

    digitalWrite(4, HIGH);
    Stop(500);
    digitalWrite(4, LOW);
    Backward(400);
    turnLeft(400);
    Serial.print("distance to nearest object:");
    Serial.println(distance);
    Serial.println(" cm");

  }

  else {

    digitalWrite(4, LOW);
    Forward(10);
    Serial.print("distance to nearest object:");
    Serial.println(distance);
    Serial.println(" cm");

  }

  delay(100);

}

void Backward(int time)
{
  digitalWrite(MA1, LOW);
  digitalWrite(MA2, HIGH);
  analogWrite(PWM_A, SPEED);

  digitalWrite(MB1, HIGH);
  digitalWrite(MB2, LOW);
  analogWrite(PWM_B, SPEED);

  delay(time);
}

void Forward (int time)
{
  digitalWrite(MA1, HIGH);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, SPEED);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, HIGH);
  analogWrite(PWM_B, SPEED);

  delay(time);
}

void turnLeft(int time)
{
  digitalWrite(MA1, HIGH);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, SPEED);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, LOW);
  analogWrite(PWM_B, 0);

  delay(time);
}

void turnRight(int time)
{
  digitalWrite(MA1, LOW);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, 0);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, HIGH);
  analogWrite(PWM_B, SPEED);

  delay(time);
}

void Stop(int time)
{
  digitalWrite(MA1, LOW);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, 0);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, LOW);
  analogWrite(PWM_B, 0);

  delay(time);

}




วีดีโอผลลัพธ์ หุ่นยนต์หลบสิ่งกีดขวาง Arduino UNO + HY-SRF05