RQ-BOTการใช้งานหุ่นยนต์

รูจ้กั ATX2

เขียนโปรแกรม Arduino

หุ่นยนตเ์คล่ือนท่ีอย่างง่าย

หุ่นยนตเ์ดินตามเส้น

Gripper

โมดลูอลัตรา้โซนิก

Encoder การเขียนเมนู

บอกโจทย์

ซ้อม

แข่งขนั

หลกัการของระบบควบคมุ

อินพตุ เอาตพ์ตุประมวลผล

บอรด์ ATX2

แบตเตอร่ี อแดปเตอร์

เปิด/ปิด

เซอรโ์ว

USB (Download)ANALOG

I2C

DIGITALANALOG

มอเตอร์

จอสี128x160 จดุ

ส่ือสารอนุกรม 1

OK

RESET

knob

ไฟเตือนแบต

ไฟเลี้ยง CPUไฟเลี้ยง SERVO

ล าโพงเปียโซ

IPST-SE

ATX2

Motor x 6Servo x 8I/O x 18

Motor x 2Servo x 4I/O x 20

เปรียบเทียบ IPST-SE vs ATX2

คอมไพเลอร ์Open Source

ATX2

Software

Edit+Compile+Download

Arduino

ติดตัง้โปรแกรม Arduino 1.07

1. เปิดโปรแกรมในแผน่ CDROM 2. ดบัเบ้ิลคลิกไฟล ์Arduino 1.07_TPA1503133. ท าตามขัน้ตอนติดตัง้โปรแกรม

ขัน้ตอนติดตัง้โปรแกรม

10

เม่ือจบขัน้ตอนน้ีจะมีหน้าต่าง ติดตัง้ไดรเวอร์ห้ามกด Cancel

เพ่ือให้คอมพิวเตอรร์ู้จกักบับอรด์ ATX2

ขัน้ตอนติดตัง้ไดรเวอร์

ขัน้ตอนติดตัง้ไดรเวอร์

ROBOT MINDMAP

void setup() {

}void loop() {

}

ส าหรบัก าหนดค่า เกิดขึน้ครัง้เดียว

โปรแกรมหลกัท างานต่อเน่ือง

รปูแบบของโปรแกรม Arduino

START

ท างานใน Setup

ท างานใน Loop

รปูแบบของโปรแกรม Arduino

ตรวจสอบว่าเลือกบอรด์ ATX2 แล้ว

สายเช่ือมต่อแบตเตอร่ี

แบตเตอร่ี Li-Po

แดง บวกด า ลบ

2 เซล 7.4Vกระแส 1100mA

จ่ายกระแส 30 เท่าชารจ์ 5 เท่า

แบตเตอร่ีและขัว้ต่อ

2 เซล 7.4Vกระแส 1100mAhจ่ายกระแส 30 เท่า

ชารจ์ 5 เท่า

รายละเอียดแบตเตอร่ี ลิเธียม-โพลิเมอร์

1 เซล 3.7V อนุกรมกนั 2 เซล = 7.4V

จ่ายกระแสชัว่ขณะได้ 1100 x 30 = 33000mA O_o!ชารจ์ได้ 5 เท่า 1100x5 = 5500mA ใช้เวลาประมาณ 20 นาที

จ่ายไฟ 1100 mA ต่อเน่ืองได้ประมาณ 1 ชัว่โมง

ติดตัง้แบตเตอร่ีกบัหุ่นยนต์

สถานะแบตเตอร่ี

ถ้า LED ติด ต้องเปล่ียนแบตทนัที

1. ต่อแบตเตอร่ี

2.ต่อสาย USB กบัคอมพิวเตอร์

3.ไฟ USB ติดสว่าง

ขัน้ตอนการเช่ือมต่อ

เลือกพอรต์อนุกรม

#include

void setup(){

glcd(0,0,"Hello World");

}void loop(){

}

ทดสอบโปรแกรมแรก

ตรวจสอบไวยกรณ์/อพัโหลด

1. คอมไพล์

2. แจ้งว่าคอมไพลผ์า่น

3. อพัโหลดไปยงั ATX2

ผลลพัธท่ี์บอรด์ ATX2

คณุสมบติัของขอ GLCD

glcd แสดงข้อความท่ีจอ GLCD ได้ 21 ตวั 16 บรรทดั (size 1) รปูแบบ

void glcd(x,y,*p,...)

พารามิเตอร์x คือต าแหน่งบรรทดัมีค่าตัง้แต่ 0-15y คือต าแหน่งตวัอกัษรมีค่าตัง้แต่ 0-24*p คือข้อความท่ีต้องการน ามาแสดง

ค่าพิเศษ%d แสดงตวัเลขจ านวนเตม็ในช่วง -32,768 ถึง 32,767%h แสดงตวัเลขฐานสิบหก%b แสดงตวัเลขฐานสอง%l แสดงตวัเลขจ านวนเตม็ในช่วง -2,147,483,648 ถึง 2,147,483,647%f แสดงผลตวัเลขจ านวนจริง (แสดงทศนิยม 3 หลกั)

ค าสัง่ของ GLCD

ปกติเป็น Mode 0

ตวัอย่าง : glcdMode(1);

ค าสัง่ของ glcdMode (หมนุหน้าจอ)

setTextSize()

ตวัอย่าง : setTextSize(4);

ขนาดตวัอกัษรเป็น 4 เท่าขนาดปกติ

ปรบัขนาดตวัอกัษร

setTextColor(COLOR)

GLCD_RED,

GLCD_GREEN,

GLCD_BLUE,

GLCD_YELLOW,

GLCD_BLACK,

GLCD_WHITE,

GLCD_CYAN,

GLCD_MAGENTA

GLCD_ORANGE

GLCD_LIME

GLCD_VIOLET

GLCD_PINK

GLCD_DOLLAR

GLCD_SKY

GLCD_BROWN

GLCD_DARKGREEN

GLCD_NAVY

GLCD_GRAY

GLCD_DARKGRAY

ตวัอย่าง#include

void setup(){

setTextColor(GLCD_WHITE);

glcd(0,0,"Hello");

setTextColor(GLCD_GREEN);

glcd(1,0,"World");

}void loop(){}

ค่าสีตวัอกัษร

ตวัอย่าง#include

void setup(){

setTextBackgroundColor(GLCD_RED);

setTextColor(GLCD_YELLOW);

glcd(0,0,"Hello World");

}void loop(){}

setTextBackgroundColor(COLOR)

ค่าสีพืน้หลงัตวัอกัษร

glcdRect(x,y,width,height,color)

glcdFillRect(x,y,width,height,color)

glcdCircle(x,y,radius,color)

glcdFillCircle(x,y,radius,color)

glcdLine(x1,y1,x2,y2,color)

x ต าแหน่งแนวนอนy ต าแหน่งแนวตัง้

width ความกว้างheight ความสูงredius รัศมีcolor สี

แสดงรปูทรงเรขาคณิต

สร้างวงกลมสีแดงอยู่กึ่งกลางจอภาพ รัศมีเตม็จอพอดี

บททดสอบ 1

ลากเส้น 4 เส้นโดยมีจุดตัดอยู่กลางจอภาพพอดี

บททดสอบ 2

สร้างส่ีเหล่ียมซ้อนกันดังรูป

บททดสอบ 3

glcd แสดงข้อความท่ีจอ GLCD ได้ 21 ตวั 16 บรรทดั (size 1) รปูแบบ

void glcd(x,y,*p,...)

พารามิเตอร์x คือต าแหน่งบรรทดัมีค่าตัง้แต่ 0-15y คือต าแหน่งตวัอกัษรมีค่าตัง้แต่ 0-24*p คือข้อความท่ีต้องการน ามาแสดง

ค่าพิเศษ%d แสดงตวัเลขจ านวนเตม็ในช่วง -32,768 ถึง 32,767%h แสดงตวัเลขฐานสิบหก%b แสดงตวัเลขฐานสอง%l แสดงตวัเลขจ านวนเตม็ในช่วง -2,147,483,648 ถึง 2,147,483,647%f แสดงผลตวัเลขจ านวนจริง (แสดงทศนิยม 3 หลกั)

ค าสัง่ของ GLCD

glcd(0,0,"%d",100);

การแสดงผลค่าตวัเลข

byte

0-65535 (unsigned int)word

0-255 (unsigned char)

0-1boolean True False

-32768 ถึง 32767int-128 ถงึ 127char-3.4 x 1038 ถงึ 3.4 x 1038 float

หาข้อมลูเพ่ิมเติมจาก reference

ค่าตวัแปรท่ีใช้งานบอ่ยๆ

ATX2 กบั ZX-Switch01

ต่อกบัขา 26 ต่อกบัขา 27

out(ch,state);

สง่คา่สถานะ(state) 0 หรือ 1

ออกไปยงัต าแหนง่ขา (ch)ท่ีระบุ

เช่น out(17,1);

ค าสัง่ส่งค่าออกเอาตพ์ตุดิจิตอล

• ใช้ล าโพงเปียโซ มีอิมพีแดนซ์ 32W• มีค่าความถ่ีย่าน 300Hz ถึง 3000 Hz

ATX2 กบั ล าโพงเปียโซ

beep();

ก าเนิดเสียงความถ่ี 500 Hz นาน 0.1 วินาที

sound(freq,time);

freq ความถีเ่สียง

time เวลาทีเ่สียงดงั

#include

void setup(){

}

void loop(){

beep();

delay(1000);

}

ตวัอย่าง : สร้างเสียงต๊ิดทกุๆ 1 วินาที

#include

void setup(){}

void loop(){

sound(1200,500);

sleep(1000);

}

ตวัอย่าง : สรา้งเสียงความถ่ี 1200 Hz นาน 0.5 วินาที

หมนุเพ่ือปรบัค่า 0-1000

knob() : ตวัต้านทานปรบัค่าได้

knob() ใช้อ่านค่าตวัต้านทานปรบัค่าได้บน ATX2 ค่าอยู่ในช่วง 0-1000รปูแบบ

knob();

การคืนค่า

ค่าท่ีอ่านได้จาก knob มีค่าระหว่าง 0-1000ตวัอย่าง

glcd(1,1,"%d",knob());

การใช้งานค าสัง่ knob()

รปูแบบknob(x);

x คือค่า Scale หรือช่วงท่ีต้องการตวัอย่าง

glcd(1,1,"%d",knob(180));

หน้าจอจะแสดงค่า 0-180 เพราะตัง้ค่าสเกลเป็น 180

การใช้งานค าสัง่ knob(x)

รปูแบบknob(x,y);

x คือค่า Scale ช่วงเร่ิมต้นy คือค่า Scale ช่วงท้าย

ตวัอย่างglcd(1,1,"%d",knob(10,90));

หน้าจอจะแสดงค่า 10-90 ตามการหมุน knob()

การใช้งานค าสัง่ knob(x,y)

glcd(0,0,"%d",knob());

การแสดงผลค่า knob() ท่ีจอ GLCD

สวิตช ์OK

กดเป็นจริงไม่กดเป็นเทจ็

ค าสัง่ sw_OK()

#include

void setup(){}

void loop(){

if(sw_OK()){

sound(1200,100);

}

}

ตวัอย่าง

สวิตช ์OK

รอจนกระทัง่กดสวิตช ์OKแล้วปล่อยสวิตช ์จึงจะท างาน

ค าสัง่ sw_OK_press()

#include

void setup(){}

void loop(){

sw_OK_press();

beep();

}

ตวัอย่าง

1. แสดงข้อความท่ีหน้าจอ2. รอจนกระทัง่กดสวิตช ์OK3. ท างานค าสัง่ถดัไป

ค าสัง่ OK ()

#include

void setup(){

OK();

}

void loop(){

}

ตวัอย่าง

motor (CH,POW)

CH 1-6 คือมอเตอร์ 1 ถงึ 612 ส าหรับมอเตอร์ 1 และ 234 ส าหรับมอเตอร์ 3 และ 456 ส าหรับมอเตอร์ 5 และ 6100,ALL,ALL4 ส าหรับมอเตอร์ 1 ถงึ 4106 หรือ ALL6 ส าหรับมอเตอร์ทัง้ 6 ตัว

POW ความเร็ว -100 ถงึ 100ค่าบวก เดนิหน้าค่าลบ ถอยหลัง

ค าสัง่ขบัมอเตอร์

motor (CH,POW)

motor(1,60); ให้มอเตอร์ 1 ไปหน้าความเร็ว 60%motor(12,-80); ให้มอเตอร์ 1 และ 2 ถอยหลังความเร็ว 80%motor(ALL,100); ให้มอเตอร์ 1-4 ไปด้านหน้าด้วยความเร็ว 100%

ตวัอย่าง : motor()

motor_stop(CH)ค าสัง่หยดุมอเตอร์

CH 1-6 คือมอเตอร์ 1 ถงึ 612 ส าหรับมอเตอร์ 1 และ 234 ส าหรับมอเตอร์ 3 และ 456 ส าหรับมอเตอร์ 5 และ 6100,ALL,ALL4 ส าหรับมอเตอร์ 1 ถงึ 4106 หรือ ALL6 ส าหรับมอเตอร์ทัง้ 6 ตัว

ค าสัง่ ao() เท่ากบั motor_stop(12); มอเตอร ์1 และ 2 หยดุค าสัง่ AO() เท่ากบั motor_stop(ALL); มอเตอร ์1-4 หยดุ

เดินหน้า

fd(speed);

ถอยหลงั

bk(speed);

เลีย้วซ้าย

sl(speed);

เลีย้วขวา

sr(speed);

tl(speed);

เลีย้วด้านเดียว

tr(speed);

หยดุหุ่นยนต์ao();

speed = 0-100

ฟังกช์ัน่ขบัเคล่ือนหุ่นยนต ์กรณีขบัเคล่ือน 2 ล้อ

motor1 motor2

motor1 motor2

motor1 motor2 motor1 motor2

เดินหน้า

FD(speed);

ถอยหลงั

เลีย้วซ้าย เลีย้วขวา

หยดุหุ่นยนต์AO();

speed = 0-100

ฟังกช์ัน่ขบัเคล่ือนหุ่นยนต ์กรณีขบัเคล่ือน 4 ล้อ

motor2

motor1 motor3

motor4

BK(speed);

motor2

motor1 motor3

motor4

SL(speed);

motor2

motor1 motor3

motor4

SR(speed);

motor2

motor1 motor3

motor4

ทดสอบขบัเคล่ือน เดินหน้า 1 วินาที ถอยหลงั 1 วินาที#include

void setup(){OK();}

void loop(){

FD(50); delay(1000);

BK(50); delay(1000);

}

ทดสอบขบัเคล่ือน เดินวนเป็นรปูส่ีเหล่ียม#include

void setup(){

OK();}

void loop(){

FD(50);

delay(1000);SR(50);

delay(700);}

ปรบัหุ่นยนตใ์ห้เคล่ือนท่ีตรง

FD2(Speed1,speed2); เดนิหน้า

BK2(Speed1,speed2); ถอยหลงั

Speed1 คือมอเตอร ์1 และ 2Speed2 คือมอเตอร ์3 และ 4

motor2motor1

motor3motor4

แผงวงจรตรวจจบัการสะท้อน ZX-03

ต าแหน่งติดตัง้แผงวงจร ZX-03

A4 A5

ไม่เกิน 1 CM

ต าแหน่งติดตัง้แผงวงจร ZX-03

A4A5

อ่านค่าจาก ZX-03 แสดงท่ี GLCD

#include

void setup(){

setTextSize(3);

glcdMode(3);

}void loop(){

glcd(0,0,"L=%d ",analog(5));

glcd(1,0,"R=%d ",analog(4));

}

อ่านสีขาว

อ่านสีด า

หาค่ากลางRefL =(900+120)/2 = 510RefR =(880+130)/2 = 505

L R L R

ตรวจสอบผลการสะท้อนแสง ZX-03

อ่านค่าสีด า อ่านค่าสีขาว

หุ่นยนตเ์คล่ือนท่ีหยดุท่ีเส้นด า#include

void setup(){

OK();

glcdClear();

FD(50);

while(analog(5)>510);

AO();

glcd(1,1,"Stop... ");

} void loop(){}

รปูแบบการเคล่ือนท่ีตามเส้นอย่างง่าย

ด้านซ้ายและขวาเจอสีขาว

if(L>250&&R>250){

FD(60);}

ด้านซ้ายเจอสีด า

if(L250){

SL(60);}

ด้านขวาเจอสีด า

if(L>250&&R

เจอสีด า สองด้าน

if(L>250&&R

รปูแบบเคล่ือนท่ีตามเส้นอย่างง่าย

โปรแกรมทดสอบ

#include

int L,R,RefL=510,RefR=505;

void setup(){OK();}

void loop(){

L=analog(5);

R=analog(4);

if(L>RefL&&R>RefR){ FD(40); }

else if(LRefR){SL(40);}

else if(L>RefL&&R

เจอเส้นตดัส่งเสียงออกล าโพง และเดินข้ามไป

โปรแกรมทดสอบ

#include

int L,R,RefL=510,RefR=505;

void setup(){OK();}

void loop(){

L=analog(5);

R=analog(4);

if(L>RefL&&R>RefR){ FD(40); }

else if(LRefR){SL(40);}

else if(L>RefL&&R

เจอเส้นตดัเล้ียวขวา

โปรแกรมทดสอบ

#include

int L,R,RefL=510,RefR=505;

void setup(){OK();}

void loop(){

L=analog(5);

R=analog(4);

if(LRefR){FD(40);}

else if(LRefR){SL(40);}

else if(L>RefL&&R

เทคนิคการเลี้ยวให้ได้ 90 องศาพอดี เมื่อเจอเส้นตดั

void R90(int x){

ชุดค าสัง่y=x+2;

.}

ช่ือฟังกช์ัน่

ชดุค าสัง่ในฟังกช์ัน่

การใช้งานฟังกช์ัน่ R90(200);

ตวัแปรท่ีส่งไปยงัฟังกช์ัน่

รปูแบบการสร้างฟังกช์ัน่

void R90(){

FD(40);sound(500,100);

while(analog(5)>RefR){SR(60);}

while(analog(5)RefL){SL(60);}

while(analog(4)

เซอรโ์วมอเตอรม์าตรฐาน แกนหมุน 180 องศา

ไฟเลีย้ง (+) GND (-)

สัญญาณ (S)

GND (-)

สัญญาณ (S)

SM Gripper 2015

SERVO1 : SV1

SERVO2: SV2เฟืองโลหะ

เฟืองพลาสตกิ

ฟังกช์ัน่ขบัเซอรโ์วมอเตอร์

servo(CH,POS);

CH ช่องท่ีใช้ขบั = 1 ถึง 8

POS ต าแหน่งองศาเซอรโ์ว = 0-180 ,-1ค่า -1 หมายถึงหยดุจ่ายสญัญาณให้เซอรโ์วเซอรโ์วจะไม่ลอ็กแกน

โปรแกรมทดสอบ SERVO1

#include

int x;

void setup(){

OK();glcdClear();

}void loop(){

x=knob(180);

servo(1,x);

glcd(1,1,"%d ",x);

}

หมุน knob ทดสอบ

#include

int x;

void setup(){

OK();glcdClear();

}void loop(){

x=knob(180);

servo(2,x);

glcd(1,1,"%d ",x);

}

โปรแกรมทดสอบ SERVO1

หมุน knob ทดสอบ

สร้างฟังกช์ัน่ sHome() เพ่ือให้เซอรโ์วอยู่ในต าแหน่ง home

อลัตร้าโซนิค HC-SR04

GND (-)

ไฟเลีย้ง (+)

Echo(25)Trigger(24)

ฟังกช์ัน่อ่านค่าจาก HC-SR04

sonar();

คืนค่าระยะทางเป็นเซนตเิมตร__sonarTime();

คืนค่าระยะทางเป็นค่าดบิ

ตวัอย่างการใช้งานฟังกช์ัน่ Sonar#include

void setup(){

OK();glcdClear();

}void loop(){

glcd(0,0,"%d ",sonar());

}

แสดงค่าเป็นเซนตเิมตร

ตวัอย่างการใช้งานฟังกช์ัน่ Sonar

#include

int x;

void setup(){

OK();glcdClear();

FD(100);

while(sonar()>8);

BK(100);delay(50);AO();

}void loop(){

sw_OK_press();

FD(100);

while(sonar()>8);

BK(100);delay(50);AO();

}

Encoder เคร่ืองพมิพ์ Inkjet

Encoder เคร่ืองพมิพ์ Inkjet

Encoder อย่างง่ายจาก ZX-03Q

Encoder แบบก้ามปสู าหรบั RQ-Bot

Encoder ก้ามปู

จาน Encoder 20 ร่อง

ตดิตัง้ที่ล้อหลังขวาต่อสายเข้าที่ขา 18/INT2

ฟังกช์ัน่เจอเส้นตดัเลี้ยวขวา/ซ้าย

5.7 CM จ านวนช่องเท่ากบั 20 ช่องเส้นผา่นศนูยก์ลาง 5.7 CM

เส้นรอบวง = pD

เส้นรอบวง = 17.75 CM

3.1415x5.7 = 17.75

แนวคิดการเขียนโปรแกรมอย่างง่าย

อ่านเป็นขาว 1 ครัง้ ด า 1 ครัง้

นับเป็น 1 ค่า

while(x

เรียกใช้งานผา่นไลบราร่ี ATX2_enc.h

encCnt 1. ตวัแปรencCntน้ีจะเพ่ิมค่าอตัโนมติั (เม่ือเกิดการเปล่ียนแปลงท่ี Encoder)

2.เพ่ิมทัง้กรณีเจอช่องว่างและไม่เจอช่องว่าง

3. ถ้าต้องการนับค่าใหม่ ให้ใช้

encCnt =0;4. ตรวจสอบค่าได้ง่ายๆ เช่น

while(encCnt

ตวัอย่างการใช้งาน

#include

void setup(){

OK();glcdClear();

}void loop(){

glcd(0,0,"%d

",encCnt);

}

ทดลองหมนุล้อดคู่าท่ีเปล่ียนแปลง

ตวัอย่างการใช้งาน

#include

void setup(){

OK();glcdClear();

FD(100);

while(encCnt

เขียนเมนูอย่างง่าย

#include

int k;

void setup(){

OK();glcdClear();}

void loop(){

k=knob(3);

glcd(0,0,"Prog %d ",k+1);

if(sw_OK()){

if(k==0){P1();}

else if(k==1){P2();}

else if(k==2){P3();}

else if(k==3){P4();}

}}

void P1(){

while(1){

glcd(1,1,"L=%d

",analog(0));

glcd(3,1,"R=%d

",analog(1));

}}void P2(){

while(1){ beep(16); }}

void P3(){

while(1){ fd(100); }}

void P4(){

while(1){bk(100); }}

กฤษดา ใจเยน็ KRITSADA JAIYENบริษทั อินโนเวตีฟ เอก็เพอริเมนต ์จ ากดั108 ซอยสขุมุวิท 101/2 ถ.สขุมุวิทแขวงบางนา เขตบางนา กรงุเทพฯ 10260โทรศพัท ์02-7477001-4 โทรสาร 02-7477005

Email : kritsada@inex.co.th

www.inex.co.th

mailto:kritsada@inex.co.th