Lab 7 การแทร็กเส้น V.1
Lab 7 Line Tracking 1
หลักการเขียน Code แทร็กเส้น 4 เซนเซอร์ในวนรอบ
สำหรับ Arduino Nano
Arduino IDE Code
//////////////////ผนวกไลบรารี่จอแสดงผล//////////////////
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 OLED(-1);
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define DR1 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7
#define DR2 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8
#define PWMR 6 /// กำหนดสัญญาณ PWM ขวาพอร์ต 6
/////////////////////////////////
#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4
#define PWML 5 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 5
/////////////////ตั้งค่าพอร์ต Analog//////////////
#define A0 200 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)
#define A1 200 // ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)
#define A2 200 // ค่ากลางเซนเซอร์ A2 (ขวากลาง)
#define A3 200 // ค่ากลางเซนเซอร์ A3 (ขวาสุด)
/////////////////////////////////////////////////////
void setup() {
OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C
//////////////////////////////////////////////////
pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT
////////กำหนดสัญญาณทุกขาเป็น OUTPUT ///////////////
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
////////////////////////////
}
void loop() {
int sw = digitalRead(button);
if (sw==0){while(true){
/////////// ตั้งค่าเซนเซอร์ตามการอ่านพอร์ตอนาล็อก S0,S1,S2,S3
int s0 = analogRead(0);
int s1 = analogRead(1);
int s2 = analogRead(2);
int s3 = analogRead(3);
////////////////////////////////////////////////////////
/*
OLED.clearDisplay();
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 1
OLED.print(" S0 = "); OLED.println(s0); // แสดงค่าเซนเซอร์ S0
OLED.print(" S1 = "); OLED.println(s1); // แสดงค่าเซนเซอร์ S1
OLED.print(" S2 = "); OLED.println(s2); // แสดงค่าเซนเซอร์ S2
OLED.print(" S3 = "); OLED.println(s3); // แสดงค่าเซนเซอร์ S3
OLED.display();
*/
///////////////////เงื่อนไขการแทกเส้นที่นี่ หาอัตราที่เหมาะสมเอง///////////////
if (s0 < A0){run(-75, 75);delay(120);}
if (s1 < A1){run(0, 75);delay(120);}
if (s2 < A2){run(75, 0);delay(60);}
if (s3 < A3){run(75, -75);delay(60);}
else {run(50,50);delay(20);}
//////////////////////////////////////////////////////////////////
//delay(50); // เปลี่ยนค่าเซนเซอร์ทุก 0.1 วินาที
}
}
}
/////////////////////////////////////////////////////////////////
void run(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, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, spr);
}
else if (spr < 0)
{
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -spr);
}
else
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, LOW);
}
}
สำหรับบอร์ด Phurawong Robot Team
Arduino IDE Code
#include <SPI.h>
#include <Wire.h>
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
//////////////////ผนวกไลบรารี่จอแสดงผล//////////////////
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 OLED(-1);
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
//////////////////////////////////////
#define display_RESET 4
Adafruit_SSD1306 display(display_RESET);
Servo keepservo;
Servo upservo;
/////////////////////////////////////
//#define PWML 5 // motor L
#define IN1L 3 //
#define IN2L 5 //
//#define PWMR 6 // motor R
#define IN1R 6 //
#define IN2R 11 //
#define buttonPin 2
//////////////////////////////////////////
/////////////////ตั้งค่าพอร์ต Analog//////////////
#define A0 500 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)
#define A1 500 // ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)
#define A2 500 // ค่ากลางเซนเซอร์ A2 (ขวากลาง)
#define A3 500 // ค่ากลางเซนเซอร์ A3 (ขวาสุด)
/////////////////////////////////////////////////////
void setup() {
/////////////////////////////////////////
/////////// ช่วงกำหนด pin งดเปลี่ยนแปลง////////
// pinMode(PWML,OUTPUT);
pinMode(IN1L,OUTPUT);
pinMode(IN2L,OUTPUT);
// pinMode(PWMR,OUTPUT);
pinMode(IN1R,OUTPUT);
pinMode(IN2R,OUTPUT);
pinMode(button, INPUT);
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3D (for the 128x64)
digitalWrite(button,LOW);
}
void loop() {
int sw = digitalRead(button);
if (sw==1){while(true){
/////////// ตั้งค่าเซนเซอร์ตามการอ่านพอร์ตอนาล็อก S0,S1,S2,S3
int s0 = analogRead(0);
int s1 = analogRead(1);
int s2 = analogRead(2);
int s3 = analogRead(3);
////////////////////////////////////////////////////////
/*
OLED.clearDisplay();
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 1
OLED.print(" S0 = "); OLED.println(s0); // แสดงค่าเซนเซอร์ S0
OLED.print(" S1 = "); OLED.println(s1); // แสดงค่าเซนเซอร์ S1
OLED.print(" S2 = "); OLED.println(s2); // แสดงค่าเซนเซอร์ S2
OLED.print(" S3 = "); OLED.println(s3); // แสดงค่าเซนเซอร์ S3
OLED.display();
*/
///////////////////เงื่อนไขการแทกเส้นที่นี่ หาอัตราที่เหมาะสมเอง///////////////
if (s0 < A0){run(-100, 100);delay(70);}
if (s1 < A1){run(0, 100);delay(70);}
if (s2 < A2){run(100, 0);delay(30);}
if (s3 < A3){run(100, -100);delay(30);}
else {run(90,90);delay(20);}
//////////////////////////////////////////////////////////////////
//delay(50); // เปลี่ยนค่าเซนเซอร์ทุก 0.1 วินาที
}
}
}
/////////////////////////////////////////////////////////////////
void run(int sl,int sr){
if((sl==0) and (sr==0)){
digitalWrite(IN1L,HIGH);
digitalWrite(IN1R,HIGH);
digitalWrite(IN2L,HIGH);
digitalWrite(IN2R,HIGH);
delay(100);
digitalWrite(IN1L,LOW);
digitalWrite(IN1R,LOW);
digitalWrite(IN2L,LOW);
digitalWrite(IN2R,LOW);
}
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){
analogWrite(IN1L,sl);
digitalWrite(IN2L,LOW);
// analogWrite(PWML,sl);
}else if(sl<0){
digitalWrite(IN1L,LOW);
analogWrite(IN2L,-sl);
// analogWrite(PWML,-sl);
}else{
digitalWrite(IN1L,HIGH);
digitalWrite(IN2L,HIGH);
// analogWrite(PWML,50);
}
if(sr>0){
analogWrite(IN1R,sr);
digitalWrite(IN2R,LOW);
// analogWrite(PWMR,sr);
}else if(sr<0){
digitalWrite(IN1R,LOW);
analogWrite(IN2R,-sr);
// analogWrite(PWMR,-sr);
}else{
digitalWrite(IN1R,HIGH);
digitalWrite(IN2R,HIGH);
// analogWrite(PWMR,50);
}
}
สำหรับ Arduino UNO
Arduino IDE
//////////////////ผนวกไลบรารี่จอแสดงผล//////////////////
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define DR1 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7
#define DR2 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8
#define PWMR 6 /// กำหนดสัญญาณ PWM ขวาพอร์ต 6
/////////////////////////////////
#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4
#define PWML 5 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 5
/////////////////ตั้งค่าพอร์ต Analog//////////////
/////////////////////////////////////////////////////
void setup() {
//////////////////////////////////////////////////
pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT
pinMode(10, INPUT);
pinMode(11, INPUT);
pinMode(12, INPUT);
pinMode(13, INPUT);
////////กำหนดสัญญาณทุกขาเป็น OUTPUT ///////////////
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
////////////////////////////
}
void loop() {
int sw = digitalRead(button);
if (sw==1){while(true){
/////////// ตั้งค่าเซนเซอร์ตามการอ่านพอร์ตอนาล็อก S0,S1,S2,S3
int s0 = digitalRead(10);
int s1 = digitalRead(11);
int s2 = digitalRead(12);
int s3 = digitalRead(13);
////////////////////////////////////////////////////////
///////////////////เงื่อนไขการแทกเส้นที่นี่ หาอัตราที่เหมาะสมเอง///////////////
if (s0==1){run(-150,150);delay(500);}
if (s1==1){run(-150,150);delay(300);}
if (s2==1){run(150,-150);delay(300);}
if (s3==1){run(150,-150);delay(500);}
else {run(100,100);delay(20);}
//////////////////////////////////////////////////////////////////
delay(50); // เปลี่ยนค่าเซนเซอร์ทุก 0.1 วินาที
}
}
}
/////////////////////////////////////////////////////////////////
void run(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);
analogWrite(PWML, -255);
}
//////////////////////////////////////
if (spr > 0)
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, spr);
}
else if (spr < 0)
{
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -spr);
}
else
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -255);
}
}