LAB 11 หุ่นยนต์ทำภารกิจ

คำอธิบาย

คำสั่งต่างๆที่จำเป็นได้รวมอยู่ใน code นี้แล้ว

โดยได้สร้าง เมนู ให้จำนวน 12 เมนู

เมนู ที่ 0 ฟังก์ชั่นอ่านค่าแสงเซนเซอร์ sensor();

เมนูที่ 1 ฟังก์ชั่นอ่านค่าตำแหน่งเซอโว servotest();

เมนูที่ 2 ฟังก์ชั่นเดินตามเส้น เจอเส้นตัดแล้วหยุด P();

เมนูที่ 3 ฟังก์ชั่นเลี้ยวซ้ายเจอเส้นแล้วหยุด L();

เมนูที่ 4 ฟังก์ชั่นเลี้ยวขวาเจอเส้นแล้วหยุด R();

เมนูที่ 5 ฟังก์ชั่นยกกล่อง up();

เมนูที่ 6 ฟังก์ชั่นวางกล่อง down();

code ตัวอย่าง

#include <SPI.h>
#include <Wire.h>#include <Servo.h>#include <Adafruit_MCP3008.h>#include <Adafruit_GFX.h>#include <Adafruit_SSD1306.h>Adafruit_SSD1306 OLED(-1);Adafruit_MCP3008 adc;

#define PWML 5 // motor L#define IN1L 4 // #define IN2L 7 //
#define PWMR 6 // motor R#define IN1R 15 // #define IN2R 14 //

///////////////////////////////////////////////
const int button = 8;
////////ตัวแปรเก็บค่าแสงจากเซนเซอร์ int S1 = 0; int S2 = 0; int S3 = 0; int S4 = 0;

//////////ค่ากลางเซนเซอร์ เอาไว้เปนียบเทียบค่าแสง//////////int CEN1 = 550;int CEN2 = 350;int CEN3 = 550;int CEN4 = 550;
void P() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุด/////////////////////////////{while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3if ((S2 > CEN2) and (S3 > CEN3)){run(100, 100);}if (S2 < CEN2){run(-50, 100);}if (S3 < CEN3){run(100, -50);}//if (S1 < CEN1){run(-40, 75);}//if (S4 < CEN4){run(75, -40);}if (((S2 < CEN2) and (S3 < CEN3)) or ((S2 < CEN2) and (S1 < CEN1)) or ((S3 < CEN3) and (S4 < CEN4))) {run(100, 100);delay(180);run(0, 0);delay(100);tone(3,1000,100);break;}}}
void P(int t) ///////////// ฟังก์ชันแทกเส้น/////////////////////////////{ int j = 0; while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3 if ((S2 > CEN2) and (S3 > CEN3)){run(75, 75);}if (S2 < CEN2){run(-30, 75);}if (S3 < CEN3){run(75, -30);}//if (S1 < CEN1){run(-40, 75);}//if (S4 < CEN4){run(75, -40);}if (((S2 < CEN2) and (S3 < CEN3)) or ((S2 < CEN2) and (S1 < CEN1)) or ((S3 < CEN3) and (S4 < CEN4))){run(75, 75);}if (j>t){run(0, 0);tone(3,900,500);break;}j++;}}//////////////////////////////////////////////////////////////////////////////////
void L(){L0();L1();delay(100);tone(3,1000,50);} /////// เลี้ยวซ้ายvoid R(){R3();R2();delay(100);tone(3,1000,50);} ///////// เลี้ยวvoid L0(){while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3 run(-100,100);if(S1<CEN1){run(0,0);break;}}} //S0void L1(){while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3 run(-100,100);if(S2<CEN2){run(0,0);break;}}} //S1void L2(){while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3R run(-100,100);if(S3<CEN3){run(0,0);break;}}} //S2void L3(){while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3 run(-100,100);if(S4<CEN4){run(0,0);break;}}} //S3//////////////////////เลี้ยวขวา//////////////////////////////////////////void R0(){while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3 run(100,-100);if(S1<CEN1){run(0,0);break;}}} //S0void R1(){while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3 run(100,-100);if(S2<CEN2){run(0,0);break;}}} //S1void R2(){while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3 run(100,-100);if(S3<CEN3){run(0,0);break;}}} //S2void R3(){while (1){ S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3 run(100,-100);if(S4<CEN4){run(0,0);break;}}} //S3void b(){run(-75,-70);delay(1000);run(0,0);delay(600);} // ถอยหลัง

///////////////////////องศา เซอร์โว///////////////////////const int v_up = 60 ; // <---- กรอกองศาเซอร์โวตรงนี้const int v_down = 0; // <----
///////////////ตั้งพอร์ตเซอร์โว////////////////////////////////int servo1 = 2; // เปิดใช้งาน s1 ที่พอร์ต 2
Servo sv1; // ประกาศการใช้งาน sv1

void setup(){ OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64) adc.begin(13, 11, 12, 10);//เริ่มจากใช้ adc จาก library pinMode(PWML,OUTPUT); pinMode(IN1L,OUTPUT); pinMode(IN2L,OUTPUT); pinMode(PWMR,OUTPUT); pinMode(IN1R,OUTPUT); pinMode(IN2R,OUTPUT); pinMode(button, INPUT); ///////////// ตั้งพอร์ตเซอร์โว////////////sv1.attach(servo1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1

}
void set_servo(){ while(1){ int vr = analogRead(A7); // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 7 int nob = map(vr, 0, 1023, 0, 180); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180 OLED.clearDisplay(); // เคลียร์หน้าจอ oled OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0 OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2 OLED.println(nob); // นำค่า nob มาแสดงใน oled OLED.display(); // เปิดฟังก์ชันแสดงผล sv1.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้ delay(50); // หน่วงเวลา 0.05 วินาที}}

void sensor(){ while (true) { S1 = adc.readADC(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0 S2 = adc.readADC(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1 S3 = adc.readADC(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2 S4 = adc.readADC(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
OLED.clearDisplay(); //ล้างหน้าจอ OLED.setTextColor(WHITE,BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ จอที่ใช้ไม่สามารถเปลี่ยนสีได้ OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0 OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2 OLED.print(" S1 : "); // แสดงข้อความ S1 : OLED.println(S1); // แสดงค่า S1 OLED.print(" S2 : "); // แสดงข้อความ S2 : OLED.println(S2); // แสดงค่า S2 OLED.print(" S3 : "); // แสดงข้อความ S3 : OLED.println(S3); // แสดงค่า S3 OLED.print(" S4 : "); // แสดงข้อความ S4 : OLED.println(S4); // แสดงค่า S4 OLED.display(); // สั่งให้ จอแสดงผลdelay(100); }}
void loop(){ int sw = digitalRead(button); // ให้ sw อ่านค่า digital จากพอร์ต 2(button) int nob = analogRead(7); // ให้ nob เทียบเท่าค่า A7 int menu = map(nob, 0, 1023, 0, 9); // เทียบอัตราส่วนของพอร์ต A7 จาก 0-1023 เพื่อทำเป็นเมนู 0-12 OLED.clearDisplay(); // ล้างค่าหน้าจอ OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0 OLED.setTextSize(3); // เซตขนาดอักษรมีขนาดเป็น 2 OLED.print(" "); // วรรค OLED.println(menu); // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-12 OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2 OLED.println(" "); OLED.println(" FB:OAT.KT.ROBOT"); OLED.println(" "); OLED.print(" "); // วรรค OLED.print(nob); // แสดงค่าที่อ่านได้จาก nob หรือ Analog7 OLED.println(" MeNu"); OLED.display(); if ((sw == 1) and (menu == 0)) { sensor(); } if ((sw == 1) and (menu == 1)) { set_servo(); } 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(); } delay(100);}
void up() { /*เก็บกล่อง/กระป๋อง*/
sv1.write(v_up);delay(500); }
void down() { /*วางกล่อง/กระป๋อง*/sv1.write(v_down);delay(500);
}
/* ***MENU*** */
void menu2() /// code 2 ที่นี่{ P();}void menu3() /// code 3 ที่นี่{ L();}void menu4() /// code 4 ที่นี่{ R();}void menu5() /// code 5 ที่นี่{ b();}void menu6() /// code 6 ที่นี่{ run(100,100);delay(2000); run(100,-100);delay(2000); run(-100,100);delay(2000); run(-100,-100);delay(2000);}void menu7() /// code 7 ที่นี่{ }void menu8() /// code 7 ที่นี่{
}
///////////////////////////////////////////////////////////////////////// ฟังก์ชั่นสั่งงานมอเตอร์ Motor(ความเร็วซ้าย,ความเร็วขวา);//// Motor(100,100); // เดินหน้าทั้งสองล้อ// Motor(-100,-100); // ถอยหลังทั้งสองล้อ// Motor(-100,100); // ล้อซ้ายถอย ล้อขวาเดินหน้า// Motor(100,-100); // ล้อซ้ายเดินหน้า ล้อขวาถอย// Motor(0,0); // หยุดทั้ง สองล้อ//// สามารถกำหนดความเร็วได้ -255 ถึง 255/////////////////////////////////////////////////////////////////////////void run(int sl,int sr){
if(sr>255) sr = 255; else if(sr<-255) sr = -255; if(sl>255) sl = 255; else if(sl<-255) sl = -255; if(sl>0){ digitalWrite(IN1L,HIGH); digitalWrite(IN2L,LOW); analogWrite(PWML,sl); }else if(sl<0){ digitalWrite(IN1L,LOW); digitalWrite(IN2L,HIGH); analogWrite(PWML,-sl); }else{ digitalWrite(IN1L,HIGH); digitalWrite(IN2L,HIGH);
} if(sr>0){ digitalWrite(IN1R,HIGH); digitalWrite(IN2R,LOW); analogWrite(PWMR,sr); }else if(sr<0){ digitalWrite(IN1R,LOW); digitalWrite(IN2R,HIGH); analogWrite(PWMR,-sr); }else{ digitalWrite(IN1R,HIGH); digitalWrite(IN2R,HIGH);
} }

แหล่งเรียนรู้เพิ่มเติม จากศูนย์หุ่นยนต์โรงเรียนกมลาไสย คลิก

Page updated

Report abuse