เดโม & ตัวอย่างใช้งานจริง

13.1 ภาพรวมเดโม

ในบทนี้มีเดโม 2 แบบ:

ทั้ง 2 เดโมใช้พิกัดเดียวกัน เปลี่ยนแค่วิธีจับ ทำให้เปรียบเทียบได้ง่าย

Flow ของ Pick & Place โครงสร้างเหมือนกันทั้ง 2 demos

13.2 การเตรียม

อุปกรณ์ที่ต้องใช้

• myCobot 280 JN พร้อมเปิดเครื่องและทดสอบ Login แล้ว (ดู บทที่ 4)
• End-Effector Gripper สำหรับ Demo 1, Vacuum Pump สำหรับ Demo 2
• บล็อกไม้/พลาสติก ขนาดประมาณ 2×2 ซม. น้ำหนัก <150 กรัม
• โต๊ะทำงานที่มีพื้นที่ราบ ไม่มีสิ่งกีดขวาง

การสอนตำแหน่ง (Teach Points) ด้วย myBlockly

ก่อนรันสคริปต์ ต้องรู้พิกัดของ จุดหยิบ และ จุดวาง ในโลกจริง วิธีหา:

1. เปิด myBlockly และ Connect กับหุ่นยนต์
2. คลิก Free Mode (จะปลด servo เพื่อขยับด้วยมือ)
3. ขยับแขนด้วยมือไปที่ จุดหยิบ (Grab Point)
4. คลิก Get Coords จะเห็นค่า [X, Y, Z, Rx, Ry, Rz]
5. บันทึกค่าเหล่านี้ลงในตัวแปร grab_point ในสคริปต์
6. ทำซ้ำสำหรับ จุดวาง (Place Point) บันทึกใน place_point
7. ปิด myBlockly ก่อนรัน Python ไม่งั้นพอร์ตจะชน

13.3 Demo 1: Gripper ขนบล็อก

13.3.1 ทดสอบ Gripper ก่อน

ก่อนรันโปรแกรมเต็ม ทดสอบเปิด/ปิด Gripper ก่อน:

from pymycobot import MyCobot280
import time

arm = MyCobot280("/dev/ttyTHS1", 1000000)

for i in range(2):
    arm.set_gripper_state(1, 100)   # ปิด
    time.sleep(1)
    arm.set_gripper_state(0, 100)   # เปิด
    time.sleep(1)

หาก Gripper เปิด-ปิดได้ปกติ ผ่าน ไปขั้นถัดไป

13.3.2 โปรแกรม Pick & Place เต็ม

from pymycobot import MyCobot280
import time

# ----------- ตัวแปร (ปรับค่าตามจุดที่สอน) -----------
init_angles = [18.36, 6.5, -124.45, 26.54, -1.84, -110.47]

# พิกัด [X, Y, Z, Rx, Ry, Rz]
grab_point  = [158.4, -66.9, 77.9, 179.01, 0.88, 52.41]
place_point = [158.4,  36.9, 77.9, 179.01, 0.88, 52.41]

# ความสูงปลอดภัยที่ลอยเหนือวัตถุ
SAFE_HEIGHT = 70   # มิลลิเมตร

# ----------- เริ่มต้น -----------
arm = MyCobot280("/dev/ttyTHS1", 1000000)

def lift_above(point):
    """ลอยเหนือ point เป็นระยะ SAFE_HEIGHT"""
    return [point[0], point[1], point[2] + SAFE_HEIGHT,
            point[3], point[4], point[5]]

if __name__ == "__main__":
    # 1) เปิด Gripper เตรียมพร้อม
    arm.set_gripper_state(0, 100)
    time.sleep(1)

    # 2) ไป Home
    arm.send_angles(init_angles, 100)
    time.sleep(2)

    # 3) ลอยเหนือจุดหยิบ
    arm.send_coords(lift_above(grab_point), 100, 1)
    time.sleep(2)

    # 4) ลงไปที่จุดหยิบ
    arm.send_coords(grab_point, 100, 1)
    time.sleep(2)

    # 5) ปิด Gripper หยิบของ
    arm.set_gripper_state(1, 100)
    time.sleep(1)

    # 6) ยกขึ้น
    arm.send_coords(lift_above(grab_point), 100, 1)
    time.sleep(2)

    # 7) ย้ายไปลอยเหนือจุดวาง
    arm.send_coords(lift_above(place_point), 100, 1)
    time.sleep(2)

    # 8) ลงไปที่จุดวาง
    arm.send_coords(place_point, 100, 1)
    time.sleep(2)

    # 9) เปิด Gripper ปล่อยของ
    arm.set_gripper_state(0, 100)
    time.sleep(1)

    # 10) ยกขึ้นกลับเป็นท่าจบ
    arm.send_coords(lift_above(place_point), 100, 1)
    time.sleep(2)

    print("✓ Demo completed.")

คำอธิบายขั้นตอน

1. เปิด Gripper เผื่อยังคาของไว้จากครั้งก่อน
2. กลับท่า Home เพื่อเริ่มต้นจากตำแหน่งที่รู้
3. เคลื่อนไปลอย เหนือ จุดหยิบ 70 มม. เพื่อให้ลงตรงตำแหน่ง ไม่ชนของ
4. ลงตรงไปที่จุดหยิบ (Z ลด 70 มม.)
5. ปิด Gripper หยิบ
6. ยกขึ้น (ลอยเหนือ)
7. เลื่อนไปเหนือจุดวาง
8. ลงตรงไปที่จุดวาง
9. เปิด Gripper ปล่อย
10. ยกขึ้นกลับ เพื่อพร้อมทำรอบถัดไป

13.4 Demo 2: Vacuum Pump ขนบล็อก

13.4.1 การต่อสายปั๊ม

ก่อนรันโค้ด ต้องต่อสายปั๊ม 4 เส้น (GND / 5V / G2 / G5) เข้ากับ GPIO ของ Jetson Nano ตามตาราง หากยังไม่ได้ต่อ ดูภาพประกอบและขั้นตอนเต็ม ๆ ใน บทที่ 3.4.1 การติดตั้งปั๊มสุญญากาศ

13.4.2 ทดสอบปั๊มก่อน

from pymycobot import MyCobot280
import time
import Jetson.GPIO as GPIO

arm = MyCobot280("/dev/ttyTHS1", 1000000)
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)

def pump_on():
    GPIO.output(20, 0)   # Logic invert: 0 = ON
    time.sleep(0.05)

def pump_off():
    GPIO.output(20, 1)   # 1 = OFF
    time.sleep(0.05)
    GPIO.output(21, 0)   # เปิดวาล์วปล่อยลม
    time.sleep(1)
    GPIO.output(21, 1)   # ปิดวาล์ว
    time.sleep(0.05)

for i in range(2):
    pump_on()
    time.sleep(2)
    pump_off()
    time.sleep(2)

GPIO.cleanup()

13.4.3 โปรแกรม Pick & Place เต็ม (ปั๊ม)

from pymycobot import MyCobot280
import time
import Jetson.GPIO as GPIO

# ----------- ตัวแปร -----------
init_angles = [18.36, 6.5, -124.45, 26.54, -1.84, -110.47]
grab_point  = [158.4, -66.9, 77.9, 179.01, 0.88, 52.41]
place_point = [158.4,  36.9, 77.9, 179.01, 0.88, 52.41]
SAFE_HEIGHT = 70

# ----------- Setup -----------
arm = MyCobot280("/dev/ttyTHS1", 1000000)
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)

def pump_on():
    GPIO.output(20, 0)
    time.sleep(0.05)

def pump_off():
    GPIO.output(20, 1)
    time.sleep(0.05)
    GPIO.output(21, 0)
    time.sleep(1)
    GPIO.output(21, 1)
    time.sleep(0.05)

def lift_above(p):
    return [p[0], p[1], p[2] + SAFE_HEIGHT, p[3], p[4], p[5]]

# ----------- Main -----------
if __name__ == "__main__":
    try:
        pump_off()                       # เริ่มต้นปั๊มปิด
        time.sleep(1)
        arm.send_angles(init_angles, 100)
        time.sleep(2)

        arm.send_coords(lift_above(grab_point), 100, 1)
        time.sleep(2)
        arm.send_coords(grab_point, 100, 1)
        time.sleep(2)

        pump_on()                        # ดูดวัตถุ
        time.sleep(1)

        arm.send_coords(lift_above(grab_point), 100, 1)
        time.sleep(2)
        arm.send_coords(lift_above(place_point), 100, 1)
        time.sleep(2)
        arm.send_coords(place_point, 100, 1)
        time.sleep(2)

        pump_off()                       # ปล่อยวัตถุ
        time.sleep(1)

        arm.send_coords(lift_above(place_point), 100, 1)
        time.sleep(2)
        print("✓ Demo completed.")

    finally:
        GPIO.cleanup()

13.5 ปรับให้เข้ากับงานของคุณ

เดโมนี้เป็นตัวอย่างพื้นฐานที่สุด ขั้นต่อไปคือเอาไปต่อกับ Vision:

• ใช้ AIKit + Color Detection แทนการระบุพิกัด grab_point ตายตัว ใช้กล้อง 3D หาวัตถุสีแดง/น้ำเงิน แล้วคำนวณพิกัดส่งให้แขน ดู บทที่ 5
• ใช้ YOLOv8 ตรวจจับวัตถุในชีวิตประจำวัน (แก้ว, ขวด) แล้วสั่งหยิบแบบเลือก class
• ลูปทำซ้ำ เพิ่ม for i in range(10): รอบโค้ดหลัก ทำให้เป็นสายการผลิตจำลอง
• หลายจุด กำหนด grab_points และ place_points เป็น list หลายจุด สลับหยิบไป-มาเป็นแพทเทิร์น

13.6 เคล็ดลับการดีบัก

• หุ่นยนต์เคลื่อนไม่ตรง เช็คว่ารัน Calibrate แล้วหรือยัง
• เคลื่อนช้าเกินไป เปลี่ยน speed จาก 100 เป็น 80 ลดจังหวะ sleep
• เคลื่อนเร็วเกิน ของล้ม ลด speed ลงเป็น 30-50
• ปั๊ม/Gripper ไม่จับ ตรวจสายไฟ + พื้นผิว ดู A.3
• เกิน Joint Limit แสดง Error 1-6 ปรับ Home angles ใหม่

13.7 เดโมขั้นสูง: เชื่อม PLC ผ่าน IO

สำหรับ Industrial use case สามารถให้ PLC ส่งสัญญาณมาให้หุ่นยนต์ทำงาน ได้ผ่าน IO ของฐาน เช่น สัญญาณ trigger จาก sensor บนสายการผลิต แล้วหุ่นยนต์รับและตอบสนอง

Flow ทั่วไป

1. PLC ส่งสัญญาณ 24V relay ลด level เป็น 3.3V เข้า GPIO input ของ Jetson
2. Python อ่าน GPIO ใน loop, รอ rising edge
3. เมื่อตรวจจับ สั่งหุ่นยนต์ทำงาน (เช่น กลับ Home, หยิบของ, ฯลฯ)
4. เมื่อเสร็จ ส่งสัญญาณกลับ PLC ผ่าน GPIO output relay 24V output

from pymycobot import MyCobot280
import Jetson.GPIO as GPIO
import time

mc = MyCobot280('/dev/ttyTHS1', 1000000)
GPIO.setmode(GPIO.BCM)
GPIO.setup(17, GPIO.IN)    # input จาก PLC (ผ่าน relay)
GPIO.setup(18, GPIO.OUT)   # output ไป PLC (ผ่าน relay)

print("รอสัญญาณจาก PLC...")
while True:
    if GPIO.input(17) == GPIO.HIGH:
        print("→ PLC trigger! เริ่ม sequence")
        mc.send_angles([0, 0, 0, 0, 0, 0], 50)
        time.sleep(3)
        GPIO.output(18, GPIO.HIGH)   # บอก PLC ว่าทำเสร็จ
        time.sleep(0.5)
        GPIO.output(18, GPIO.LOW)
    time.sleep(0.05)   # debounce

ดูรายละเอียดเพิ่มเติม + วงจร relay ที่ Successful Cases PLC IO Interactive Control

13.8 เดโมเพิ่มเติม

เดโมอื่น ๆ ที่ Elephant Robotics จัดเตรียมไว้:

• Successful Cases (รายการเดโมทั้งหมด)
• Demonstration Code (Python examples)
• "Elephant Robot myCobot Creative Video Collection" (Bilibili) รวบรวมงาน commercial/educational
• pymycobot/demo บน GitHub