พีรัชชัย โอรมย์: โปรเจครถติดตามวัตถุ (CAR TRACKING)

โปรเจครถติดตามวัตถุ (CAR TRACKING) งานที่14

โปรเจครถติดตามวัตถุ (Car Tracking)

พัฒนาโดยโมดูลกล้อง Pixy CMUcam5 และบอร์ด Arduino Mega 2560

ในบทความ “โปรเจครถติดตามวัตถุ (Car Tracking) พัฒนาโดยโมดูลกล้อง Pixy CMUcam5 และบอร์ด Arduino Mega 2560” นี้ วัตถุประสงค์คือการประยุกต์การใช้งานกล้อง Pixy CMUcam5 เชื่อมต่อกับไมโครคอนโทรลเลอร์เพื่อทำแอพพลิเคชั่นเป็นรถติดตามวัตถุ ศึกษาเรียนรู้การทำงานของกล้อง Pixy CMUcam5 เชื่อมต่อสื่อสารกับไมโครคอนโทรลเลอร์ โดยผู้ใช้ต้องกำหนดลักษณะสีที่ต้องการตรวจจับจากคอมพิวเตอร์ให้กับกล้องก่อน แล้วเมื่อกล้องทำงานจะส่งผลลัพธ์ไปยังไมโครคอนโทรลเลอร์เพื่อควบคุมการทำงานของมอเตอร์ที่ควบคุมการเคลื่อนที่ของรถ รวมทั้งตัวกล้องเองก็จะส่งคำสั่งไปควบคุมมอเตอร์ในการหมุนซ้าย-ขวาและปรับก้ม-เงยตัวกล้องด้วย

อุปกรณ์ที่ใช้ในบทความนี้ประกอบด้วย (คลิกที่ชื่อสินค้า เพื่อลิงค์ไปดูรายละเอียดสินค้า)

>> ราคารวมอุปกรณ์ตามรายการด้านล่างนี้ อยู่ประมาณ 6,040.15 บาท (รวม VAT) <<

(เป็นเพียงตัวอย่างการประยุกต์ใช้งาน หากลูกค้าสนใจรบกวนสั่งซื้อสินค้าแยกชิ้นนะคะ (ไม่มีจัดชุดสำเร็จรูปค่ะ))

1. Pixy CMUcam5 (EFDV347) จำนวน 1 pcs ราคา 2,850 บาท กล้องที่ใช้เป็นเซ็นเซอร์ในการทำตรวจจับและหาตำแหน่งของสีที่ต้องการเพื่อส่งข้อมูลไปสั่งงานมอเตอร์ต่อไป

2. FPV Nylon Head (EFDV341) จำนวน 1 pcs ราคา 95 บาท ขาตั้ง 2 แกน ใช้งานคู่กับ Servo 9g, 12g สามารถเคลื่อนที่ได้ทั้ง 2 แกนนำไปประยุกต์ใช้งานเป็นขาตั้งกล้อง Webcam ที่สามารถหมุนและปรับทิศทางได้

3. Tower Pro Micro Servo Motor SG90 (EFDV242) จำนวน 2 pcs ราคาต่อชิ้น 90 บาท มอเตอร์ปรับทิศทางหันกล้องใช้งานคู่กับขาตั้ง

4. Arduino Mega 2560 (EADN015) จำนวน 1 pcs ราคา 1,650 บาท โมโครคอนโทรลเลอร์ที่ใช้ในการควบมอเตอร์ขับเคลื่อนตัวรถ

5. Ardumoto Motor Driver Shield L298P (EFDV278) จำนวน 1 pcs ราคา 450 บาท Shield สำหรับขับกระแสให้มอเตอร์

6. Smart Car with Battery Case (EFDV396) ราคา 420 บาท ชุดโครงรถอะคริลิคใสพร้อมอุปกรณ์ จำนวน 1 pcs ตรงนี้ผู้ใช้สามารถทำขึ้นเองได้ ใช้มอเตอร์กระแสตรง 2 ตัว

จากบทความที่แล้ว “Unboxing preview Pixy CMUcam5” คือกล้องจากทาง CMUcam ที่มีการคิดค้นและพัฒนาอย่างต่อเนื่อง Pixy CMUcam5 เป็นตัวล่าสุดที่เพิ่งออกจำหน่ายเมื่อไม่นานนี้ โดยเพิ่มความสามารถในการประมวลผลภาพที่ได้เข้าไปด้วยทำให้มีความสามารถเพิ่มมากขึ้นและใช้งานง่ายขึ้น สามารถเอาผลลัพธ์ของการทำงานไปใช้ได้ทันที สิ่งที่ Pixy CMUcam5 ทำได้ก็คือ

การสื่อสารกับไมโครคอนโทรลเลอร์ต่างๆ ผ่านทาง UART Serial, SPI, I2C, USB, etc..
ตรวจจับวัตถุสีต่างๆ ได้จำนวนหลายชิ้นในเวลาเดียวกัน
ควบคุมการทำงานด้วย LPC4330 ARMCortex-M4 เป็น Processor หลักในการทำ Digital Signal Processing ยังสามารถควบคุม Servo ได้ในตัวด้วย

ขั้นตอนในการเริ่มต้นใช้งานและเชื่อมต่อ Pixy CMUcam5 สามารถดูได้จากบทความที่แล้ว “Unboxing preview Pixy CMUcam5” สำหรับในบทความนี้ เราจะอธิบายเริ่มต้นตั้งแต่ส่วนประกอบต่างๆบนโมดูลกล้อง, การประกอบขาตั้งกล้องที่สามารถแพนได้ และการตั้งค่า Configure ต่างๆในการใช้งานกับไมโครคอนโทรลเลอร์ Arduino Mega 2560
ลำดับแรกในการเริ่มต้นเราจะมาทดสอบเจ้ากล้อง Pixy กันครับ ให้เราต่อกล้องเข้ากับคอมพิวเตอร์ก่อนเพื่อทำการทดสอบการทำงาน

หลังจากนั้นทำการเปิดโปรแกรม PixyMon ที่ลงไว้ก่อนหน้านี้ขึ้นมาดังภาพด้านล่าง

ให้เราเปิดภาพขึ้นมาดูก่อนโดยเลือกที่เมนู Action > Raw Video

หลังจากนั้นให้ทำการปรับโฟกัสที่เลนส์หน้ากล้องเพื่อใช้ในการตรวจจับวัตถุที่แม่นยำมากขึ้น

ลำดับต่อมาทดสอบการตรวจจับวัตถุกันเลย โดยเลือกที่ Action> Set Signature 1… กำหนดวัตถุที่ต้องการตรวจจับวัตถุที่ 1

หลังจากภาพหยุดนิ่ง ให้เราเลือกบริเวณสีที่เราต้องตรวจจับดังภาพด้านล่าง

เมื่อทำการเลือกสีที่ต้องการ กล้องจะทำการตรวจจับทันที จากภาพด้านล่างจะเห็นได้ว่ามีกอบสีขาวเมื่อตรวจพบสีที่กำหนดไว้ S=1

ทดสอบขยับวัตถุเพื่อตรวจสอบว่ากล้องสามารถใช้งานได้ปกติ

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

1. Button คือ ปุ่มที่ใช้เลือกวัตถุที่ต้องการตรวจจับ ตามรูปแบบที่บันทึกไว้ 1-7 และเป็นปุ่มที่ใช้เริ่มและหยุดการตรวจจับอีกด้วย
2. USB คือ พอร์ตที่ใช้ในการเชื่อมต่อไปยังคอมพิวเตอร์หรืออุปกรณ์อื่นๆ เพื่อกำหนดค่าต่างๆ
3. I/O Port (UART, SPI, I2C etc.) คือ พอร์ตที่ใช้ในการเชื่อมต่อกับไมโครคอนโทรเลอร์ในรูปแบบต่างๆ สามารถดูลายละเอียดจากภาพด้านล่าง
4. RC Servo(2) คือ พอร์ตที่ใช้ในการเชื่อมต่อกับ Servo สามารถต่อได้สูงสุดถึง 2 Servo
5. Power In(6-10V) คือ พอร์ตสำหรับจ่ายไฟเลี้ยง 6-10V เราสามาใช้จ่ายไฟเลี้ยงได้หลายช่องทางทั้ง USB และ I/O Port

พอร์ตเชื่อมต่อกับ Servo สามารถเชื่อมต่อได้ถึง 2 Servo และมีฟังก์ชั่นในการควบคุมให้อีกด้วย

การประกอบขาตั้ง Servo อุปกรณ์ที่ใช้มีดังนี้

Tower Pro Micro Servo Motor SG90 (EFDV242) จำนวน 2 pcs
FPV Nylon Head (EFDV341) จำนวน 1 pcs
Screws x 10 (มากับชุด FPV nylon head)

Step 1: ตัดแขนของ Servo ให้ได้ขนาดดังภาพด้านล่าง

Step 2: เริ่มประกอบส่วนแรก ส่วนของฐานดูได้จากภาพด้านล่าง

Step 3: ประกอบแขนของ Servo เข้ากับช่องที่กำหนดไว้ดังภาพด้านล่าง

Step 4: ประกอบ Servo ตัวที่ 2 เข้ากับแขนที่ประกอบไว้ก่อนหน้านี้ตามภาพด้านล่าง

Step 5: ประกอบส่วนบนสุดเข้ากับ Servo ตัวที่ 2

Step 5.1: ประกอบส่วนบนสุดเข้ากับ Servo ตัวที่ 2 ถูกต้องจะเป็นดังภาพด้านล่าง

Step 6: ประกอบขาตั้งส่วนฐานล่างสุดเข้ากับแขนของ Servo

Step 7: ประกอบขาตั้งส่วนฐานล่างสุดเข้ากับ ชุดส่วนบนที่ประกอบเสร็จแล้ว

Step 8: ทำการตัดพลาสติกส่วนด้านบนออก พร้อมทั้งเจาะรูสำหรับยึดน็อต

Step 9: ทำการประกอบกล้องเขากับขาตั้งดังภาพด้านล่าง

Step 10: ทำการต่อสาย Servo เข้ากับกล้อง Pixyดังนี้

Servo ตัวฐานต่อเข้ากับ >>> ช่อง RC-Servo PWM0
Servo ตัวบนต่อเข้ากับ >>> ช่อง RC-Servo PWM1

ลำดับต่อมาทำการตั้งค่าเจ้า Pixy ของเราเพื่อเตรียมเชื่อมต่อเข้ากับไมโครคอนโทรลเลอร์โดยเลือกที่เมนู File > Configure…

จากนั้นจะเห็นหน้าต่างใหม่ที่ใช้ในการตั้งค่าส่วนต่างๆ สามารถดูลายละเอียดการตั้งค่าได้จากข้อมูลด้านล่าง

Max blocks (กำหนดจำนวนวัตถุที่ตรวจพบสูงสุดในหนึ่งเฟรม) (ต้องตั้งค่าตามความเหมาะสมขึ้นอยู่กับจำนวนวัตถุที่ต้องการตรวจจับ)
- Sets the maximum total blocks sent per frame.
- default: 1000
Max blocks per signature (กำหนดจำนวนวัตถุที่ตรวจพบสูงสุดต่อ 1 สี) (ต้องตั้งค่าตามความเหมาะสมขึ้นอยู่ดับความต้องการข้อมูล)
- Sets the maximum blocks for each color signature sent for each frame.
- default: 1000
Min block area (กำหนดขนาดของวัตถุที่ตรวจพบเล็กที่สุดและจะไม่ถูกส่งข้อมูลออก)
- Sets the minimum required area in pixels for a block. Blocks with less area won't be sent.
- default: 20
Default program (กำหนดหมายเลขโปรแกรมเมื่อทำการเปิดขึ้น)
- Selects the program number that's run by default upon power-up.
- default: 0
Brightness (กำหนดค่าความสว่าง) (ต้องตั้งค่าตามความเหมาะสมขึ้นอยู่กับสถานที่)
- Sets the average brightness of the camera, can be between 0 and 255.
- default: 80

Min saturation (กำหนดขั้นต่ำค่าความอิ่มตัวของสีขณะกำหนด Signatures)
- Sets the minimum allowed color saturation for when generating color signatures. Applies during teaching.
- default: 10.0
Hue spread (กำหนดการรวมตัวของจุดสี)
- Sets how inclusive the color signatures are with respect to hue. Applies during teaching.
- default: 1.0
Saturation spread (กำหนดการรวมตัวของการอิ่มตัว)
- Sets how inclusive the color signatures are with respect to saturation. Applies during teaching.
- default: 1.0

Data out port (กำหนดพอร์ตสำหรับใช้ในการสื่อสาร) (ต้องตั้งค่าตามความเหมาะสม)
- Selects the port that's is used to output data. 0=SPI, 1=I2C, 2=UART, 3=analog/digital x, 4=analog/digital y
- default: 0
I2C address (กำหนดหมายเลขที่อยู่สำหรับการสื่อสารรูปแบบ I2C) (ต้องตั้งค่าตามความเหมาะสม)
- Sets the I2C address if you are using I2C data out port.
- default: 0x54
UART baudrate (กำหนดความเร็วการสื่อสารในรูปแบบ UART) (ต้องตั้งค่าตามความเหมาะสม)
- Sets the UART baudrate if you are using UART data out port.
- default: 19200

Pan P gain , Pan D gain , Tilt P gain , Tilt D gain (กำหนดขอบเขตการเคลื่อนที่ของการแพน Servo)

เมื่อทำการตั้งค่าส่วนต่างๆ เสร็จแล้ว (ตั้งตามค่าเดิมของโรงงานก่อน) เราต้องทำการทดสอบการประกอบกล้องกับขาตั้งนั้นใช้งานได้หรือไม่ โดยไปที่เมนู Action >Run pan/tilt demo (กำหนดสีที่ต้องการตรวจจับไว้ก่อนหน้านี้คือสีแดง)

การเชื่อมต่อระหว่างไมโครคอนโทรลเลอร์และกล้อง Pixy CMUcam5 ซึ่งในบทความนี้เราจะต่อกับ Arduino Mega 2560 เป็นไมโครคอนโทรลเลอร์เบอร์ Atmel ATmega2560 จัดเป็น MCU AVR อีกหนึ่งรุ่นที่มี I/O มาให้เยอะพอสมควรสามารถต่ออุปกณ์ภายนอกได้มากขึ้น
พอร์ตที่ใช้ในการเชื่อมต่อก็คือ I/O Port (SPI) ของกล้อง Pixy ลำดับแรกให้ทำการต่อสายที่บรรจุอยู่ภายในกล่องของ Pixy เข้ากับตัวกล้องทางพอร์ต I/O Port เชื่อมต่อไปยัง Arduino โดยการนำสายอีกฝั่งไปต่อกับกับ Arduino Mega 2560 ทางพอร์ต ICSP ดังภาพด้านล่าง

ต่อไปให้โหลด LibraryPixyFor Arduino และแตกไฟล์ไปยังโฟล์เดอร์ libraries ของ Arduino IDE ที่ได้ทำการลงไว้ ตัวอย่างเช่น C:\Program Files (x86)\Arduino\libraries

ตั้งค่าการเชื่อมต่อระหว่าง Arduino กับ Pixy โดยทำการเปิดโปรแกรม PixyMon > File > Configure… : Data out port= 0 (SPI) รายละเอียดการตั้งค่าสามารถดูได้จากข้อมูลด้านล่างของภาพ

0: SPI - this is the default port that uses 3 wires (pins 1, 3, and 4 of the I/O connector) and is used to communicate with Arduino
1: I2C - this is a multi-drop 2-wire port (pins 5 and 9 of the I/O connector) that allows a single master to communicate with up to 127 slaves (up to 127 Pixys).
2: UART - this is the common "serial port" (pins 1 and 4 of the I/O connector). Pixy receives data via pin 1 (input) and transmits data via pin 4 (output).
3: analog/digital x - this will output the x value of the largest detected object as an analog value between 0 and 3.3V (pin 3). It also outputs whether an object is detected or not as a digital signal (pin 1 of the I/O connector).
4: analog/digital y - this will output the y value of the largest detected object as an analog value between 0 and 3.3V (pin 3). It also outputs whether an object is detected or not as a digital signal (pin 1 of the I/O connector).

ทำการทดสอบการเชื่อมต่อระหว่าง Pixy และ Arduino โดยทำการเปิดโปรแกรม Arduino เลือกที่เมนู File > Example > Pixy > hello_world ทำการอัพโหลดไปยัง Arduino พร้อมทั้งเปิด Serial Monitor ไว้ดังภาพด้านล่าง

หลังจากนั้นไปยังหน้าจอ PixyMon ทำการกำหนดสีที่ต้องการตรวจจับและเลือกที่เมนู Action > Run default program ดังภาพด้านล่าง

กลับไปยังหน้าต่าง Serial Monitor ของ Arduino เราจะได้ข้มูลจากกล้อง Pixy ดังภาพด้านล่างสรุปได้ว่าการเชื่อมต่อสำเร็จ

รายละเอียดของข้อมูลที่ได้ออกมาประกอบไปด้วย
Detected 1:
block 0: sig: 1 x: 159 y: 109 width: 61 height: 61

Detected 1 คือ จำนวนที่ตรวจพบ จำนวน 1 กรอบ
block 0 คือ กรอบที่ 0
sig 1 คือ Signature ที่ 1
x คือ ตำแหน่งที่ตรวจพบในแนวแกน x
y คือ ตำแหน่งที่ตรวจพบในแนวแกน y
width คือ ความกว้างของสิ่งที่ตรวจพบ
height คือ ความสูงของสิ่งที่ตรวจพบ

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

>> Download Programming Code

>> Download Library

#include <SPI.h>   

#include <Pixy.h> 

#define X_CENTER    160L  

#define Y_CENTER    100L 

#define RCS_MIN_POS     0L 

#define RCS_MAX_POS     1000L 

#define RCS_CENTER_POS  ((RCS_MAX_POS-RCS_MIN_POS)/2) 

#define Forward  1 

#define Backward 0 

#define MOTOR_A 0 

#define MOTOR_B 1 

const byte PWMA = 3;  // PWM control (speed) for motor A

const byte PWMB = 11; // PWM control (speed) for motor B

const byte DIRA = 12; // Direction control for motor A

const byte DIRB = 13; // Direction control for motor B

ในส่วนแรกเป็นส่วนที่เราเรียกใช้ Library และกำหนดค่าเริ่มต้นต่างๆ ที่จำเป็นต้องใช้ในโปรแกรมรวมไปถึงขาที่ใช้เชื่อมต่อ โดยในที่นี้เราจะเรียกใช้ Library ด้วยกัน 2 ส่วน คือ ส่วนของ SPI และ Pixy หลังจากนั้นทำการกำหนดค่าและขาต่างๆ ที่นำไปใช้งาน

//---------------------------------------

// Servo Loop Class

// A Proportional/Derivative feedback

// loop for pan/tilt servo tracking of

// blocks.

// (Based on Pixy CMUcam5 example code)

//---------------------------------------

class ServoLoop

{

public:

    ServoLoop(int32_t proportionalGain, int32_t derivativeGain);

    void update(int32_t error);

    int32_t m_pos;

    int32_t m_prevError;

    int32_t m_proportionalGain;

    int32_t m_derivativeGain;

};

// ServoLoop Constructor

ServoLoop::ServoLoop(int32_t proportionalGain, int32_t derivativeGain)

{

    m_pos = RCS_CENTER_POS;

    m_proportionalGain = proportionalGain;

    m_derivativeGain = derivativeGain;

    m_prevError = 0x80000000L;

}

// ServoLoop Update 

// Calculates new output based on the measured

// error and the current state.

void ServoLoop::update(int32_t error)

{

    long int velocity;

    char buf[32];

    if (m_prevError!=0x80000000)

    {   

        velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;

        m_pos += velocity;

        if (m_pos>RCS_MAX_POS) 

        {

            m_pos = RCS_MAX_POS; 

        }

        else if (m_pos<RCS_MIN_POS) 

        {

            m_pos = RCS_MIN_POS;

        }

    }

    m_prevError = error;

}

// End Servo Loop Class

//---------------------------------------

ส่วนต่อมาคือส่วนที่ใช้ในการควบคุม Servo เป็นส่วนที่ใช้ในการคำณวนและควบคุมตำแหน่งของ Servo สามารถดูข้อมูลได้ที่ Example > Pixy > pantilt

1

2

3

4

Pixy pixy;  // Declare the camera object

ServoLoop panLoop(200, 200);  // Servo loop for pan

ServoLoop tiltLoop(200, 200); // Servo loop for tilt

ในส่วนนี้เป็นประกาศชื่อ Object เข้ากับ Library และกำหนดขอบเขตการ Pan Servo

//---------------------------------------

// Setup - runs once at startup

//---------------------------------------

void setup()

{

    Serial.begin(9600);

    Serial.print("Starting...\n");

    pixy.init();

      setupArdumoto(); //ฟังก์ชั่นกำหนด Pin และโหมดการทำงานของ Motor

}

ส่วนของ Setup เป็นส่วนที่ใช้ในกำหนดการทำงานต่างๆ เช่น กำหนดโหมดการทำงานให้เป็น Input/output เป็นต้น พร้อมทั้งสั่งเริ่มต้นการทำงาน

uint32_t lastBlockTime = 0;

//---------------------------------------

// Main loop - runs continuously after setup

//---------------------------------------

void loop()

{ 

    uint16_t blocks;

    blocks = pixy.getBlocks();

    // If we have blocks in sight, track and follow them

    if (blocks)

    {

        int trackedBlock = TrackBlock(blocks);

        FollowBlock(trackedBlock);

        lastBlockTime = millis();

    }  

    else if (millis() - lastBlockTime > 100)

    {

            stopArdumoto(MOTOR_A);

            stopArdumoto(MOTOR_B);

        ScanForBlocks();

    }

}

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

int oldX, oldY, oldSignature;

//---------------------------------------

// Track blocks via the Pixy pan/tilt mech

// (based in part on Pixy CMUcam5 pantilt example)

//---------------------------------------

int TrackBlock(int blockCount)

{

    int trackedBlock = 0;

    long maxSize = 0;

    Serial.print("blocks =");

    Serial.println(blockCount);

    for (int n = 0; n < blockCount; n++)

    {

        if ((oldSignature == 0) || (pixy.blocks[n].signature == oldSignature))

        {

            long newSize = pixy.blocks[n].height * pixy.blocks[n].width;

            if (newSize > maxSize)

            {

                trackedBlock = n;

                maxSize = newSize;

            }

        }

    }

    int32_t panError = X_CENTER - pixy.blocks[trackedBlock].x;

    int32_t tiltError = pixy.blocks[trackedBlock].y - Y_CENTER;

    panLoop.update(panError);

    tiltLoop.update(tiltError);

    pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);

    oldX = pixy.blocks[trackedBlock].x;

    oldY = pixy.blocks[trackedBlock].y;

    oldSignature = pixy.blocks[trackedBlock].signature;

    return trackedBlock;

}

ฟังก์ชั่นในการตรวจสอบวัตถุ เป็นฟังก์ชั่นที่รับข้อมูลมาจากฟังก์ชั่น main และนำข้อมูลที่ได้มาทำการตรวจสอบว่าข้อมูลที่ได้รับมานั้นมีอะไรบ้าง หลังจากตรวจสอบก็ทำการคำนวณและส่งข้อมูลไปยังฟังก์ชั่นในการควบคุม Servo และมอเตอร์ขับเคลื่อนต่อไป

//---------------------------------------

// Follow blocks

// This code makes the robot base turn 

// and move to follow the pan/tilt tracking

// of the head.

//---------------------------------------

int32_t size = 500;

void FollowBlock(int trackedBlock)

{

    int32_t followError = RCS_CENTER_POS - panLoop.m_pos;  // How far off-center are we looking now?

    // Size is the area of the object.

    // We keep a running average of the last 8.

    size += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height; 

    size -= size >> 3;

    // Forward speed decreases as we approach the object (size is larger)

    int forwardSpeed = constrain(500 - (size/256), -250, 250);  

    // Steering differential is proportional to the error times the forward speed

    int32_t differential = (followError + (followError * forwardSpeed))>>8;

    // Adjust the left and right speeds by the steering differential.

    int leftSpeed = constrain(forwardSpeed + differential, -250, 250);

    int rightSpeed = constrain(forwardSpeed - differential, -250, 250);

    // And set the motor speeds

        if(leftSpeed > 0)

        {

          driveArdumoto(MOTOR_A, Backward, leftSpeed);

        }

        if(rightSpeed > 0)

        {

          driveArdumoto(MOTOR_B, Backward, rightSpeed);

        }

        if(leftSpeed < 0)

        {

          driveArdumoto(MOTOR_A, Forward, (leftSpeed*-1));

        }

        if(rightSpeed < 0)

        {

          driveArdumoto(MOTOR_B, Forward, (rightSpeed*-1));

        }

}

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

//---------------------------------------

// Random search for blocks

//

// This code pans back and forth at random

// until a block is detected

//---------------------------------------

int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;

uint32_t lastMove = 0;

void ScanForBlocks()

{

    if (millis() - lastMove > 20)

    {

        lastMove = millis();

        panLoop.m_pos += scanIncrement;

        if ((panLoop.m_pos >= RCS_MAX_POS)||(panLoop.m_pos <= RCS_MIN_POS))

        {

            tiltLoop.m_pos = random(RCS_MAX_POS * 0.6, RCS_MAX_POS);

            scanIncrement = -scanIncrement;

            if (scanIncrement < 0)

            {

                                driveArdumoto(MOTOR_A, Backward, 150);

                                driveArdumoto(MOTOR_B, Forward, 150);

            }

            else

            {

                                driveArdumoto(MOTOR_A, Forward, 150);

                                driveArdumoto(MOTOR_B, Backward, 150);

            }

            delay(random(250, 500));

        }

        pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);

    }

}

ฟังก์ชั่นในการค้นหาวัตถุ เป็นฟังก์ชั่นที่ใช้ในการสั่งงาน Servo และมอเตอร์เมื่อไม่พบวัตถุจะทำการสุ่มการเคลื่อนไหวและเคลื่อนที่เพื่อตรวจหาวัตถุที่ต้องการต่อไป

void driveArdumoto(byte motor, byte dir, byte spd)

{

  if (motor == MOTOR_A)

  {

    digitalWrite(DIRA, dir);

    analogWrite(PWMA, spd);

  }

  else if (motor == MOTOR_B)

  {

    digitalWrite(DIRB, dir);

    analogWrite(PWMB, spd);

  }  

}

ฟังก์ชั่นควบคุมบอร์ดไดร์ฟหรือมอเตอร์ เป็นฟังก์ชั่นที่ใช้ในการสั่งงานและควบคุมความเร็วของมอเตอร์

1

2

3

4

void stopArdumoto(byte motor)

{

  driveArdumoto(motor, 0, 0);

}

ฟังก์ชั่นที่ใช้ในการหยุดมอเตอร์

void setupArdumoto()

{

  // All pins should be setup as outputs:

  pinMode(PWMA, OUTPUT);

  pinMode(PWMB, OUTPUT);

  pinMode(DIRA, OUTPUT);

  pinMode(DIRB, OUTPUT);

  // Initialize all pins as low:

  digitalWrite(PWMA, LOW);

  digitalWrite(PWMB, LOW);

  digitalWrite(DIRA, LOW);

  digitalWrite(DIRB, LOW);

}

ฟังก์ชั่นที่ใช้ในการกำหนดขาและโหมดในการทำงานของส่วนที่ใช้ในการควบคุมมอเตอร์
วิธีการทดสอบ
ให้ทำการประกอบส่วนต่างๆเข้าด้วยกันดังนี้
1. ประกอบโครงรถ
2. นำ Shield Motor L298 ต่อเข้ากับ Arduino Mega2560
3. ต่อมอเตอร์ล้อขวาต่อเข้ากับ Shield ทางช่อง Motor A
4. ต่อมอเตอร์ล้อซ้ายต่อเข้ากับ Shield ทางช่อง Motor B
5. ต่อกล้อง Pixy + ขาตั้ง เข้ากับ Arduino Mega2560 ทางพอร์ต ICSP ดังภาด้านล่าง

จากโค้ดด้านบนเราจะเห็นได้ว่าการทำงานของโค้ดนี้คือ เมื่อทำการ Set signature ที่ต้องการตัวกล้อง Pixy นั้นจะส่งข้อมูลออกมาก็ต่อเมื่อมีการเรียกใช้ข้อมูล เมื่อได้ข้อมูลมานั้นเราต้องทำการตรวจสอบเพื่อหาขนาดและตำแหน่งของวัตถุที่พบ พร้อมทั้งสั่งงานให้ส่วนของ Servo และมอเตอร์ให้เคลื่อนที่ตามที่ได้รับข้อมูลมา
วิธีการทดสอบ ให้ทำการอัพโหลดโค้ดด้านบนไปยัง Arduino Mega2560 พร้อมทั้งเชื่อมต่อกล้องไปยังคอมพิวเตอร์และเปิดโปรแกรม PixyMon หลังจากนั้นทำการ Set signature และเลือกไปที่เมนู Run default program เมื่อกล้องทำงานให้ทำการถอดสายกล้องและ Arduino ออก พร้อมทั้งหาแหล่งจ่ายไฟมาจ่ายเข้ายังช่องด้านบนของ Shield ตัวรถก็พร้อมทำงาน ดูวิธีการได้จาก VDO ด้านล่าง

พีรัชชัย โอรมย์

วันอังคารที่ 18 กันยายน พ.ศ. 2561

โปรเจครถติดตามวัตถุ (CAR TRACKING)

โปรเจครถติดตามวัตถุ (CAR TRACKING) งานที่14

ไม่มีความคิดเห็น:

แสดงความคิดเห็น

งานที่ 5

รายงานการละเมิด