โปรเจครถติดตามวัตถุ (Car Tracking)พัฒนาโดยโมดูลกล้อง Pixy2 และบอร์ด Arduino Mega 2560 ตอนที่ 2
ต่อเนื่องหลังจากที่ได้ทำการติดตั้งในส่วนของกล้องเข้ากับตัว Pan-tilt เรียบร้อยแล้ว ในบทความนี้เราจะนำข้อมูลไปทำการเขียนโปรแกรมควบคุมรถให้ทำการติดตามวัตถุที่เราต้องการ
ขั้นตอนการตั้งค่าการตรวจจับสี สามารถแบ่งออกเป็น 2 แบบ คือ
– Single object tracking คือ การตรวจจับสีเพียงสีเดียวบนวัตถุ
– Color codes object tracking คือ การตรวจจับสีหลายสีบนวัตถุ
1. การตั้งค่ากล้อง Pixy2 แบบ Single object tracking ก่อนอื่นทำการตั้งค่าเป็นค่าเริ่มต้นไปที่ File > Restore default parameter ดังภาพด้านล่าง

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


จะเห็นว่าเมื่อมีการตรวจจับสีที่ตั้งค่าได้ จะแสดงให้เห็นการตรวจจับวัตถุ

เมื่อกล้องตรวจจับวัตถุได้แล้ว ต่อไปคือทดสอบ Run ตัวอย่าง pan_tilt_demo เพื่อทดสอบการหมุนตามวัตถุของ Servo Motor โดยไปที่ เมนู Program > pan_tilt_demo จะเห็นได้ว่า Servo motor จะหมุนไปตามวัตถุที่เคลื่อนไหว

2. การตั้งค่ากล้อง Pixy2 แบบ Color codes object tracking ก่อนอื่นทำการตั้งค่าเป็นค่าเริ่มต้นไปที่ File > Restore default parameter
จากนั้นให้ทำการเลือกสีที่ 1 ของวัตถุที่ต้องการจะตรวจจับโดยไปที่เมนู Action > เลือก Set CC signature 1 และทำการคลิ๊กเม้าท์ไปบริเวณของสีบน 1 บนวัตถุ


ทำการเลือกสีที่ 2 ของวัตถุที่ต้องการจะตรวจจับโดยไปที่เมนู Action > เลือก Set CC signature 2 และทำการคลิ๊กเม้าท์ไปบริเวณของสีบน 2 บนวัตถุ


จะเห็นว่าต้องมีการตรวจจับสีของวัตถุได้ทั้ง 2 สี จึงแสดงให้เห็นการตรวจจับวัตถุนั้น

เมื่อกล้องตรวจจับวัตถุได้แล้ว ต่อไปคือทดสอบ Run ตัวอย่าง pan_tilt_demo เพื่อทดสอบการหมุนตามวัตถุของ Servo Motor โดยไปที่ เมนู Program > pan_tilt_demo


จะเห็นได้ว่า Servo motor จะหมุนไปตามวัตถุที่เคลื่อนไหว

ขั้นตอนต่อไปให้ทำการประกอบตัวกล้อง Pixy2 เข้ากับตัวรถ ที่ต่อ Arduino Mega 2560 กับ Ardumoto โดยต่อพ่วงมอเตอร์ซ้ายและขวา กับตัว Ardumoto และต่อ Pixy2 เข้ากับ ICSP ต้องตัวบอร์ด Arduino Mega2560 ผ่าน Ardumoto ที่เสียบอยู่ด้านบน ดังภาพ

จากนั้นทำการต่อแบตเตอร์รี่ไปที่ Ardumoto ซึ่งจะจ่ายไฟฟ้าให้กับทั้งตัวบอร์ด Arduino Mega 2560 , ตัว Pixy2 และ มอเตอร์ทั้งสองตัว โดยให้ต่อขั้วบวกจากรางถ่านไปยังขั้วบวกของ Ardumoto และ ต่อขั้วลบจากรางถ่านไปยังขั้วลบของ Ardumoto ดังภาพ

ภาพโดยรวมหลังจากเชื่อมต่ออุปกรณ์ทุกตัวเรียบร้อยแล้ว


การตั้งค่าในส่วนของ Arduino และการอัพโหลดโค้ด
เมื่อทำการตั้งค่าตัวกล้อง Pixy2 กับและ Run ตัวอย่าง pan_tilt_demo แบบ Single object tracking หรือ Color codes object tracking อย่างใดอย่างหนึ่ง หลังจากนั้นให้ทำการติดตั้ง Pixy2 Library ให้เรียบร้อยแล้ว ขั้นตอนต่อไปคือการอัพโหลดโค้ดรถติดตามวัตถุลงบน Arduino Mega 2560 ด้วย Arduino IDE
โดยเข้าไปที่ File >> Examples >> Pixy2 >> ccc_zumo_chase

จากนั้นทำการเลือกประเภทของบอร์ด และ Com Port ให้ถูกต้อง และทำการ Upload โปรแกรม

จะเห็นได้ว่าตัวหุ่นจะวิ่งตามวัตถุที่เราได้ทำการตั้งค่าให้ตัวกล้อง Pixy2 ตรวจจับไว้ก่อนหน้านี้ ดังภาพ ใช้การตั้งค่ากล้อง Pixy2 แบบ Single object tracking

>> คำอธิบายการทำงานของโค้ดในแต่ละส่วนที่สำคัญ <<
ทำการเพิ่ม Library ที่ต้องการจะใช้งาน สามารถแก้ไขขา PWM และ DIR ได้ใน ZumoMotors.cpp หากใช้ Motor Driver ตัวอื่น
#include <Pixy2.h> #include <PIDLoop.h> #include <ZumoMotors.h>
กำหนดความเร็วของตัวหุ่นได้ จนถึงค่าความเร็วสูงสุดที่ 400 กำหนดค่าขนาดความกว้างของ Block ที่เข้าใกล้หรือไกล และกำหนดช่วงของการวิ่งไปทางตรงตั้งแต่ -150 ถึง 150
#define MAX_TRANSLATE_VELOCITY 250 // this limits how fast Zumo travels forward (400 is max possible for Zumo) #define Close_block_size 190 // too Close block size #define Far_block_size 160 // too Far block size #define straight_grap 150 // Straight Grap Value for go straight
ประกาศค่า PID Loop สามารถปรับแต่งได้ทั้งบนโปรแกรม PixyMon หรือบนตัวโค้ดได้
Pixy2 pixy; ZumoMotors motors; PIDLoop panLoop(350, 0, 600, true); PIDLoop tiltLoop(500, 0, 700, true); PIDLoop rotateLoop(300, 600, 300, false); PIDLoop translateLoop(400, 800, 300, false);
Setup เป็นส่วนที่ใช้ในกำหนดการทำงานต่างๆ เช่น กำหนดโหมดการทำงานให้เป็น Input/output เป็นต้น พร้อมทั้งสั่งเริ่มต้นการทำงาน
void setup() { Serial.begin(115200); Serial.print("Starting...\n"); // initialize motor objects motors.setLeftSpeed(0); motors.setRightSpeed(0); // need to initialize pixy object pixy.init(); // user color connected components program pixy.changeProg("color_connected_components"); }
ส่วนนี้จะเป็นฟังก์ชั่นสำหรับการเช็คจำนวน Block และค่า index และคำนวนค่าความกว้างของวัตถุ
int16_t acquireBlock() { if (pixy.ccc.numBlocks && pixy.ccc.blocks[0].m_age > 30) return pixy.ccc.blocks[0].m_index; return -1; } int Width; Block *trackBlock(uint8_t index) { uint8_t i; for (i = 0; i < pixy.ccc.numBlocks; i++) { Serial.print("Width: "); Serial.print(pixy.ccc.blocks[i].m_width); Width = pixy.ccc.blocks[i].m_width; if (index == pixy.ccc.blocks[i].m_index) return &pixy.ccc.blocks[i]; } return NULL; }
โค้ดในส่วน Loop จะเป็นส่วนของการทำงานวนซ้ำแบบไม่รู้จบ โดยจะเริ่มจากการตรวจจับจำนวน Block จากนั้นจะเข้าสู่การทำงานในส่วนของคำสั่งขับเคลื่อนมอเตอร์ให้หันไปตามทิศทางของวัตถุ ที่ตัวกล้องตรวจจับได้ โดยจะใช้ค่า Close_block_size สำหรับคำนวนในการถอยหลัง Far_block_size จะใช้สำหรับคำนวนในการวิ่งตามวัตถุ และ straight_grap จะใช้สำหรับการวิ่งตรงเข้าหาวัตถุ
ค่าจำเป็นที่ใช้ในการคำนวนตำแหน่งวัตถุคือ rotateLoop.m_command ค่าตำแหน่งวัตถุในแนวราบ และ translateLoop.m_command ค่าตำแหน่งวัตถุในแนวตั้ง
void loop() { static int16_t index = -1; int32_t panOffset, tiltOffset, headingOffset, left, right; Block *block = NULL; pixy.ccc.getBlocks(); if (index == -1) // search.... { Serial.println("Searching for block..."); index = acquireBlock(); if (index >= 0) Serial.println("Found block!"); } // If we've found a block, find it, track it if (index >= 0) block = trackBlock(index); // If we're able to track it, move motors if (block) { // calculate pan and tilt errors panOffset = (int32_t)pixy.frameWidth / 2 - (int32_t)block->m_x; tiltOffset = (int32_t)block->m_y - (int32_t)pixy.frameHeight / 2; // calculate how to move pan and tilt servos panLoop.update(panOffset); tiltLoop.update(tiltOffset); // move servos pixy.setServos(panLoop.m_command, tiltLoop.m_command); // calculate translate and rotate errors panOffset += panLoop.m_command - PIXY_RCS_CENTER_POS; tiltOffset += tiltLoop.m_command - PIXY_RCS_CENTER_POS - PIXY_RCS_CENTER_POS / 2 + PIXY_RCS_CENTER_POS / 8; rotateLoop.update(panOffset); translateLoop.update(-tiltOffset); // keep translation velocity below maximum if (translateLoop.m_command > MAX_TRANSLATE_VELOCITY) translateLoop.m_command = MAX_TRANSLATE_VELOCITY; // for calculate Vertical angle // set wheel velocities if (Width < Close_block_size && Width > Far_block_size ) { motors.setLeftSpeed(0); motors.setRightSpeed(0); rotateLoop.reset(); translateLoop.reset(); } else if (Width >= Close_block_size ) { // if Width value more than 90 too close if (rotateLoop.m_command >= -straight_grap && rotateLoop.m_command <= straight_grap) { motors.setLeftSpeed(-MAX_TRANSLATE_VELOCITY); motors.setRightSpeed(-MAX_TRANSLATE_VELOCITY); } else if (rotateLoop.m_command <= -straight_grap ) { motors.setLeftSpeed(0); motors.setRightSpeed(-MAX_TRANSLATE_VELOCITY); } else if (rotateLoop.m_command >= straight_grap ) { motors.setLeftSpeed(-MAX_TRANSLATE_VELOCITY); motors.setRightSpeed(0); } } else if (Width <= Far_block_size) { // if Width value less than 60 too far if (rotateLoop.m_command >= -straight_grap && rotateLoop.m_command <= straight_grap) { motors.setLeftSpeed(MAX_TRANSLATE_VELOCITY); motors.setRightSpeed(MAX_TRANSLATE_VELOCITY); } else if (rotateLoop.m_command <= -straight_grap ) { motors.setLeftSpeed(MAX_TRANSLATE_VELOCITY); motors.setRightSpeed(0); } else if (rotateLoop.m_command >= straight_grap ) { motors.setLeftSpeed(0); motors.setRightSpeed(MAX_TRANSLATE_VELOCITY); } } Serial.print(" rotateLoop : "); Serial.print(rotateLoop.m_command); // Horizontal value Serial.print(" translateLoop : "); Serial.println(translateLoop.m_command); // Vertical value } else // no object detected, stop motors, go into search state { rotateLoop.reset(); translateLoop.reset(); motors.setLeftSpeed(0); motors.setRightSpeed(0); index = -1; // set search state } }