การใช้ Optical Mouse เป็นเซ็นเซอร์วัดการเคลื่อนที่แกน X และ Y โดยใช้ Raspberry Pi


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

บทความนี้จะกล่าวถึงการใช้ Optical เมาส์วัดระยะการเคลื่อนที่ในแนวแกน X และ Y บนคอมพิวเตอร์ Raspberry Pi ซึ่งเป็นวิธีที่มีจุดเด่นคือ

  • Raspberry Pi สามารถอ่านค่าจากเมาส์ที่เสียบกับพอร์ท USB ได้โดยตรง ไม่จำเป็นต้องแกะเมาส์ต่อวงจรเพิ่มเติมใดๆ ซึ่งนับว่าทำให้งานนี้ง่ายขึ้นมาก แทนที่จะต้องทำวงจรและเขียนโปรแกรมอ่านค่าจาก encoder หรือใช้งานระบบบัส I2C คอมพิวเตอร์ Raspberry Pi สามารถอ่านค่าการเคลื่อนที่จากไฟล์เช่น /dev/inputs/mice ได้โดยตรง
  • สามารถใช้กับ USB เมาส์ในท้องตลาดได้ทุกตัว  ในระบบสมองกลฝังตัวแบบดั้งเดิมที่ต้องแกะเมาส์ต่อวงจรนั้น มักพบว่าเมาส์ในปัจจุบันชิพภายในถูกยุบรวม และเข้าถึงข้อมูลผ่านทาง USB เท่านั้น ซึ่งไม่สามารถพ่วงเข้ากับไมโครคอนโทรเลอร์ได้ ดังนั้นเมาส์ส่วนใหญ่ในตลาดจึงใช้งานไม่ได้ บ่อยครั้งผู้พัฒนาต้องไปค้นหาเมาส์รุ่นเก่าๆ ที่อุปกรณ์ภายในยังแยกชิ้นอยู่มาใช้งานแทน แต่สำหรับ Raspberry Pi นั้นเนื่องจากใช้พอร์ท USB อยู่แล้วดังนั้นจึงสามารถใช้กับเมาส์ USB ได้ทุกรุ่น

optical mouse experiment

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

  • Optical เมาส์ – จะเป็นเมาส์รุ่นใดก็ได้รวมทั้งเมาส์ไร้สาย ขอเพียงให้เชื่อมต่อกับคอมพิวเตอร์ผ่านทาง USB และใช้งานได้กับระบบปฏิบัติการ Linux (ส่วนใหญ่ใช้ได้อยู่แล้ว) ในบทความนี้ได้ทดสอบกับเมาส์สองตัวดังในภาพ ตัวแรกเป็นเมาส์แบบสวมนิ้ว ซึ่งบังเอิญไปพบที่ร้านขายคอมพิวเตอร์ทั่วไป และคิดว่าเหมาะที่จะเอามาใช้กับงานควบคุมและหุ่นยนต์เพราะกินที่น้อย ราคาเพียงร้อยกว่าบาท ตัวที่สองเป็นเมาส์ไร้สายแบบเลเซอร์ คุณภาพการตรวจจับการเคลื่อนที่สูง นำมาใช้เพื่อเปรียบเทียบ
  • Raspberry Pi – ต้องมีพอร์ท USB ว่างอย่างน้อย 1 พอร์ท แต่จริงๆ แล้วใช้คอมพิวเตอร์ระบบ Linux ทั่วไปก็ได้เหมือนกัน

หลักการเขียนโปรแกรม

เมาส์แบบ USB เมื่อต่อเข้ากับระบบ Linux จะปรากฏเป็นไฟล์ใน /dev/inputs ชื่อว่า mouse0 โดยหากมีเมาส์ต่ออยู่หลายตัวก็จะปรากฏเป็น /dev/inputs/mouse1, /dev/inputs/mouse2, … เพิ่มขึ้นไปเรื่อยๆ นอกจากนั้นยังมีไฟล์ชื่อ /dev/inputs/mice ซึ่งจะรายงานการเคลื่อนที่ที่เกิดจากเมาส์ทุกตัวรวมกัน (แต่จะแยกแยะว่าเมาส์ตัวใดขยับไม่ได้) ดังนั้นหากในระบบมีเมาส์เพียงตัวเดียว จะเลือกใช้ไฟล์ /dev/inputs/mouse0 หรือ /dev/inputs/mice ก็จะได้ผลเหมือนกัน

เนื่องจากในบทความนี้มีการทดสอบเมาส์สองตัว ดังนั้นจะเลือกอ่านค่าจากไฟล์ของเมาส์แยกเป็นตัวๆ คือ /dev/inputs/mouse0 และ /dev/inputs/mouse1

รูปแบบของข้อมูล

การอ่านค่าจากเมาส์นั้นทำได้เหมือนกับการอ่านไฟล์ธรรมดา โดยข้อมูลการเคลื่อนที่จะมีอยู่ 3 ไบท์ด้วยกันดังนี้

ไบท์ที่ 1: เป็นสถานะของเมาส์ แต่ละบิตมีความหมายดังนี้

  • บิท 0 – สถานะของปุ่มซ้าย (1 = กด, 0 = ปล่อย)
  • บิท 1 – สถานะของปุ่มขวา
  • บิท 2 – สถานะของปุ่มกลาง (ถ้ามี)
  • บิท 3 – ไม่ได้ใช้ ค่าเป็น 1 เสมอ
  • บิท 4 – ทิศทางการเคลื่อนที่ในแกนนอน (X) โดย 1 = ขยับไปทางซ้าย, 0 =  ขยับไปทางขวา
  • บิท 5 – ทิศทางการเคลื่อนที่ในกานตั้ง (Y) โดย 1 = ขยับลง, 0 =  ขยับขึ้น
  • บิท 6 – Overflow แกนนอน จะเป็น 1 เมื่อเมาส์เคลื่อนที่เร็วจนตัวแปรนับค่าเต็ม (ค่าเกิน +/- 127)
  • บิท 7 – Overflow แกนตั้ง

ไบท์ที่ 2: ค่าการเคลื่อนที่แกนนอน – เก็บระยะที่เคลื่อนที่ไปนับตั้งแต่การอ่านครั้งก่อนหน้า (ไม่ใช่ตำแหน่ง) โดยค่านี้จะเก็บแบบ 2’s complement (-128 ถึง +127)

ไบท์ที่ 3: ค่าการเคลื่อนที่แกนตั้ง

ความละเอียดของเมาส์ส่งผลต่อค่าที่อ่านได้ – ระยะที่อ่านได้จากเมาส์นี้มีหน่วยเป็น “จุด” เมาส์ทั่วๆ ไปในปัจจุบันจะมีความละเอียดที่ 800 PPI (Points Per Inch) หรือ 800 จุดต่อนิ้ว ดังนั้นหากระยะการเคลื่อนที่รวมได้ 800 จุด ก็แสดงว่าเมาส์เคลื่อนที่ไปได้ 1 นิ้ว เมาส์คุณภาพสูงอาจจะมีความละเอียดสูงขึ้น เมาส์เลเซอร์อาจมีความละเอียด 1600 PPI หรือสูงกว่านั้น ดังนั้นเมาส์ที่ความละเอียดนี้จะอ่านค่าได้เป็นสองเท่าของเมาส์ 800 PPI ที่ระยะการเคลื่อนที่เดียวกัน

โปรแกรม Python

โปรแกรมต่อไปนี้จะทำการอ่านค่าจากไฟล์ /dev/inputs/mouse0 แล้วนำมาคำนวณตำแหน่งของเมาส์

import time

f = open('/dev/input/mouse0', 'rb')

xpos = 0
ypos = 0

print "started"

while True:
        # read from the file. ord() converts char to integer
        state = ord(f.read(1))
        dx = ord(f.read(1))
        dy = ord(f.read(1))

        #convert bits in 'state' in to an array 'mouse_state'
        mouse_state = [(state & (1 << i)) >> i for i in xrange(8)]

        # if mouse moving to the left. dx must be a negative value
        if mouse_state[4] == 1:
                dx = dx - 256    # convert 2's complement negative value

        # if mouse moving down
        if mouse_state[5] == 1:
                dy = dy - 256

        if (mouse_state[6] == 1) or (mouse_state[7]==1):
                print "Overflow!"

        # update the position
        xpos += dx
        ypos += dy

        print "DX=%4d  DY=%4d, xpos=%6d  ypos=%6d" % (dx,dy,xpos,ypos)

ทดสอบโปรแกรมนี้เพื่อดูว่าค่าที่ได้ตรงกับการเคลื่อนที่จริงหรือไม่

การทดลอง

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

บทความนี้ได้ทดลองเมาส์สองตัว โดยทดสอบการเคลื่อนที่ในแนวนอน (แกน X) โดยเคลื่อนไปและกลับแล้วดูว่าค่าตำแหน่งที่คำนวนได้นั้นเป็นเท่าใด ในทางทฤษฎีหากเมาส์เคลื่อนไปแล้วกลับมาที่จุดเดิม ค่าตำแหน่งแนวนอนจะต้องเป็น 0 แต่จากการทดลองค่าที่ได้ก็จะผิดเพี้ยนไปบ้าง ดังแสดงใน vdo ต่อไปนี้

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

เลเซอร์เมาส์ ทำการทดลอง 10 ครั้ง แต่ละครั้งจะขยับเมาส์ตามแนวนอนไปมา 5 รอบ ระยะการเคลื่อนที่ในแต่ละรอบอยู่ที่ประมาณ 16 – 20 นิ้ว  (ไป-กลับ) ค่าความผิดพลาดที่ได้ทั้ง 10 ค่าจะตัดค่าที่ผิดพลาดสูงสุดและต่ำสุดทิ้งไป

  •  ความผิดพลาดเฉลี่ย = 0.16 นิ้ว
  • ความผิดพลาดสูงสุด = 0.30 นิ้ว
  • ความผิดพลาดต่ำสุด = 0.06 นิ้ว
  • ค่าเบี่ยงเบน = 0.08 นิ้ว

เมาส์ Optical แสงสีแดงราคาถูก (150 บาท)  ทำการทดลองเหมือนกัน

  •  ความผิดพลาดเฉลี่ย = 0.21 นิ้ว
  • ความผิดพลาดสูงสุด = 0.67 นิ้ว
  • ความผิดพลาดต่ำสุด = 0.03 นิ้ว
  • ค่าเบี่ยงเบน = 0.21 นิ้ว

ปรับโปรแกรมเพื่อการนำไปใช้

ข้อเสียอย่างหนึ่งของโปรแกรม Python ข้างต้นคือ โปรแกรมจะค้างอยู่ที่คำสั่งอ่านค่าจากไฟล์เมาส์ ในระหว่างนั้นจะไม่ทำงานคำสั่งอื่นใดได้ หรือพูดอีกอย่างหนึ่งคือโปรแกรมจะค้างที่คำสั่งนี้ จนกว่าเมาส์จะมีการเคลื่อนไหว ในการนำไปใช้งานจริงย่อมไม่ต้องการให้โปรแกรมค้างเช่นนี้ ดังนั้นวิธีการที่ดีกว่าคือการสร้าง thread ให้กับโปรแกรมส่วนนี้แยกต่างหากและให้ทำงานคู่ขนานกับโปรแกรมหลัก

import threading
import time

class mouseTrack(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)
        self.xpos = 0
        self.ypos = 0
        self.needToStop = False

    def run(self):
        self.f = open('/dev/input/mouse0', 'rb')

        print "thread started"
        while (not self.needToStop):

            # read from the file. ord() converts char to integer
            state = ord(self.f.read(1))
            dx = ord(self.f.read(1))
            dy = ord(self.f.read(1))

            #convert bits in 'state' in to an array 'mouse_state'
            mouse_state = [(state & (1 << i)) >> i for i in xrange(8)]

            # if mouse moving to the left. dx must be a negative value
            if mouse_state[4] == 1:
                    dx = dx - 256    # convert 2's complement negative value

            # if mouse moving down
            if mouse_state[5] == 1:
                    dy = dy - 256

            if (mouse_state[6] == 1) or (mouse_state[7]==1):
                    print "Overflow!"

            # update the position
            self.xpos += dx
            self.ypos += dy

        print "Exiting thread"

if __name__ == '__main__':

    mt = mouseTrack()
    mt.start()   # start the thread

    while True:
        try:
                # print the position every 1 second
                print "xpos=%6d  ypos=%6d" % (mt.xpos, mt.ypos)
                time.sleep(1)

        # detect Ctrl-C 
        except (KeyboardInterrupt, SystemExit):
                # stop the thread by setting this variable
                mt.needToStop = True
                break

โปรแกรมข้างต้นจะสร้าง thread ขึ้นมาซึ่งทำหน้าที่ตรวจสอบการเคลื่อนที่ของเมาส์ ส่วนตัวโปรแกรมหลัก ก็จะวนพิมพ์ค่า xpos, ypos ออกมาทุกๆ 1 วินาที  จุดเด่นของโปรแกรมนี้คือโปรแกรมหลักสามารถไปทำงานอะไรก็ได้ thread จะทำงานอยู่แยกต่างหากคู่ขนานกันไป และคอยคำนวณค่า xpos, ypos ให้

สรุป

บทความนี้แสดงวิธีการใช้เมาส์เป็นเซ็นเซอร์วัดระยะการเคลื่อนที่ตามแกนนอนและแกนตั้ง จากการทดลองพบว่าเมาส์ให้ผลการวัดที่ดีเหมาะกับงานที่ยอมรับความคลาดเคลื่อนได้อย่างน้อย 1 นิ้ว (ยกเว้นถ้ามีการเคลื่อนที่น้อยๆ และช้าๆ ความแม่นยำน่าจะสูงขึ้น)   อย่างไรก็ดีอุปกรณ์จะต้องมีการ “ตั้งศูนย์” ใหม่อยู่อย่างสม่ำเสมอ เพราะเมาส์เมื่อเคลื่อนที่ไปนานๆ จะมีค่าความผิดพลาดสะสมที่สูงขึ้นเรื่อยๆ หากไม่ตั้งศูนย์บ่อยพอ (การตั้งศูนย์มักหมายถึงการใช้เซ็นเซอร์ชนิดอื่นเข้าช่วยตรวจหาตำแหน่งที่ทราบพิกัดแน่นอน) สุดท้ายค่าที่ได้ก็จะห่างออกจากความเป็นจริงไปเรื่อยๆ