Lab 7 การแทร็กเส้น V1

สำหรับบอร์ด 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);

}}

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