แขนจับ 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() เพื่อหยุดการเคลื่อนที่
โจทย์เพิ่มเติม
ถ้าย้ายตำแหน่งเริ่มต้นไปยังตำแหน่งดังรูปจะต้องปรับแก้โค้ดอย่างไรบ้าง ลองเขียนโค้ดเพื่อทดสอบดูนะครับ