ทดลองทำหุ่นยนต์ซูโม่

const int IN1 = 7; const int IN2 = 6; const int IN3 = 5; const int IN4 = 4; const int ENA = 9; const int ENB = 3;
const int senserR = A0; const int senserL = A5; int sensorValueL = 0; int sensorValueR = 0;
//——————————————-
const int trig = 12; //ประกาศขา trig
const int echo = 13; //ประกาศขา echo
long duration, distance; //ประกาศตัวแปรเก็บค่าระยะ
void setup() {
Serial.begin(9600);
pinMode (IN1, OUTPUT);
pinMode (IN2, OUTPUT);
pinMode (IN3, OUTPUT);
pinMode (IN4, OUTPUT);
pinMode (ENA, OUTPUT);
pinMode (ENB, OUTPUT);
pinMode(echo, INPUT); //สั่งให้ขา echo ใช้งานเป็น input
pinMode(trig, OUTPUT); //สั่งให้ขา trig ใช้งานเป็น output
}
void FD() {//ฟังก์ชันเดินหน้า
analogWrite(ENA, 255);//ล้อซ้าย
analogWrite(ENB, 255);//ล้อขวา
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void BK() {//ฟังก์ชันถอยหลัง
analogWrite(ENA, 255);//ล้อซ้าย
analogWrite(ENB, 255);//ล้อขวา
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void stoprobot() {//ฟังก์ชันหยุด
analogWrite(ENA, 0);//ล้อซ้าย
analogWrite(ENB, 0);//ล้อขวา
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void survey() {//ฟังก์ชันสำรวจ
analogWrite(ENA, 50);//ล้อซ้าย
analogWrite(ENB, 100);//ล้อขวา
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void SL() {//ฟังก์ชันหมุนซ้าย
analogWrite(ENA, 255);//ล้อซ้าย
analogWrite(ENB, 255);//ล้อขวา
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void loop() {
digitalWrite(trig, LOW);
delayMicroseconds(1);
digitalWrite(trig, HIGH);
delayMicroseconds(1);
digitalWrite(trig, LOW); //ใช้งานขา trig
duration = pulseIn(echo, HIGH); //อ่านค่าของ echo
distance = (duration / 2) / 29.1; //คำนวณเป็น centimeters
//——————————-
sensorValueL = analogRead(senserL); sensorValueR = analogRead(senserR);
Serial.print(distance); Serial.println(” cm\n”);
Serial.print(“sensorL = “); Serial.print(sensorValueL); Serial.print(” sensorR = “); Serial.println(sensorValueR);
if (sensorValueL > 500 && sensorValueR > 500) {
Serial.print(“survey!”);
survey();
}
if (sensorValueL <= 500 && sensorValueR <= 500) { Serial.print(“BK!”); BK(); } if (sensorValueR < 500 && sensorValueL > 500) {
BK();
delay(1500);
SL();
delay(1000);
}
if(distance<=30){
FD();
}
delay(2);

}