[TH] ESP8266+RoboServo

บทความนี้เป็นตัวอย่างการเขียนโปรแกรมภาษาไพธอนเพื่อสั่งงานมอเตอร์ไฟฟ้ากระแสตรงที่เรียกว่าเซอร์โวมอเตอร์ (Servo Motor) และเมื่อนำเซอร์โวมอเตอร์มาขับเคลื่อนล้อทางด้านซ้ายและขวาทำให้สามารถทำงานเป็นหุ่นยนต์เคลื่อนที่ด้วยล้ออย่างง่าย ๆ ได้ นอกจากนี้ ตัวอย่างโปรแกรมของบทความนี้ เป็นการสั่งงานการเคลื่อนที่หุ่นยนต์ขับเคลื่อนด้วยล้อเพื่อสั่งเดินหน้า ถอยหลัง เลี้ยวซ้าย เลี้ยวขวา และหยุด

อุปกรณ์

อุปกรณ์ที่ใช้ปะกอบการเขียนบทความนี้ประกอบด้วย

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

การสั่งงานเซอร์โวมอเตอร์

เซอร์โวมอเตอร์มีสายเชื่อมต่อ 3 เส้น สำหรับเป็น Vcc (สีแดง) Gnd (สีดำหรือน้ำตาล) และสัญญาณควบคุม (สีเหลือง) โดยแรงดันที่จ่ายเข้าขา Vcc นั้นขึ้นอยู่กับคุฯสมบัติของเซอร์โวที่เลือกใช้งาน

การสั่งงานเซอร์โวต้องอาศัยการส่งพัลซ์ (Pulse) ไปให้กับขาสัญญาณควบคุมของมอเตอร์เพื่อสั่งให้มอเตอร์หมุนทวนเข็มนาฬิกา ตามเข็มนาฬิกา และหยุด ด้วยการกำหนดค่า Duty Cycle โดยรูปแบบการสร้างวัตถุสำหรับควบคุม มีรูปแบบการเขียนดังนี้

ตัวแปร = machine.PWM( machine.Pin( ขาที่เชื่อมต่อ ), freq=50 )

การสั่งงานให้มอเตอร์หมุนทวนเข็มนาฬิกาต้องกำหนดค่า Duty Cycle ซึ่งมอเตอร์แต่ละรุ่นอาจมีค่าที่แตกต่างกัน แต่มอเตอร์ที่ใช้กับบทความนี้เขียนสั่งงานได้ดังนี้

ตัวแปร.duty( 120 )

การสั่งให้หมุนตามเข็มนาฬิกาเขียนได้ดังนี้

ตัวแปร.duty( 40 )

การสั่งให้หยุดเขียนได้ดังนี้

ตัวแปร.duty( 0 )

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

การเชื่อมระหว่าง ESP8266 กับเซอร์โวมอเตอร์ ผู้เขียนได้ดำเนินการดังนี้

ขา D1 หรือ GPIO5 ของ ESP8266 ต่อเข้ากับขาสัญญาณของมอเตอร์ตัวด้านซ้ายของหุ่นยนต์เคลื่อนที่ด้วยล้อ
ขา D2 หรือ GPIO4 ของ ESP8266 ต่อเข้ากับขาสัญญาณของมอตเรอ์ตัวด้านขวาของหุ่นยนต์เคลื่อนที่ด้วยล้อ

การสั่งให้หุ่นยนต์เคลื่อนที่ด้วยล้อเคลื่อนที่ มีหลักการดังนี้

การเดินหน้า อาศัยการหมุนมอเตอร์ซ้ายแบบทวนเข็มนาฬิกาและมอเตอร์ขวาเป็นแบบตามเข็มนาฬิกา
การถอยหลัง อาศัยการหมุนมอเตอร์ซ้ายเป็นแบบตามเข็มนาฬิกาและมอเตอร์ขวาหมุนทวนเข็มนาฬิกา
การเลี้ยวซ้าย เป็นการสั่งการให้มอเตอร์ซ้ายหมุนตามเข็มนาฬิกาและมอเตอร์ขวาหมุนทวนเข็มนาฬิกา
การเลี้ยวขวา กำหนดให้มอเตอร์ซ้ายหมุนทวนเข็มนาฬิกาและมอเตอร์ขวาหมุนตามเข็มนาฬิกา
หยุด ทำโดยการสั่งให้มอเตอร์ทั้ง 2 ตัวหยุด หรือมีค่า Duty Cycle เป็น 0

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

ตัวอย่างโปรแกรม code11-1 เป็นการสั่งงานให้หุ่นยนต์เคลื่อนที่ด้วยล้อสามารถเดินหน้า ถอยหลัง เลี้ยวซ้าย เลี้ยวขวา และหยุดได้ด้วยการสั่งงานจากเว็บบราวเซอร์ โดยการเชื่อมต่อเข้ากับ AP ชื่อ JarutEx-AP และกำหนดรหัสเข้าใช้เป็น 123456789 (สามารถแก้ไขเป็นชื่อ AP และรหัสผ่านที่ต้องการได้) หลังจากนั้นเข้า Browser และใช้ URL เป็น 192.168.4.1 จะแสดงหน้าจอดังภาพที่ 4-8 ซึ่งเป็นการแสดงสถานะ Stop, Forward, Backward, Turn Left และ Turn Right ของหุ่นยนต์เคลื่อนที่ด้วยล้อ

# code11-1
import socket
import network
import esp
import time
import gc
import time
import machine as mc
servoL = mc.PWM(mc.Pin(5),freq=50)
servoR = mc.PWM(mc.Pin(4),freq=50)

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)

motionState = 0 # 0-Stop,1-Forward,2-Backward,3-TurnLeft,4-TurnRight

def webPage():
    global motionState
    html = """
<!DOCTYPE html><html><head><meta name="viewport" content="width=device-width, initial-scale=1">
<style>
.button {  border: none;  color: white;  padding: 20px;  text-align: center;  text-decoration: none;
  display: inline-block;  font-size: 14
  px;  margin: 4px 2px;  cursor: pointer;  border-radius: 4%;
  width: 100%; height: 100%;
}
.button1 {  background-color: #3ABC40; }
.button2 {  background-color: #BC4040; }
</style></head><body><table>
"""
    if motionState == 0:
        html += """
<tr>
<td></td>
<td><a href='/?robot=forward'><button class='button button2'>Forward</button></a></td>
<td></td>
</tr>
<tr>
<td><a href='/?robot=left'><button class='button button2'>Turn Left</button></a></td>
<td><a href='/?robot=stop'><button  class='button button1'>Stop</button></a></td>
<td><a href='/?robot=right'><button class='button button2'>Turn Right</button></a></td>
</tr>
<tr>
<td></td>
<td><a href='/?robot=backward'><button class='button button2'>Backward</button></a></td>
<td></td>
</tr>
"""
    elif motionState == 1:
        html += """
<tr>
<td></td>
<td><a href='/?robot=forward'><button class='button button1'>Forward</button></a></td>
<td></td>
</tr>
<tr>
<td><a href='/?robot=left'><button class='button button2'>Turn Left</button></a></td>
<td><a href='/?robot=stop'><button  class='button button2'>Stop</button></a></td>
<td><a href='/?robot=right'><button class='button button2'>Turn Right</button></a></td>
</tr>
<tr>
<td></td>
<td><a href='/?robot=backward'><button class='button button2'>Backward</button></a></td>
<td></td>
</tr>
"""
    elif motionState == 2:
        html += """
<tr>
<td></td>
<td><a href='/?robot=forward'><button class='button button2'>Forward</button></a></td>
<td></td>
</tr>
<tr>
<td><a href='/?robot=left'><button class='button button2'>Turn Left</button></a></td>
<td><a href='/?robot=stop'><button  class='button button2'>Stop</button></a></td>
<td><a href='/?robot=right'><button class='button button2'>Turn Right</button></a></td>
</tr>
<tr>
<td></td>
<td><a href='/?robot=backward'><button class='button button1'>Backward</button></a></td>
<td></td>
</tr>
"""
    elif motionState == 3:
        html += """
<tr>
<td></td>
<td><a href='/?robot=forward'><button class='button button2'>Forward</button></a></td>
<td></td>
</tr>
<tr>
<td><a href='/?robot=left'><button class='button button1'>Turn Left</button></a></td>
<td><a href='/?robot=stop'><button  class='button button2'>Stop</button></a></td>
<td><a href='/?robot=right'><button class='button button2'>Turn Right</button></a></td>
</tr>
<tr>
<td></td>
<td><a href='/?robot=backward'><button class='button button2'>Backward</button></a></td>
<td></td>
</tr>
"""
    else:
        html += """
<tr>
<td></td>
<td><a href='/?robot=forward'><button class='button button2'>Forward</button></a></td>
<td></td>
</tr>
<tr>
<td><a href='/?robot=left'><button class='button button2'>Turn Left</button></a></td>
<td><a href='/?robot=stop'><button  class='button button2'>Stop</button></a></td>
<td><a href='/?robot=right'><button class='button button1'>Turn Right</button></a></td>
</tr>
<tr>
<td></td>
<td><a href='/?robot=backward'><button class='button button2'>Backward</button></a></td>
<td></td>
</tr>
"""
    html += "</table></body></html>"
    return html

# main program
if __name__ == '__main__':
    robotStop()
    ap = network.WLAN(network.AP_IF)
    ssid = 'JarutEx-AP'
    password = '123456789'
    ap.active(True)
    ap.config(essid=ssid, password=password)
    while ap.active() == False:
          pass
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.bind(('', 80))
    s.listen(1)
    while True:
        conn, addr = s.accept()
        #print('conn {} from {}'.format(conn, addr))
        request = str(conn.recv(1024))
        #print('request = {}'.format(request))
        robotCmdStop = request.find('/?robot=stop')
        robotCmdForward = request.find('/?robot=forward')
        robotCmdBackward = request.find('/?robot=backward')
        robotCmdTurnLeft = request.find('/?robot=left')
        robotCmdTurnRight = request.find('/?robot=right')
        if robotCmdStop == 6:
            motionState = 0
            robotStop()
        if robotCmdForward == 6:
            motionState = 1
            robotForward()
        if robotCmdBackward == 6:
            motionState = 2
            robotBackward()
        if robotCmdTurnLeft == 6:
            motionState = 3
            robotTurnLeft()
        if robotCmdTurnRight == 6:
            motionState = 4
            robotTurnRight()
        response = webPage()
        conn.send('HTTP/1.1 200 OK\n')
        conn.send('Content-Type: text/html\n')
        conn.send('Connection: close\n\n')
        conn.send(response)
        conn.close()

ภาพที่ 4 หน้าจอเมื่อหุ่นยนต์เคลื่อนที่ด้วยล้อหยุดเคลื่อนที่

ภาพที่ 5 ภาพหน้าจอเมื่อหุ่นยนต์เคลื่อนที่ได้ด้านหน้า

สรุป

จากบทความนี้ผู้อ่านสามารถเขียนโปรแกรมภาษาไพธอนเพื่อสั่งงานเซอร์โวมอเตอร์ โดยใช้ฟังก์ขัน PWM และกำหนดค่า Duty Cycle เพื่อให้มอเตอร์หมุนทวนเข็มนาฬิกา ตามเข็มนาฬิกา และหยุด พร้อมทั้งนำมาประยุกต์ใช้ในการขับเคลื่อนหุ่นยนต์เคลื่อนที่ด้วยล้อ พร้อมทั้งสั่งงานผ่านเว็บ

พวกเราหวังว่าบทความนี้คงเป็นประโยชน์แก่ผู้อ่านไม่มากก็น้อย และสุดท้ายขอให้สนุกกับการเขียนโปรแกรมครับ

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