สร้างหุ่นยนต์ด้วย Arduino ตอนที่ 1 ฝึกให้หุ่นยนต์หลบหลีกสิ่งกีดขวาง

วันนี้ก็จะเริ่มทดลองใช้ Arduino รุ่น UNO R3 MEGA328P เป็นตัวประมวลผล, Servo ในส่วนเคลื่อนที่ซ้าย-ขวา และ Ping Ultrasonic Sensor (HC-SR04) เป็นตัว Sensor ว่าหุ่นยนต์อยู่ในระยะไหนจากวัตถุ  ผลออกมาเป็นที่น่าพอใจ

#define echoPin 7 // Echo Pin
#define trigPin 8 // Trigger Pin
#define LEDPin 13 // Onboard LED

#include <servo.h> 

 
Servo myservo;  // create servo object to control a servo 
                // a maximum of eight servo objects can be created 
long duration, inches, cm,checkl;
int pos = 0;    // variable to store the servo position 

int R_Angle = 0;
int C_Angle = 90;
int L_Angle = 180;  
 
// value names used to hold antennae state
int side_R_val; 
int side_L_val; 
int side_C_val; 
 
void setup() 
{ 
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(LEDPin, OUTPUT); // Use LED indicator (if required)
} 
 
 
void loop() 
{   
    
  checkl = microsecondsToCentimeters();
  
  if (checkl<=10){
    stop_motors();
    
    myservo.write(L_Angle); 
    delay(500);
    side_L_val = microsecondsToCentimeters();
    Serial.print(side_L_val);
    Serial.println();
    delay(500);
    
    myservo.write(C_Angle);
    delay(500);
    side_C_val = microsecondsToCentimeters();
    Serial.print(side_C_val);
    Serial.println();
    delay(500);
    
    myservo.write(R_Angle); 
    delay(500);
    side_R_val = microsecondsToCentimeters();
    Serial.print(side_R_val);
    Serial.println();
    delay(500);
    
    if (side_L_val > side_R_val){
      myservo.write(C_Angle);
      turn_left();
      delay(2000);
    }else{  
      myservo.write(C_Angle);
      turn_right();
      delay(2000);
    }
    
  }else
  {
    forward_motors();
  }
  }

// Beginning motor control functions 
 
void stop_motors(){ 
  Serial.print("stop motors");
  Serial.println();
}

void turn_right(){ 
  Serial.print("turn right");
  Serial.println();
}

void turn_left(){ 
  Serial.print("turn left");
  Serial.println();
}

void reverse_motors(){ 
  Serial.print("reverse motors");
  Serial.println();
}

void forward_motors(){
  Serial.print("forward motors");
  Serial.println();
}


long microsecondsToCentimeters()
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  
    /* The following trigPin/echoPin cycle is used to determine the
 distance of the nearest object by bouncing soundwaves off of it. */ 

 digitalWrite(trigPin, LOW); 
 delayMicroseconds(2); 

 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10); 
 
 digitalWrite(trigPin, LOW);
 duration = pulseIn(echoPin, HIGH);
 

 return duration / 29 / 2;;
}