บทความนี้เป็นตัวอย่างการนำหุ่นยนต์รถที่ขับเคลื่อนด้วยมอเตอร์แบบเซอร์โวดังที่กล่าวไปในบทความก่อนหน้านี้มาใช้งาน โดยในบทความนี้ใช้การเคลื่อนที่ด้วยการกำหนดกฎการเคลื่อนที่ภายใต้การตัดสินใจเรื่องระยะทางที่อ่านได้จากเซ็นเซอร์วัดระยะทางอย่าง Ultrasonic Sensor

ภาพที่ 1

อุปกรณ์

อุปกรณ์ในการทดลองครั้งนี้ประกอบด้วย

  1. ESP8266
  2. R-Base ของบ.อีทีที ซึ่งเป็นชุดโครงตัวรถ ทำด้วย ALUMINUM หนา 3 มม.
  3. เซอร์โวจำนวน 2 ตัว แบบที่สามารถหมุนได้ 360 องศา
  4. ล้อติดกับเซอร์โว หรือใช้ชุดตีนตะขาบ (R-TANK  V1 KIT)
  5. ล้อพยุง
  6. แหล่งจ่ายไฟ (ผู้เขียนใช้ Power Bank)
  7. เซ็นเซอร์อัลตราโซนิก (HC-SR04 SENSOR)
  8. เซอร์โว 1 ตัว

เซ็นเซอร์ระยะทาง

การใช้เซ็นเซอร์วัดระยะทางประเภท Ultrasonic หรือ HC-SR04 SENSOR มีหลักการทำงานคือ ส่งพัลซ์ไปทางขา Trig หลังจากนั้นรอรับการสะท้อนกลับมายังขา Echo หลังจากนั้นคำนวณระยะทางจากเวลาตั้งแต่เริ่มสะท้อนจนได้รับเสียงสะท้อนดังฟังก์ชัน distance

def distance():
    echoPin.value()
    trigPin.on()
    trigPin.off()
    usec = mc.time_pulse_us( echoPin, 1 )
    echoTime = usec/1000000
    distanceCm = (echoTime/2)*34300
    return distanceCm
ภาพที่ 2 เซ็นเซอร์ Ultrasonic ที่ติดตั้งบนแกนของเซอร์โวมอเตอร์

การเชื่อมต่อ

การเชื่อมต่อระหว่าง ESP8266 กับโมดูลต่าง ๆ เป็นดังนี้

ESP8266โมดูลขาของโมดูล
GNDServo LeftGND
5VServo LeftVcc
GPIO5Servo LeftSignal
GNDServo RightGND
5VServo RightVcc
GPIO4Servo RightSignal
GNDServo UltrasonicGND
5VServo UltrasonicVcc
GPIO2Servo UltrasonicSignal
GNDUltrasonic SensorGND
GPIO12Ultrasonic SensorEcho
GPIO14Ultrasonic SensorTrig
5VUltrasonic SensorVcc
ภาพที่ 3

ตัวอย่างโปรแกรม

ตัวอย่างโปรแกรม code12-1 เป็นการอ่านค่าระยะทางจากเซ็นเซอร์ ถ้าเซ็นเซอร์พบว่าระยะทางทางซ้าย ขวา หรือกลางน้อยกว่า 13 ซม. จะทำการถอยหลัง แต่ถ้าระยะทางทางซ้ายมีค่ามากที่สุดจะเลี้ยวซ้าย แล้วเดินหน้า ถ้าระยะทางทางขวามากที่สุดจะเลี้ยวขวาและเดินหน้า

# code12-1
import esp
import time
import gc
import machine as mc
gc.enable()
gc.collect()
mc.freq(160000000)
servoL = mc.PWM(mc.Pin(5,mc.Pin.OUT),freq=50) # D1 คุมมอเตอร์ซ้าย
servoR = mc.PWM(mc.Pin(4,mc.Pin.OUT),freq=50) # D2 คุมมอเตอร์ขวา
servoU = mc.PWM(mc.Pin(2,mc.Pin.OUT),freq=50) # D4 คุม Ultrasonic
trigPin = mc.Pin(14,mc.Pin.OUT) # D5
echoPin = mc.Pin(12,mc.Pin.IN) # D6
trigPin.value(0)
waitTime = 350
time.sleep_ms(3000) # wait

def distance():
    echoPin.value()
    trigPin.on()
    trigPin.off()
    usec = mc.time_pulse_us( echoPin, 1 )
    echoTime = usec/1000000
    distanceCm = (echoTime/2)*34300
    return distanceCm

def sensorMoveCenter():
    global servoU
    servoU.duty(70)
    time.sleep_ms(waitTime)
    
def sensorMoveLeft():
    global servoU
    servoU.duty(90)
    time.sleep_ms(waitTime)

def sensorMoveRight():
    global servoU
    servoU.duty(50)
    time.sleep_ms(waitTime)

def robotForward():
    global servoL,servoR
    servoL.duty(120)
    servoR.duty(40)

def robotStop():
    global servoL,servoR
    servoL.duty(0)
    servoR.duty(0)
    
def robotBackward():
    global servoL,servoR
    servoL.duty(40)
    servoR.duty(120)
    
def robotTurnRight():
    global servoL,servoR
    servoL.duty(120)
    servoR.duty(120)
    
def robotTurnLeft():
    global servoL,servoR
    servoL.duty(40)
    servoR.duty(40)

while True:
    robotStop()
    time.sleep_ms(100)
    sensorMoveLeft()
    distLeft = distance()
    sensorMoveRight()
    time.sleep_ms(waitTime)
    distRight = distance()
    sensorMoveCenter()
    distCenter = distance()
    print("distance at left/right/center = {}/{}/{} cm.".format(distLeft,distRight,distCenter))
    if (distCenter < 13.0) or (distLeft < 13.0) or (distRight < 13.0):
        robotBackward()
        print("backward")
        time.sleep_ms(2*waitTime)
    else:
        if (distLeft > distRight) and (distLeft > distCenter):
            print("Left")
            robotTurnLeft()
            time.sleep_ms(waitTime)
            robotStop()
        if (distRight > distLeft) and (distRight > distCenter):
            print("right")
            robotTurnRight()
            time.sleep_ms(waitTime)
            robotStop()
        robotForward()
        print("Forward")
        time.sleep_ms(waitTime)

สรุป

จากบทความนี้ผู้อ่านสามารถเขียนโปรแกรมภาษาไพธอนเพื่อทำการอ่านค่าและคำนวณระยะทางจากเซ็นเซอร HC-SR04 ซึ่งใช้หลักการหาระยะทางจากการสะท้อนของคลื่นเสียง และนำความรู้มาประยุกต์กับการขับเคลื่อนหุ่นยนต์เคลื่อนที่ด้วยล้อ เพื่อให้หุ่นยนต์มีการตัดสินใจในการทำงาน และทำงานตามเงื่อนไขที่กำหนดไว้ ซึ่งเป็นการใช้ปัญญาประดิษฐ์แบบง่าย ๆ ประเภทการตั้งเงื่อนไขการทำงาน

ทีมงานของเราหวังว่าผู้อ่านจะได้ประโยขน์จากบทความนี้ และสามารถนำไปประยุกต์ใช้งาน หรือสร้างเงื่อนไขการทำงานได้หลากหลาย หรือมีความถูกต้องแม่นยำมากยิ่งขึ้น หรือสร้างปัญญาประดิษฐ์ให้แก่หุ่นยนต์รถให้มีความซับซ่อนมากยิ่งขึ้นไป

สุดท้ายนี้ขอให้สนุกกับการเขียนโปรแกรมครับ

(C) 2020, โดย อ.ดนัย เจษฎาฐิติกุล/อ.จารุต บุศราทิจ
ปรับปรุงเมื่อ 2020-10-23