เงื่อนไขที่จะทำให้หุ่นยนต์เดินตามเส้น

อุปกรณ์ประกอบ LAB

• 1. บอร์ด Kruro Nano

  2. มอเตอร์ หรือ เซตหุ่นยนต์เพื่อการศึกษา

ขั้นเตรียมการ

ตรวจสอบการเชื่อมต่อมอเตอร์ให้ถูกต้อง โดยมีวิธีเช็คดังนี้

1. เสียบสายไปที่ช่องต่อมอเตอร์ ให้ มอเตอร์ซ้ายอยู่ที่ Motor1

2. ตรวจสอบทิศทางเบื้องต้นโดยการหมุนล้อไปข้างหน้า ถ้าไฟสถานะมอเตอร์ขึ้นเป็นสีเขียนแสดงว่าถูกต้อง หากขึ้นสีแดงให้สลับขั้วมอเตอร์แล้วทดสอบอีกครั้ง

ตัวอย่าง Code

#include <SPI.h>

#include <Wire.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 = 300;

int CEN2 = 350;

int CEN3 = 300;

int CEN4 = 250;

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);

}

void loop()

{

int sw = digitalRead(button); // ให้ sw อ่านค่า digital จากพอร์ต 2(button)

int nob = analogRead(7); // ให้ nob เทียบเท่าค่า A7

int menu = map(nob, 0, 1023, 0, 1); // เทียบอัตราส่วนของพอร์ต A7 จาก 0-1023 เพื่อทำเป็นเมนู 0-1

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(" TKL ROBOT");

OLED.println(" ");

OLED.print(" "); // วรรค

OLED.print(nob); // แสดงค่าที่อ่านได้จาก nob หรือ Analog7

OLED.println(" MeNu");

OLED.display();

if ((sw == 1) and (menu == 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(100,100);} //เซนเซอร์ 2 และ 3 เจอขาว ให้มอเตอร์ซ้ายและขวาเดินหน้าด้วยความเร็ว 100

if(S2<CEN2) {run(0,100); } // เซนเซอซ้ายเจอดำ

if(S3<CEN3) {run(100,0); } // เซนเซอขวาเจอดำ

if(S1<CEN1) {run(-100,100); } // เซนเซอซ้ายเจอดำ

if(S4<CEN4) {run(100,-100); } // เซนเซอขวาเจอดำ

}

}

delay(100);

}

/* ***MENU*** */

///////////////////////////////////////////////////////////////////////

// ฟังก์ชั่นสั่งงานมอเตอร์ 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);

}

}

แบบฝึก

ให้ผู้เรียนทดลองเพิ่มความเร็วหรือลดความเพื่อให้การวิ่งตามเส้นไปได้เร็วขึ้น

เทคนิค

หุ่นยนต์แต่ละตัวประกอบขึ้นจากมือ อาจมีความผิดพลาดของโครงสร้างทำให้มีปัญหาในการเดินไม่ตรง

ดังนั้นก่อนที่ผู้เรียนจะเขียนโปรแกรมควรทำการจูนหุ่นหยนต์เสียก่อน ด้วยการใช้ เพิ่มกำลังไฟให้มอเตอร์ที่วิ่งช้ากว่า เพื่อให้หุ่นยนต์เดินได้ตรงที่สุด