แขนจับ Gripper-x
แขนจับ Gripper-X ประกอบไปด้วยเซอร์โวมอเตอร์ 2 ตัว ตัวแรกเชื่อมต่อกับโครงสร้างที่ทำหน้าที่หนีบ อีกตัวต่อเข้ากับโครงสร้างที่ทำหน้าที่ยก
การทำงานของเซอร์โวมอเตอร์
เซอร์โวมอเตอร์คือมอเตอร์พร้อมชุดเฟืองขับเช่นเดียวกับมอเตอร์ขับเคลื่อน แต่เซอร์โวมอเตอร์จะมีวงจรอิเล็กทรอนิกส์ควบคุมอยู่ภายในตัว ทำหน้าที่ระบุตำแหน่งของเซอร์โวมอเตอร์ ซึ่งจะเคลื่อนที่ไปยังตำแหน่งที่กำหนดเท่านั้น ไม่ได้เคลื่อนที่อย่างต่อเนื่องเหมือนชุดเฟืองขับมอเตอร์ทั่วไป ระยะการเคลื่อนที่ของเซอร์โวมอเตอร์จะอยู่ประมาณ 180 องศา
เซอร์โวมอเตอร์ MG90S เซอร์โวมอเตอร์ขนาดเล็กเฟืองโลหะคุณสมบัติ• หนัก: 13.4 g |
การควบคุมเซอร์โวมอเตอร์
เซอร์โวมอเตอร์จะมีสายออกมา 3 เส้นประกอบด้วยไฟบวก
ไฟลบ(กราวด์) และสัญญาณควบคุม ซึ่งการป้อนสัญญาณไปควบคุมจะใช้หลักการความกว้างพัลส์ เพื่อไปควบคุมตำแหน่งการเคลื่อนที่ของเซอร์โวมอเตอร์
การติดตั้งแขนจับ Gripper X
รายการอุปกรณ์
ขั้นตอนการประกอบ
1. ที่ฐานของ POP-X2 Rover นำเสารองสกรู ยึดด้วยนอต 3 มม. ในตำแหน่งดังรูป
2. นำ Gripper-X วางลงให้ตำแหน่งรูตรงกับตำแหน่งเสารองสกรู จากนั้นใช้สกรู 3×6 มม. ขันยึดให้แน่น
3. เสียบสายเซอร์โวตัวหนีบเข้าที่ช่อง SV1 เสียบสายเซอร์โวตัวยกเข้าที่ช่อง SV2 สังเกตสายสีน้ำตาล(กราวด์) ให้ตรงกับจุดต่อสีดำบนบอร์ด POP-X2
ชุดคำสั่งสำหรับควบคุมเซอร์โวมอเตอร์รูปแบบservo(CH,POS); CH ช่องที่ใช้ (เสียบสาย) 1-3 POS ตำแหน่งเซอร์โวมอเตอร์ = 0-180 หรือ -1 -1 หมายถึงหยุดจ่ายสัญญาณให้เซอร์โว เซอร์โวจะไม่ล็อกแกน |
ตัวอย่างที่ 28 ทดสอบหาตำแหน่งของเซอร์โวหนีบ (SV1)
เพื่อให้ทราบค่าตัวเลของศา ที่จะส่งให้เซอร์โวกางออกและหุบเข้า จะใช้ตัวต้านทานปรับค่าได้ knob() ในการปรับค่าแล้วนำค่าแสดงหน้าจอ พร้อมทั้งส่งเป็นค่า POS ไปควบคุมเซอร์โวด้วย
ดังนั้นเมื่อหมุน knob ตำแหน่งเซอร์โวก็จะเปลี่ยนตาม ทำการจดบันทึกค่าเมื่อเซอร์โวกางสุดและค่าเมื่อแขนเซอร์โวเข้ามาชนกันพอดี
#include <popx2.h> int x; void setup(){ setTextSize(3); glcdMode(3); } void loop(){ x=knob(180); servo(1,x); glcd(1,3,"%d ",x); }
การทดสอบ
เมื่อโหลดโค้ดเรียบร้อยหน้าจอจะแสดงค่าตัวเลข ซึ่งเป็นค่าที่จะส่งไปควบคุมตำแหน่งเซอร์โวด้วย ทดลองปรับ knob จนปลายแขนมาชนกันพอดี สังเกตค่าที่หน้าจอ GLCD จากนั้นปรับ knob อีกครั้งให้แขนกางออก สังเกตค่าที่หน้าจอ GLCD ทำการบันทึกค่าเอาไว้
ตัวอย่างที่ 29 ทดสอบหาตำแหน่งของเซอร์โวยก (SV2)
ทำแบบเดียวกันกับตัวอย่างที่ 28 ครับ แต่เปลี่ยนตำแหน่งเซอร์โวเป็น SV2 ที่ทำหน้าที่ยกขึ้นและวางแทน บันทึกค่าที่ได้เอาไว้ (ค่าอาจจะแตกต่างกันสำหรับ Gripper-X แต่ละตัว)
#include <popx2.h> int x; void setup(){ setTextSize(3); glcdMode(3); } void loop(){ x=knob(180); servo(2,x); glcd(1,3,"%d ",x); }
ตัวอย่างที่ 30 การสร้างฟังก์ชั่นควบคุมการคีบและวางกระป๋อง
ตัวอย่างจะทดสอบการคีบกระป๋องและวาง (เลือกใช้กระป๋องกาแฟ) โดยขั้นตอนการคีบและวางจะต้องสั่งเซอร์โวมอเตอร์ทำงานหลายท่าทาง ดังนั้นจึงสร้างออกมาเป็นฟังก์ชั่นชื่อ Keep() สำหรับเก็บกระป๋องและยกมาไว้ที่ตัว ฟังก์ชั่นชื่อ Place() สำหรับนำกระป๋องที่เก็บลงมาวางและยกแขนขึ้นเพื่อเก็บแขน Gripper-X
#include <popx2.h> void setup(){ servo(1,25);delay(300); // กาง servo(2,10);delay(300); // ยก } void loop(){ sw1_press();Keep(); // เก็บกระป๋อง sw1_press();Place(); // วางกระป๋อง } void Keep(){ servo(2,110);delay(300); // วาง servo(1,80);delay(300); // คีบ servo(2,10);delay(300); // ยก } void Place(){ servo(2,110);delay(300); // วาง servo(1,25);delay(300); // กาง servo(2,10);delay(300); // ยก }
สำหรับตัวเลขที่ระบุในโค้ดสำหรับการทำงานของเซอร์โวแต่ละจุด ประกอบไปด้วย Servo1 คีบและกาง
Servo2 วางและยก จะต้องนำค่าตัวเลขที่ทดสอบแล้วจากตัวอย่างก่อนหน้านี้มาป้อนเข้าไป หลังจากส่งคำสั่งไปที่เซอร์โวแล้วจะต้องตามด้วยการหน่วงเวลาจากการเคลื่อนที่จากจุดหนึ่งไปยังจุดหนึ่ง โดยค่าประมาณที่เหมาะสมเลือกใช้ 0.3 วินาที
ขณะเริ่มต้น จะต้องกำหนดให้เซอร์โวเตรียมพร้อม โดยยกแขนขึ้นแล้วกางแขนออกด้วย
การเรียกใช้งานฟังก์ชั่น จะเรียกจากการกดสวิตช์ SW1 กด 1 ครั้งเรียกฟังก์ชั่น Keep() กดอีกครั้งเรียกฟังก์ชั่น Place() วนไปเรื่อยๆ
การทดสอบ
วางกระป๋องเปล่าไว้ด้านหน้าของ POP-X2 Rover จากกดสวิตช์ SW1 แขนจับจะต้องคีบและยกกระป๋องขึ้น
เมื่อกดสวิตช์ SW1 อีกครั้ง แขนจับจะต้องวางกระป๋องลง กางแขนออกและยกแขนกลับไปที่เดิม
ข้อควรระวังไม่ควรปล่อยให้เซอร์โวมอเตอร์ค้างอยู่ที่ตำแหน่งนอกเหนือจากที่ตรวจสอบไว้ก่อนหน้านี้เป็นเวลานาน อาจทำให้เซอร์โวมอเตอร์เสียหายได้ |
ตัวอย่างที่ 31 ปรับความเร็วการเคลื่อนที่ของเซอร์โวมอเตอร์
การยกและวางที่เร็วไปทำให้เกิดแรงเหวี่ยง กระป๋องที่จับอยู่อาจจะหลุดออกได้ง่าย ตัวอย่างนี้จะใช้คำสั่ง for เพื่อวนลูปค่อยๆ ส่งค่าไปควบคุมเซอร์โวมอเตอร์ ซึ่งจะคั่นด้วยการหน่วงเวลา แล้วเอาทั้งหมดไปสร้างเป็นฟังก์ชั่น ตั้งชื่อว่า servo2 ซึ่งออกแบบมาใช้กับการยกและวางเท่านั้น
ฟังก์ชั่น servo2(pos1,pos2,speeds)
pos1 ตำแหน่งเริ่มต้นของเซอร์โวมอเตอร์
pos2 ตำแหน่งที่ต้องการเคลื่อนที่ไปของเซอร์โวมอเตอร์
speeds ค่าเวลาหน่วงที่ใช้เพื่อลดความเร็วเซอร์โวมอเตอร์
void servo2(int pos1,int pos2,int speeds){ int i; if (pos1>pos2){ for (i=pos1;i>pos2;i--){ servo(2,i);delay(speeds); } } else{ for (i=pos1;i<pos2;i++){ servo(2,i);delay(speeds); } } }
การทำงานของฟังก์ชั่น
ฟังก์ชั่น servo2 เมื่อรับค่าตำแหน่งมาแล้ว จะต้องตรวจสอบว่า ค่าเริ่มต้นกับปลายทาง ค่าใดมากกว่ากัน
ถ้าค่าเริ่มต้นมากกว่า คำสั่ง for จะต้องวนลูปแบบลดค่า
ถ้าค่าเริ่มต้นน้อยกว่า คำสั่ง for จะค่อยๆ เพิ่มค่า
จากนั้นส่งค่าตำแหน่งเซอร์โวออกไปตามด้วยค่าการหน่วงเวลา ค่ามากการเคลื่อนที่จะช้าลงมาก
การนำฟังก์ชั่นมาใช้งาน (ปรับโค้ดจากตัวอย่างที่ 30)
#include <popx2.h> void setup() { servo(1, 25); delay(300); // กาง servo(2, 10); delay(300); // ยก } void loop() { sw1_press(); Keep(); // เก็บกระป๋อง sw1_press(); Place(); // วางกระป๋อง } void Keep() { servo2(10, 110, 10); delay(300); // วาง servo(1, 80); delay(300); // คีบ servo2(110, 10, 10); delay(300); // ยก } void Place() { servo2(10, 110, 10); delay(300); // วาง servo(1, 25); delay(300); // กาง servo2(110, 10, 10); delay(300); // ยก } void servo2(int pos1, int pos2, int speeds) { int i; if (pos1 > pos2) { for (i = pos1; i > pos2; i--) { servo(2, i); delay(speeds); } } else { for (i = pos1; i < pos2; i++) { servo(2, i); delay(speeds); } } }
ตัวอย่างที่ 32 เคลื่อนที่ตามเส้นย้ายกระป๋อง
จากรูปแบบเส้นทางเดินในรูป POP-X2 Rover จะต้องเคลื่อนที่ตามเส้นไปคีบกระป๋องในพื้นที่สีเขียวแล้วเคลื่อนตามเส้นต่อเพื่อไปวางบริเวณเส้นประ ต้องนำความรู้จากบทที่ 6 ควบคู่กับตัวอย่างในบทนี้มาทำงานร่วมกัน
สร้างฟังก์ชั่นเคลื่อนที่ตามเส้นเก็บกระป๋อง
void Lift() { while (1) { L = analog(4); R = analog(2); if (L > 630 && R > 640) {fd(80);} else if (L < 630 && R > 640) {sl(80);} else if (L > 630 && R < 640) {sr(80);} else if (L < 630 && R < 640) { bk(100);delay(100);ao(); Keep(); while (analog(2) < 640) { bk(30);} while (analog(2) > 640) { bk(30);} delay(100);ao(); break; } } }
การทำงานของฟังก์ชั่น Lift()
เป็นการนำเอาฟังก์ชั่นเคลื่อนที่ตามเส้นแบบเดียวกับฟังก์ชั่น RRR() ในบทที่ 6 แต่หลังจากเจอเส้นตัดสีดำแล้ว จะสั่งถอยหลังแบบทันทีทันใดช่วงเวลาสั้นๆ เพื่อเบรคแล้วหยุด จากนั้นเรียกฟังก์ชั่น Keep() เพื่อเก็บกระป๋อง แล้วเคลื่อนที่ถอยหลัง โดยขณะเคลื่อนที่ตรวจสอบเส้นไปด้วย ซึ่งจะถอยหลังไปจนเจอเส้นแล้วถอยต่อไปอีก 0.1 วินาที แล้วหยุด เป็นการจบภารกิจการเคลื่อนที่ตามเส้นเพื่อเก็บกระป๋อง โดยตั้งชื่อฟังก์ชั่นนี้ว่า Lift()
สร้างฟังก์ชั่นเคลื่อนที่ตามเส้นวางกระป๋อง
void Release() { while (1) { L = analog(4); R = analog(2); if (L > 630 && R > 640) {fd(80);} else if (L < 630 && R > 640) {sl(80);} else if (L > 630 && R < 640) {sr(80);} else if (L < 630 && R < 640) { bk(100);delay(100);ao(); Place(); while (analog(2) < 640) { bk(30);} while (analog(2) > 640) { bk(30);} delay(100);ao(); break; } } }
การทำงานของฟังก์ชั่น Release()
หลังจากเคลื่อนที่ตามเส้นจบพบเส้นตัด จะสั่งให้หยุด (พร้อมเบรค) จากนั้นเรียกฟังก์ชั่นวางกระป๋อง ถอยหลังไปจนเจอเส้น เป็นการจบภารกิจ
โค้ดที่เสร็จสมบูรณ์
#include <popx2.h> int L, R; void setup() { servo(1, 25); delay(300); // กาง servo(2, 10); delay(300); // ยก OK(); RRR(); RRR(); Lift(); RRR(); LLL(); RRR(); RRR(); Release(); RRR(); LLL(); ao(); } void loop() { } void Keep() { // เก็บกระป๋อง servo2(10, 110, 10); delay(300); // วาง servo(1, 80); delay(300); // คีบ servo2(110, 10, 10); delay(300); // ยก } void Place() { // วางกระป๋อง servo2(10, 110, 10); delay(300); // วาง servo(1, 25); delay(300); // กาง servo2(110, 10, 10); delay(300); // ยก } void Release() { // เคลื่อนที่ตามเส้นวางกระป๋อง while (1) { L = analog(4); R = analog(2); if (L > 630 && R > 640) { fd(80); } else if (L < 630 && R > 640) { sl(80); } else if (L > 630 && R < 640) { sr(80); } else if (L < 630 && R < 640) { bk(100); delay(100); ao(); // เบรค Place(); // วางกระป๋อง while (analog(2) < 640) { // ถอยหลังจนเจอพื้นสีขาว bk(30); } while (analog(2) > 640) { // ถอยหลังจนเจอเส้นสีดำ bk(30); } delay(100); ao(); break; } } } void Lift() { while (1) { L = analog(4); R = analog(2); if (L > 630 && R > 640) { fd(80); } else if (L < 630 && R > 640) { sl(80); } else if (L > 630 && R < 640) { sr(80); } else if (L < 630 && R < 640) { bk(100); delay(100); ao(); // เบรค Keep(); // เก็บกระป๋อง while (analog(2) < 640) { // ถอยหลังจนเจอพื้นสีขาว bk(30); } while (analog(2) > 640) { // ถอยหลังจนเจอเส้นสีดำ bk(30); } delay(100); ao(); break; } } } void RRR() { // เคลื่อนที่ตามเส้น เลี้ยวขวาเมื่อเจอทางแยก while (1) { L = analog(4); R = analog(2); if (L > 630 && R > 640) { fd(80); } else if (L < 630 && R > 640) { sl(80); } else if (L > 630 && R < 640) { sr(80); } else if (L < 630 && R < 640) { fd(80); sound(2000, 100); while (analog(2) > 640) { sr(80); } while (analog(2) < 640) { sr(80); } break; } } } void LLL() { // เคลื่อนที่ตามเส้น เลี้ยวซ้ายเมื่อเจอทางแยก while (1) { L = analog(4); R = analog(2); if (L > 630 && R > 640) { fd(80); } else if (L < 630 && R > 640) { sl(80); } else if (L > 630 && R < 640) { sr(80); } else if (L < 630 && R < 640) { fd(80); sound(2000, 100); while (analog(4) > 630) { sl(80); } while (analog(4) < 630) { sl(80); } break; } } } void servo2(int pos1, int pos2, int speeds) { // สั่งงานให้เซอร์โวเคลื่อนที่ช้าลง int i; if (pos1 > pos2) { for (i = pos1; i > pos2; i--) { servo(2, i); delay(speeds); } } else { for (i = pos1; i < pos2; i++) { servo(2, i); delay(speeds); } } }
การทำงานของโปรแกรม
จากโค้ด เมื่อนำชุดคำสั่งต่างๆ ยุบรวมเป็นฟังก์ชั่นแล้ว การเรียกใช้งาน สามารถทำความเข้าใจเป็นส่วนๆ ได้ง่ายขึ้น ดังนั้นใน void setup() หลังจากสั่งให้เซอร์โวยกตัวอยู่ในตำแหน่งพร้อมแล้วก็รอกดปุ่ม OK เพื่อเริ่มต้นเคลื่อนที่ โดยวางเส้นทางการเคลื่อนที่ไว้ตามรูป เมื่อจบภารกิจ จะใช้คำสั่ง ao() เพื่อหยุดการเคลื่อนที่
โจทย์เพิ่มเติม
ถ้าย้ายตำแหน่งเริ่มต้นไปยังตำแหน่งดังรูปจะต้องปรับแก้โค้ดอย่างไรบ้าง ลองเขียนโค้ดเพื่อทดสอบดูนะครับ