LAB 11 หุ่นยนต์ทำภารกิจ
LAB 11 หุ่นยนต์ทำภารกิจ
Embedded Files
คำอธิบาย
คำอธิบาย
คำสั่งต่างๆที่จำเป็นได้รวมอยู่ใน code นี้แล้ว
โดยได้สร้าง เมนู ให้จำนวน 12 เมนู
เมนู ที่ 0 ฟังก์ชั่นอ่านค่าแสงเซนเซอร์ sensor();
เมนูที่ 1 ฟังก์ชั่นอ่านค่าตำแหน่งเซอโว servotest();
เมนูที่ 2 ฟังก์ชั่นเดินตามเส้น เจอเส้นตัดแล้วหยุด P();
เมนูที่ 3 ฟังก์ชั่นเลี้ยวซ้ายเจอเส้นแล้วหยุด L();
เมนูที่ 4 ฟังก์ชั่นเลี้ยวขวาเจอเส้นแล้วหยุด R();
เมนูที่ 5 ฟังก์ชั่นยกกล่อง up();
เมนูที่ 6 ฟังก์ชั่นวางกล่อง down();
code ตัวอย่าง
code ตัวอย่าง
///////// ผนวกไลบรารี่ ////////////
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display(-1);
#include <Servo.h>
Servo sv1; // กำหนดชื่อเซอโวตัวที่ 1 ว่า sv1
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
//////////////////////////////////////////////
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////#define PWMR 6#define DR1 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7#define DR2 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8/////////////////////////////////#define PWML 5#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9#define DL2 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4/////////////////กำหนดค่ากลางเซนเซอร์//////////////
#define A0 252 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)
#define A1 356 // ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)
#define A2 379 // ค่ากลางเซนเซอร์ A2 (ขวากลาง)
#define A3 223 // ค่ากลางเซนเซอร์ A3 (ขวาสุด)
//////////กำหนดตำแหน่งเซอโว/////////////
int servoup = 100;
int servodown = 180;int servokeep = 0;int servopaste = 120;
////สร้างตัวแปรเก็บค่าเซนเซอร์
int s0 ;
int s1 ;
int s2 ;
int s3 ;//////////////////////////////////////////////void setup() { display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64) pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT pinMode(PWML, OUTPUT); pinMode(DL1, OUTPUT); pinMode(DL2, OUTPUT); pinMode(PWMR, OUTPUT); pinMode(DR1, OUTPUT); pinMode(DR2, OUTPUT);sv1.attach(10);sv1.write(servodown);}
void loop() {
int sw = digitalRead(button); // ให้ sw อ่านค่า digital จากพอร์ต 2(button)
int nob = analogRead(7); // ให้ nob เทียบเท่าค่า A7
int menu = map(nob, 0, 1023, 0, 12); // เทียบอัตราส่วนของพอร์ต A7 จาก 0-1023 เพื่อทำเป็นเมนู 0-12
display.clearDisplay(); // ล้างค่าหน้าจอ
display.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
display.setCursor(0, 0); // เซตตำแหน่ง 0,0
display.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
display.print(" "); // วรรค
display.println(menu); // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-12
display.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
display.println(" Tkl RoBoT Team"); // พิมพ์คำว่า Tkl RoBoT Team
display.print(" "); // วรรค
display.print(nob); // แสดงค่าที่อ่านได้จาก nob หรือ Analog7
display.println(" KruRO Robot"); // พิมพ์คำว่า KruRO Robot
display.display();
if ((sw == 1) and (menu == 0))
{ menu0();
}
if ((sw == 1) and (menu == 1))
{
menu1();
}
if ((sw == 1) and (menu == 2))
{
menu2();
}
if ((sw == 1) and (menu == 3))
{
menu3();
}
if ((sw == 1) and (menu == 4))
{
menu4();
}
if ((sw == 1) and (menu == 5))
{
menu5();
}
if ((sw == 1) and (menu == 6))
{
menu6();
}
if ((sw == 1) and (menu == 7))
{
menu7();
}
if ((sw == 1) and (menu == 8))
{
menu8();
}
if ((sw == 1) and (menu == 9))
{
menu9();
}
if ((sw == 1) and (menu == 10))
{
menu10();
}
if ((sw == 1) and (menu == 11))
{
menu11();
}
if ((sw == 1) and (menu == 12))
{
menu12();
}
delay(100);
}
////////////////////////////ฟังกชั้น ทดสอบเซอโว//////////////////////void servotest(){
while(1) {
int vr = analogRead(A7); // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 7
int nob = map(vr, 0, 1023, 0, 180); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180
display.clearDisplay(); // เคลียร์หน้าจอ display
display.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
display.setCursor(0, 0); // เซตตำแหน่ง 0,0
display.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
display.println(nob); // นำค่า nob มาแสดงใน display
display.display(); // เปิดฟังก์ชันแสดงผล
sv1.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(50); // หน่วงเวลา 0.05 วินาที
}
}void up(){
sv1.write(servoup); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(100); // หน่วงเวลา 0.1 วินาที
}void down(){
sv1.write(servodown); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(100); // หน่วงเวลา 0.1 วินาที
}void analogs()
{ s0 = analogRead(0); s1 = analogRead(1); s2 = analogRead(2); s3 = analogRead(3);}
void sensor(){
while (true) {
analogs(); display.clearDisplay(); display.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ display.setCursor(0, 0); // เซตตำแหน่ง 0,0 display.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 1 display.print(" S0 = "); display.println(s0); // แสดงค่าเซนเซอร์ S0 display.print(" S1 = "); display.println(s1); // แสดงค่าเซนเซอร์ S1 display.print(" S2 = "); display.println(s2); // แสดงค่าเซนเซอร์ S2 display.print(" S3 = "); display.println(s3); // แสดงค่าเซนเซอร์ S3 display.display(); delay(50);
}
}/////////////////////////เลี้ยวซ้าย///////////////////////////////////////////
void L0(){while (1){analogs();Motor(-75,75);if(s0<A0){Motor(0,0);break;}}} //S0void L1(){while (1){analogs();Motor(-75,75);if(s1<A1){Motor(0,0);break;}}} //S1void L2(){while (1){analogs();Motor(-75,75);if(s2<A2){Motor(0,0);break;}}} //S2void L3(){while (1){analogs();Motor(-75,75);if(s3<A3){Motor(0,0);break;}}} //S3//////////////////////เลี้ยวขวา//////////////////////////////////////////void R0(){while (1){analogs();Motor(75,-75);if(s0<A0){Motor(0,0);break;}}} //S0void R1(){while (1){analogs();Motor(75,-75);if(s1<A1){Motor(0,0);break;}}} //S1void R2(){while (1){analogs();Motor(75,-75);if(s2<A2){Motor(0,0);break;}}} //S2void R3(){while (1){analogs();Motor(75,-75);if(s3<A3){Motor(0,0);break;}}} //S3/////////////////////////////////////////////////////////////////////
/////////////////////////ฟังก์ชันเส้นทาง////////////////////////////////////////void P() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุด/////////////////////////////{while (1){ s0 = analogRead(0); s1 = analogRead(1); s2 = analogRead(2); s3 = analogRead(3);if ((s1 > A1) and (s2 > A2)){Motor(75, 75);}if (s1 < A1){Motor(30, 75);}if (s2 < A2){Motor(75, 30);}if (s0 < A0){Motor(-40, 75);}if (s3 < A3){Motor(75, -40);}if (((s1 < A1) and (s2 < A2)) or ((s1 < A1) and (s0 < A0)) or ((s2 < A2) and (s3 < A3))) {Motor(75, 75);delay(180);Motor(0, 0);delay(100);break;}}}
void L(){L3();L1();delay(100);} /////// เลี้ยวซ้ายvoid R(){R0;R2();delay(100);} ///////// เลี้ยวขวา/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////
/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////
void Motor(int spl, int spr) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);{ if (spl > 0) { digitalWrite(DL1, LOW); digitalWrite(DL2, HIGH); analogWrite(PWML, spl); } else if (spl < 0) { digitalWrite(DL1, HIGH); digitalWrite(DL2, LOW); analogWrite(PWML, -spl); } else { digitalWrite(DL1, LOW); digitalWrite(DL2, LOW); } ////////////////////////////////////// if (spr > 0) { digitalWrite(DR1, HIGH); digitalWrite(DR2, LOW); analogWrite(PWMR, spr); } else if (spr < 0) { digitalWrite(DR1, LOW); digitalWrite(DR2, HIGH); analogWrite(PWMR, -spr); } else { digitalWrite(DR1, LOW); digitalWrite(DR2, LOW); }}
void menu0() /// code 1 ที่นี่{
sensor();
}
void menu1() /// code 1 ที่นี่{
servotest();
}
//////////////////////////////////เริ่มเขียนโค้ดในเมนูที่นี้///////////////////////void menu2() /// code 2 ที่นี่{
P();
}
void menu3() /// code 3 ที่นี่{
L();
}
void menu4() /// code 4 ที่นี่{ R();
}
void menu5() /// code 5 ที่นี่{ up();
}
void menu6() /// code 6 ที่นี่{ down();
}
void menu7() /// code 7 ที่นี่{Motor(-150,150);delay(1000);Motor(0,0);}
void menu8() /// code 8 ที่นี่{Motor(150,-150);delay(1000);Motor(0,0);}
void menu9() /// code 9 ที่นี่{ Motor(150,150);delay(100);Motor(0,0);}
void menu10() /// code 10 ที่นี่{
}
void menu11() /// code 11 ที่นี่{
}
void menu12() /// code 12 ที่นี่{
}
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display(-1);
#include <Servo.h>
Servo sv1; // กำหนดชื่อเซอโวตัวที่ 1 ว่า sv1
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
//////////////////////////////////////////////
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////#define PWMR 6#define DR1 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7#define DR2 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8/////////////////////////////////#define PWML 5#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9#define DL2 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4/////////////////กำหนดค่ากลางเซนเซอร์//////////////
#define A0 252 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)
#define A1 356 // ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)
#define A2 379 // ค่ากลางเซนเซอร์ A2 (ขวากลาง)
#define A3 223 // ค่ากลางเซนเซอร์ A3 (ขวาสุด)
//////////กำหนดตำแหน่งเซอโว/////////////
int servoup = 100;
int servodown = 180;int servokeep = 0;int servopaste = 120;
////สร้างตัวแปรเก็บค่าเซนเซอร์
int s0 ;
int s1 ;
int s2 ;
int s3 ;//////////////////////////////////////////////void setup() { display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64) pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT pinMode(PWML, OUTPUT); pinMode(DL1, OUTPUT); pinMode(DL2, OUTPUT); pinMode(PWMR, OUTPUT); pinMode(DR1, OUTPUT); pinMode(DR2, OUTPUT);sv1.attach(10);sv1.write(servodown);}
void loop() {
int sw = digitalRead(button); // ให้ sw อ่านค่า digital จากพอร์ต 2(button)
int nob = analogRead(7); // ให้ nob เทียบเท่าค่า A7
int menu = map(nob, 0, 1023, 0, 12); // เทียบอัตราส่วนของพอร์ต A7 จาก 0-1023 เพื่อทำเป็นเมนู 0-12
display.clearDisplay(); // ล้างค่าหน้าจอ
display.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
display.setCursor(0, 0); // เซตตำแหน่ง 0,0
display.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
display.print(" "); // วรรค
display.println(menu); // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-12
display.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
display.println(" Tkl RoBoT Team"); // พิมพ์คำว่า Tkl RoBoT Team
display.print(" "); // วรรค
display.print(nob); // แสดงค่าที่อ่านได้จาก nob หรือ Analog7
display.println(" KruRO Robot"); // พิมพ์คำว่า KruRO Robot
display.display();
if ((sw == 1) and (menu == 0))
{ menu0();
}
if ((sw == 1) and (menu == 1))
{
menu1();
}
if ((sw == 1) and (menu == 2))
{
menu2();
}
if ((sw == 1) and (menu == 3))
{
menu3();
}
if ((sw == 1) and (menu == 4))
{
menu4();
}
if ((sw == 1) and (menu == 5))
{
menu5();
}
if ((sw == 1) and (menu == 6))
{
menu6();
}
if ((sw == 1) and (menu == 7))
{
menu7();
}
if ((sw == 1) and (menu == 8))
{
menu8();
}
if ((sw == 1) and (menu == 9))
{
menu9();
}
if ((sw == 1) and (menu == 10))
{
menu10();
}
if ((sw == 1) and (menu == 11))
{
menu11();
}
if ((sw == 1) and (menu == 12))
{
menu12();
}
delay(100);
}
////////////////////////////ฟังกชั้น ทดสอบเซอโว//////////////////////void servotest(){
while(1) {
int vr = analogRead(A7); // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 7
int nob = map(vr, 0, 1023, 0, 180); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180
display.clearDisplay(); // เคลียร์หน้าจอ display
display.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
display.setCursor(0, 0); // เซตตำแหน่ง 0,0
display.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
display.println(nob); // นำค่า nob มาแสดงใน display
display.display(); // เปิดฟังก์ชันแสดงผล
sv1.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(50); // หน่วงเวลา 0.05 วินาที
}
}void up(){
sv1.write(servoup); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(100); // หน่วงเวลา 0.1 วินาที
}void down(){
sv1.write(servodown); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(100); // หน่วงเวลา 0.1 วินาที
}void analogs()
{ s0 = analogRead(0); s1 = analogRead(1); s2 = analogRead(2); s3 = analogRead(3);}
void sensor(){
while (true) {
analogs(); display.clearDisplay(); display.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ display.setCursor(0, 0); // เซตตำแหน่ง 0,0 display.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 1 display.print(" S0 = "); display.println(s0); // แสดงค่าเซนเซอร์ S0 display.print(" S1 = "); display.println(s1); // แสดงค่าเซนเซอร์ S1 display.print(" S2 = "); display.println(s2); // แสดงค่าเซนเซอร์ S2 display.print(" S3 = "); display.println(s3); // แสดงค่าเซนเซอร์ S3 display.display(); delay(50);
}
}/////////////////////////เลี้ยวซ้าย///////////////////////////////////////////
void L0(){while (1){analogs();Motor(-75,75);if(s0<A0){Motor(0,0);break;}}} //S0void L1(){while (1){analogs();Motor(-75,75);if(s1<A1){Motor(0,0);break;}}} //S1void L2(){while (1){analogs();Motor(-75,75);if(s2<A2){Motor(0,0);break;}}} //S2void L3(){while (1){analogs();Motor(-75,75);if(s3<A3){Motor(0,0);break;}}} //S3//////////////////////เลี้ยวขวา//////////////////////////////////////////void R0(){while (1){analogs();Motor(75,-75);if(s0<A0){Motor(0,0);break;}}} //S0void R1(){while (1){analogs();Motor(75,-75);if(s1<A1){Motor(0,0);break;}}} //S1void R2(){while (1){analogs();Motor(75,-75);if(s2<A2){Motor(0,0);break;}}} //S2void R3(){while (1){analogs();Motor(75,-75);if(s3<A3){Motor(0,0);break;}}} //S3/////////////////////////////////////////////////////////////////////
/////////////////////////ฟังก์ชันเส้นทาง////////////////////////////////////////void P() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุด/////////////////////////////{while (1){ s0 = analogRead(0); s1 = analogRead(1); s2 = analogRead(2); s3 = analogRead(3);if ((s1 > A1) and (s2 > A2)){Motor(75, 75);}if (s1 < A1){Motor(30, 75);}if (s2 < A2){Motor(75, 30);}if (s0 < A0){Motor(-40, 75);}if (s3 < A3){Motor(75, -40);}if (((s1 < A1) and (s2 < A2)) or ((s1 < A1) and (s0 < A0)) or ((s2 < A2) and (s3 < A3))) {Motor(75, 75);delay(180);Motor(0, 0);delay(100);break;}}}
void L(){L3();L1();delay(100);} /////// เลี้ยวซ้ายvoid R(){R0;R2();delay(100);} ///////// เลี้ยวขวา/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////
/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////
void Motor(int spl, int spr) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);{ if (spl > 0) { digitalWrite(DL1, LOW); digitalWrite(DL2, HIGH); analogWrite(PWML, spl); } else if (spl < 0) { digitalWrite(DL1, HIGH); digitalWrite(DL2, LOW); analogWrite(PWML, -spl); } else { digitalWrite(DL1, LOW); digitalWrite(DL2, LOW); } ////////////////////////////////////// if (spr > 0) { digitalWrite(DR1, HIGH); digitalWrite(DR2, LOW); analogWrite(PWMR, spr); } else if (spr < 0) { digitalWrite(DR1, LOW); digitalWrite(DR2, HIGH); analogWrite(PWMR, -spr); } else { digitalWrite(DR1, LOW); digitalWrite(DR2, LOW); }}
void menu0() /// code 1 ที่นี่{
sensor();
}
void menu1() /// code 1 ที่นี่{
servotest();
}
//////////////////////////////////เริ่มเขียนโค้ดในเมนูที่นี้///////////////////////void menu2() /// code 2 ที่นี่{
P();
}
void menu3() /// code 3 ที่นี่{
L();
}
void menu4() /// code 4 ที่นี่{ R();
}
void menu5() /// code 5 ที่นี่{ up();
}
void menu6() /// code 6 ที่นี่{ down();
}
void menu7() /// code 7 ที่นี่{Motor(-150,150);delay(1000);Motor(0,0);}
void menu8() /// code 8 ที่นี่{Motor(150,-150);delay(1000);Motor(0,0);}
void menu9() /// code 9 ที่นี่{ Motor(150,150);delay(100);Motor(0,0);}
void menu10() /// code 10 ที่นี่{
}
void menu11() /// code 11 ที่นี่{
}
void menu12() /// code 12 ที่นี่{
}
Page updated
Report abuse