สำหรับบอร์ด Kruro Robot Mega shield
การใช้งาน
- แทกเส้นเจอ ดำ-ดำ หยุด----- P();
- เลี้ยวซ้าย ---------------------- L();
- เลี้ยวขวา ---------------------- R();
- ถอยหลัง ---------------------- B();
Arduino IDE
////////////////////////////////////////////////
int button = 53; /// กำหนดปุ่มกดสวิตซ์ขา 53
int snd = 52; // ให้ snd คือพอร์ต 52 ใช้สำหรับฟังก์ชันเสียง
int s0 ;
int s1 ;
int s2 ;
int s3 ;
////////////////////////ตั้งค่าพอร์ต Analog//////////////
#define A0 600 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)
#define A1 600 // ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)
#define A2 600 // ค่ากลางเซนเซอร์ A2 (ขวากลาง)
#define A3 600 // ค่ากลางเซนเซอร์ A3 (ขวาสุด)
///////////////////////////////////////////////////////
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 8 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 8
#define PWML 2 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 2
//////////////////////////////////
#define DR1 11 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 11
#define DR2 10 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 10
#define PWMR 3 /// กำหนดสัญญาณ PWM ขวาพอร์ต 3
/////////////////////////////////////////////////
/////// ช่อง B //////////////////////////////////
#define DL1b 13 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 13
#define DL2b 12 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 12
#define PWMLb 4 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 4
//////////////////////////////////
#define DR1b 15 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 15
#define DR2b 14 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 14
#define PWMRb 5 /// กำหนดสัญญาณ PWM ขวาพอร์ต 5
/////// ช่อง c //////////////////////////////////
#define DL1c 17 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 17
#define DL2c 16 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 16
#define PWMLc 6 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 6
//////////////////////////////////
#define DR1c 23 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 23
#define DR2c 22 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 22
#define PWMRc 7 /// กำหนดสัญญาณ PWM ขวาพอร์ต 7
///////////////////////////////////////////////////////////////////////
void analogs()
{
s0 = analogRead(0);
s1 = analogRead(1);
s2 = analogRead(2);
s3 = analogRead(3);
}
void setup() {
pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT
////////กำหนดสัญญาณทุกขาเป็น OUTPUT ///////////////
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
////////////////////////////
pinMode(DL1b, OUTPUT);
pinMode(DL2b, OUTPUT);
pinMode(PWMLb, OUTPUT);
pinMode(DR1b, OUTPUT);
pinMode(DR2b, OUTPUT);
pinMode(PWMRb, OUTPUT);
//////////////////////
pinMode(DL1c, OUTPUT);
pinMode(DL2c, OUTPUT);
pinMode(PWMLc, OUTPUT);
pinMode(DR1c, OUTPUT);
pinMode(DR2c, OUTPUT);
pinMode(PWMRc, 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);
///////////////////เงื่อนไขการแทกเส้นที่นี่ หาอัตราที่เหมาะสมเอง///////////////
if (s0 < A0){run(-10, 100);delay(100);}
if (s1 < A1){run(30, 100);delay(20);}
if (s2 < A2){run(30, 100);delay(20);}
if (s3 < A3){run(100, -10);delay(100);}
else {run(100,100);delay(20);}
//////////////////////////////////////////////////////////////////
}
}
}
///////////////////////////////////////////////////////////////////
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);
}}
/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
void runb(int splb, int sprb) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{ if (splb > 0)
{
digitalWrite(DL1b, LOW);
digitalWrite(DL2b, HIGH);
analogWrite(PWMLb, splb);
}
else if (splb < 0)
{
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, LOW);
analogWrite(PWMLb, -splb);
}
else
{
digitalWrite(DL1b, LOW);
digitalWrite(DL2b, LOW);
analogWrite(PWMLb, -255);
}
//////////////////////////////////////
if (sprb > 0)
{
digitalWrite(DR1b, LOW);
digitalWrite(DR2b, HIGH);
analogWrite(PWMRb, sprb);
}
else if (sprb < 0)
{
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, LOW);
analogWrite(PWMRb, -sprb);
}
else
{
digitalWrite(DR1b, LOW);
digitalWrite(DR2b, LOW);
analogWrite(PWMRb, -255);
}}
//////////////////////////////////////
void runc(int splc, int sprc) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{ if (splc > 0)
{
digitalWrite(DL1c, LOW);
digitalWrite(DL2c, HIGH);
analogWrite(PWMLc, splc);
}
else if (splc < 0)
{
digitalWrite(DL1c, HIGH);
digitalWrite(DL2c, LOW);
analogWrite(PWMLc, -splc);
}
else
{
digitalWrite(DL1c, LOW);
digitalWrite(DL2c, LOW);
analogWrite(PWMLc, -255);
}
//////////////////////////////////////
if (sprc > 0)
{
digitalWrite(DR1c, LOW);
digitalWrite(DR2c, HIGH);
analogWrite(PWMRc, sprc);
}
else if (sprc < 0)
{
digitalWrite(DR1c, HIGH);
digitalWrite(DR2c, LOW);
analogWrite(PWMRc, -sprc);
}
else
{
digitalWrite(DR1c, LOW);
digitalWrite(DR2c, LOW);
analogWrite(PWMRc, -255);
}}
///////////////////////////////////////////////////////////////////////////////