Krang

1. เมนู 0 วัดความเข้มแสง

2. เมนู 1 sv_knob(); วัดองศาเซอร์โว

3.คำสั่งเซอร์โวยก v_before 

4.คำสั่งเซอร์โววาง v_after

5. คำสั่งแทกเส้นหยุดตรงเส้นแยก PT(); 

6. คำสั่งแทกเส้นตามเวลา PD(เวลา); 

7. คำสั่งแทกเส้นพื้นเขียวหยุด PD(เวลา); 

8.คำสั่งเลี้ยวขวา RD();

9. คำสั่งเลี้ยวซ้าย LD);

10. คำสั่งถอยหลังไม่ใช้เซนเซอร์ BD();

11. คำสั่งถอยหลังใช้เซนเซอร์B();

12. เจอเขียวแล้วหยุดเพื่อปล่อยกล่องPL();

13. ปล่อยกล่องleave();

// Code v1.15 (Refactored)

#include <Wire.h>

#include <SPI.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

#include <Servo.h>

#include <Adafruit_MCP3008.h>

Adafruit_SSD1306 OLED(-1);

Adafruit_MCP3008 adc, adc2;

/////////// ตั้งค่าปุ่มกด ///////////////////

uint8_t button = 2;   /// กำหนดปุ่มกดสวิตซ์ขา 2

uint8_t ss1 = 14;     // เปิดใช้งาน s1 ที่พอร์ต 14

uint8_t ss2 = 15;     // เปิดใช้งาน s2 ที่พอร์ต 15

Servo sv1;     

Servo sv2;

uint16_t angle_ss1 = 90;

uint16_t angle_ss2 = 90;

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

/// ตั้งค่าพอร์ตมอเตอร์ ////////////////////////////////////

#define DR1  7  /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7

#define DR2  8  /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8

#define PWMR 6  /// กำหนดสัญญาณ PWM ขวาพอร์ต 6

#define DL1  4  // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 4

#define DL2  9  // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 9

#define PWML 5  /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 5

///////////////// ตั้งค่าพอร์ต Analog //////////////

#define a1 500 

#define a2 500

#define a3 500 

#define a4 500

#define a5 500 

#define a6 500

#define a7 500 

#define a8 500

#define b1 500

#define b2 500 

#define b3 500

#define b4 500 

#define b5 500

#define b6 500 

#define b7 500

#define b8 500

#define ac6 475

uint16_t analogColor = 0;

uint16_t s1, s2, s3, s4, s5, s6, s7, s8;

uint16_t sb1, sb2, sb3, sb4, sb5, sb6, sb7, sb8;

#define ROBOT_MODE 1

// Forward Declarations

void analogs();

void analogs_2();

void sensor();

void sv_knob();

void menu2();

void menu3();

void menu4();

void menu5();

void menu6();

void menu7();

void menu8();

void menu9();

void T();

void T(byte speed);

void P();

void P(byte speed);

void P(byte speed, unsigned long t);

void PS();

void PS(byte speed);

void PP();

void PP(byte speed);

void PP(byte speed, byte ct);

void PB();

void PB(byte speed);

void PB(byte speed, unsigned long t);

void PPB();

void PPB(byte speed);

void PPB(byte speed, byte ct);

void ST(uint16_t t);

void BT(uint16_t t);

void LD(uint16_t t);

void RD(uint16_t t);

void ST(uint16_t t, byte s);

void BT(uint16_t t, byte s);

void sv_set(uint8_t set_angle);

void up(byte angle1);

void up(byte angle1, byte sp);

void down(byte angle1);

void down(byte angle1, byte sp);

void keep(uint8_t angle2);

void keep(uint8_t angle2, byte sp);

void pause(uint8_t angle2);

void pause(uint8_t angle2, uint8_t sp);

void up();

void down();

void kom();

void keep();

void pause();

void L();

void L0();

void L1();

void L2();

void R();

void R0();

void R1();

void R2();

void waitfor(int t);

void color();

void run(int spl, int spr);

void motorTest();

void yellow_1();

void yellow_2();

void yellow_3();

void yellow_4();

void red_1();

void red_2();

void red_3();

void red_4();

void start();

void backtostart();

void analogs() {

  s1 = adc.readADC(0);

  s2 = adc.readADC(1);

  s3 = adc.readADC(2);

  s4 = adc.readADC(3);

  s5 = adc.readADC(4);

  s6 = adc.readADC(5);

  s7 = adc.readADC(6);

  s8 = adc.readADC(7);

}

void analogs_2() {

  sb1 = adc2.readADC(0);

  sb2 = adc2.readADC(1);

  sb3 = adc2.readADC(2);

  sb4 = adc2.readADC(3);

  sb5 = adc2.readADC(4);

  sb6 = adc2.readADC(5);

  sb7 = adc2.readADC(6);

  sb8 = adc2.readADC(7);

}

void setup() {

  adc.begin(10);

  adc2.begin(3);

  OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C); 

  pinMode(2, INPUT); 

  pinMode(DL1, OUTPUT);

  pinMode(DL2, OUTPUT);

  pinMode(PWML, OUTPUT);

  pinMode(DR1, OUTPUT);

  pinMode(DR2, OUTPUT);

  pinMode(PWMR, OUTPUT);

  sv1.attach(ss1); 

  sv2.attach(ss2); 

  sv_set(90);

}

void loop() {

  bool sw = digitalRead(button);     

  uint16_t nob = analogRead(7);          

  uint16_t menu = map(nob, 0, 1000, 0, 9); 

  OLED.clearDisplay();              

  OLED.setTextColor(WHITE, BLACK);  

  OLED.setCursor(0, 0);       

  OLED.setTextSize(2);        

  OLED.print(F("   "));         

  OLED.println(menu);        

  OLED.setTextSize(1);        

  OLED.println(F("  KruRo"));     

  OLED.print(F("  "));                      

  OLED.print(nob);                     

  OLED.println(F(" Robot"));     

  OLED.display();

  if ((sw == ROBOT_MODE) and (menu == 0)) { sensor(); }

  if ((sw == ROBOT_MODE) and (menu == 1)) { sv_knob(); }

  if ((sw == ROBOT_MODE) and (menu == 2)) { menu2(); }

  if ((sw == ROBOT_MODE) and (menu == 3)) { menu3(); }

  if ((sw == ROBOT_MODE) and (menu == 4)) { menu4(); }

  if ((sw == ROBOT_MODE) and (menu == 5)) { menu5(); }

  if ((sw == ROBOT_MODE) and (menu == 6)) { menu6(); }

  if ((sw == ROBOT_MODE) and (menu == 7)) { menu7(); }

  if ((sw == ROBOT_MODE) and (menu == 8)) { menu8(); }

  if ((sw == ROBOT_MODE) and (menu == 9)) { menu9(); }

  delay(100);

}

void sensor() {

  while (true) {

    analogs();

    analogs_2();

    uint16_t vr = analogRead(A7);  

    uint16_t nob = map(vr, 0, 1023, 0, -180); 

    OLED.clearDisplay();

    OLED.setTextColor(WHITE, BLACK);  

    OLED.setCursor(0, nob);       

    OLED.setTextSize(1);        

    OLED.print(F("       a1 = ")); OLED.println(s1);  

    OLED.print(F("       a2 = ")); OLED.println(s2);  

    OLED.print(F("       a3 = ")); OLED.println(s3);  

    OLED.print(F("       a4 = ")); OLED.println(s4);  

    OLED.print(F("       a5 = ")); OLED.println(s5);  

    OLED.print(F("       a6 = ")); OLED.println(s6);  

    OLED.print(F("       a7 = ")); OLED.println(s7);  

    OLED.print(F("       a8 = ")); OLED.println(s8);  

    OLED.println(F(""));

    OLED.print(F("       b1 = ")); OLED.println(sb1);  

    OLED.print(F("       b2 = ")); OLED.println(sb2);  

    OLED.print(F("       b3 = ")); OLED.println(sb3);  

    OLED.print(F("       b4 = ")); OLED.println(sb4);  

    OLED.print(F("       b5 = ")); OLED.println(sb5);  

    OLED.print(F("       b6 = ")); OLED.println(sb6);  

    OLED.print(F("       b7 = ")); OLED.println(sb7);  

    OLED.print(F("       b8 = ")); OLED.println(sb8);  

    OLED.println(F(""));

    OLED.print(F("       ac6 = ")); OLED.println(analogRead(A6));  

    OLED.display();

    delay(50);

  }

}

//////////////////////////// ฟังก์ชันเซอร์โว ////////////////////////////////////////

void sv_knob() {

  bool servo_state = 0;

  while(true){

    if(servo_state == 0){

      OLED.clearDisplay();    

      OLED.setTextColor(WHITE, BLACK);  

      OLED.setCursor(0, 0);       

      OLED.setTextSize(2);        

      OLED.print(F("Servo 1"));     

      OLED.display();        

      delay(1000);

      while(1){

        uint16_t sw = digitalRead(2);

        uint16_t vr = analogRead(A7);  

        uint16_t nob = map(vr, 0, 1023, 0, 180); 

        OLED.clearDisplay();    

        OLED.setTextColor(WHITE, BLACK);  

        OLED.setCursor(0, 0);       

        OLED.setTextSize(2);        

        OLED.print(F("SV1 = "));     

        OLED.println(nob);     

        OLED.display();        

        sv1.write(nob);        

        delay(50);             

        if(sw == 1){

          servo_state = 1;

          break;

        }

      }

    }

    if(servo_state == 1){

      OLED.clearDisplay();    

      OLED.setTextColor(WHITE, BLACK);  

      OLED.setCursor(0, 0);       

      OLED.setTextSize(2);        

      OLED.print(F("Servo 2"));     

      OLED.display();        

      delay(1000);

      while(1){

        uint16_t sw = digitalRead(2);

        uint16_t vr = analogRead(A7);  

        uint16_t nob = map(vr, 0, 1023, 0, 180); 

        OLED.clearDisplay();    

        OLED.setTextColor(WHITE, BLACK);  

        OLED.setCursor(0, 0);       

        OLED.setTextSize(2);        

        OLED.print(F("SV2 = "));     

        OLED.println(nob);     

        OLED.display();        

        sv2.write(nob);        

        delay(50);             

        if(sw == 1){

          servo_state = 0;

          break;

        }

      }

    }

  }

}

//////////////////////// ฟังก์ชันเส้นทาง ////////////////////////////////////////

void T() {

  analogs();

  if(s2 < a2) { run(-40, 100); }

  else if(s3 < a3) { run(80, 100); }

  else if(s6 < a6) { run(100, 80); }

  else if(s7 < a7) { run(100, -40); }

  else { run(100, 100); }

}

void T(byte speed) {

  analogs();

  if(s2 < a2) { run(-40, 100); }

  else if(s3 < a3) { run(80, 100); }

  else if(s6 < a6) { run(100, 80); }

  else if(s7 < a7) { run(100, -40); }

  else { run(speed, speed); }

}

void P() {

  while (1) {

    analogs();

    if (((s1 < a1) and (s2 < a2)) or ((s7 < a7) and (s8 < a8))) {

      run(100, 100); delay(180); run(0, 0); delay(100); break; 

    } else {

      T();

    }

  }

}

void P(byte speed) {

  while (1) {

    analogs();

    if (((s1 < a1) and (s2 < a2)) or ((s7 < a7) and (s8 < a8))) {

      run(100, 100); delay(180); run(0, 0); delay(100); break; 

    } else {

      T(speed);

    }     

  }

}

void P(byte speed, unsigned long t) {

  unsigned long start_time = millis();

  while (1) {

    analogs();

    if (millis() - start_time >= t) { run(0, 0); break; } 

    else if (((s1 < a1) and (s2 < a2)) or ((s7 < a7) and (s8 < a8))) {

      run(speed, speed); delay(180); 

    } else {

      T(speed);

    }

  }

}

void PS() {

  while (1) {

    analogs();

    if (((s1 < a1) and (s2 < a2)) or ((s7 < a7) and (s8 < a8))) {

      run(0, 0); delay(100); break; 

    } else {

      T();

    }     

  }

}

void PS(byte speed) {

  while (1) {

    analogs();

    if (((s1 < a1) and (s2 < a2)) or ((s7 < a7) and (s8 < a8))) {

      run(0, 0); delay(100); break; 

    } else {

      T(speed);

    }     

  }

}

void PP() {

  while (1) {

    analogs();

    if (((s1 < a1) and (s2 < a2)) or ((s7 < a7) and (s8 < a8))) {

      run(100, 100); delay(100); break; 

    } else {

      T();

    }

  }

}

void PP(byte speed) {

  while (1) {

    analogs();

    if (((s1 < a1) and (s2 < a2)) or ((s7 < a7) and (s8 < a8))) {

      run(speed, speed); delay(100); break; 

    } else {

      T(speed);

    }     

  }

}

void PP(byte speed, byte ct) {

  byte counter = 0;

  while (1) {

    analogs();

    if(counter == ct) {

      run(0, 0); delay(100); break;

    }

    else if (((s1 < a1) and (s2 < a2)) or ((s7 < a7) and (s8 < a8))) {

      run(speed, speed); delay(100); counter++;

    } else {

      T(speed);

    }     

  }

}

void PB() {

  while (1) {

    analogs();

    if (((sb1 < b1) and (sb2 < b2)) or ((sb7 < b7) and (sb8 < b8))) {

      run(100, 100); delay(180); run(0, 0); delay(100); break; 

    }

    else if(sb2 < b2) { run(-100, 40); }

    else if(sb3 < b3) { run(-100, -70); }

    else if(sb6 < b6) { run(-70, -100); }

    else if(sb7 < b7) { run(40, -100); }

    else { run(-100, -100); }     

  }

}

void PB(byte speed) {

  while (1) {

    analogs();

    if (((sb1 < b1) and (sb2 < b2)) or ((sb7 < b7) and (sb8 < b8))) {

      run(100, 100); delay(180); run(0, 0); delay(100); break; 

    }

    else if(sb2 < b2) { run(-100, 40); }

    else if(sb3 < b3) { run(-100, -70); }

    else if(sb6 < b6) { run(-70, -100); }

    else if(sb7 < b7) { run(40, -100); }

    else { run(-speed, -speed); }     

  }

}

void PB(byte speed, unsigned long t) {

  unsigned long start_time = millis();

  while (1) {

    analogs();

    if (millis() - start_time >= t) { run(0, 0); break; }

    else if(sb2 < b2) { run(-100, 40); }

    else if(sb3 < b3) { run(-100, -70); }

    else if(sb6 < b6) { run(-70, -100); }

    else if(sb7 < b7) { run(40, -100); }

    else { run(-speed, -speed); }

  }

}

void PPB() {

  while (1) {

    analogs();

    if (((sb1 < b1) and (sb2 < b2)) or ((sb7 < b7) and (sb8 < b8))) {

      run(100, 100); delay(100); break; 

    }

    else if(sb2 < b2) { run(-100, 40); }

    else if(sb3 < b3) { run(-100, -70); }

    else if(sb6 < b6) { run(-70, -100); }

    else if(sb7 < b7) { run(40, -100); }

    else { run(-100, -100); }     

  }

}

void PPB(byte speed) {

  while (1) {

    analogs();

    if (((sb1 < b1) and (sb2 < b2)) or ((sb7 < b7) and (sb8 < b8))) {

      run(speed, speed); delay(100); break; 

    }

    else if(sb2 < b2) { run(-100, 40); }

    else if(sb3 < b3) { run(-100, -70); }

    else if(sb6 < b6) { run(-70, -100); }

    else if(sb7 < b7) { run(40, -100); }

    else { run(-speed, -speed); }     

  }

}

void PPB(byte speed, byte ct) {

  byte counter = 0;

  while (1) {

    analogs();

    if(counter == ct) {

      run(0, 0); delay(100); break;

    }

    if (((sb1 < b1) and (sb2 < b2)) or ((sb7 < b7) and (sb8 < b8))) {

      run(speed, speed); delay(100); counter++;

    }

    else if(sb2 < b2) { run(-100, 40); }

    else if(sb3 < b3) { run(-100, -70); }

    else if(sb6 < b6) { run(-70, -100); }

    else if(sb7 < b7) { run(40, -100); }

    else { run(-speed, -speed); }     

  }

}

void ST(uint16_t t) { run(100, 100); delay(t); run(0, 0); delay(100); } 

void BT(uint16_t t) { run(-100, -100); delay(t); run(0, 0); delay(100); } 

void LD(uint16_t t) { run(-100, 100); delay(t); run(0, 0); delay(100); } 

void RD(uint16_t t) { run(100, -100); delay(t); run(0, 0); delay(100); } 

void ST(uint16_t t, byte s) { run(s, s); delay(t); run(0, 0); delay(100); } 

void BT(uint16_t t, byte s) { run(-(s), -(s)); delay(t); run(0, 0); delay(100); } 

/////////////////////// ฟังก์ชันเซอร์โว //////////////////////////

void sv_set(uint8_t set_angle) {

  sv1.write(set_angle); 

  sv2.write(set_angle);

}

void up(byte angle1) { 

  for(uint8_t i = angle_ss1; i >= angle1; i--) { 

    sv1.write(i);

    angle_ss1 = i;

    delay(3);

  }

  sv1.write(angle1);

  delay(100);

}

void up(byte angle1, byte sp) { 

  for(uint8_t i = angle_ss1; i >= angle1; i--) { 

    sv1.write(i);

    angle_ss1 = i;

    delay(sp);

  }

  sv1.write(angle1);

  delay(100);

}

void down(byte angle1) { 

  for(uint8_t i = angle_ss1; i <= angle1; i++) { 

    sv1.write(i);

    angle_ss1 = i;

    delay(3);

  }

  sv1.write(angle1);

  delay(100);

}

void down(byte angle1, byte sp) { 

  for(uint8_t i = angle_ss1; i <= angle1; i++) { 

    sv1.write(i);

    angle_ss1 = i;

    delay(sp);

  }

  sv1.write(angle1);

  delay(100);

}

void keep(uint8_t angle2) { 

  for(uint8_t i = angle_ss2; i >= angle2; i--) { 

    sv2.write(i);

    angle_ss2 = i;

    delay(3);

  }

  sv2.write(angle2);

  delay(100);

}

void keep(uint8_t angle2, byte sp) { 

  for(uint8_t i = angle_ss2; i >= angle2; i--) { 

    sv2.write(i);

    angle_ss2 = i;

    delay(sp);

  }

  sv2.write(angle2);

  delay(100);

}

void pause(uint8_t angle2) { 

  for(uint8_t i = angle_ss2; i <= angle2; i++) { 

    sv2.write(i);

    angle_ss2 = i;

    delay(3);

  }

  sv2.write(angle2);

  delay(100);

}

void pause(uint8_t angle2, uint8_t sp) { 

  for(uint8_t i = angle_ss2; i <= angle2; i++) { 

    sv2.write(i);

    angle_ss2 = i;

    delay(sp);

  }

  sv2.write(angle2);

  delay(100);

}

void up() { 

  uint8_t angle1 = 44;    

  for(uint8_t i = angle_ss1; i >= angle1; i--) { 

    sv1.write(i);

    angle_ss1 = 80;

    delay(3);

  }

  sv1.write(angle1);

  delay(100);

}

void down() { 

  uint8_t angle1 = 119;

  for(uint8_t i = angle_ss1; i <= angle1; i++) { 

    sv1.write(i);

    angle_ss1 = i;

    delay(3);

  }

  sv1.write(angle1);

  delay(100);

}

void kom() { 

  uint8_t angle1 = 150;

  for(uint8_t i = angle_ss1; i <= angle1; i++) { 

    sv1.write(i);

    angle_ss1 = i;

    delay(3);

  }

  sv1.write(angle1);

  delay(100);

}

void keep() { 

  uint8_t angle2 = 32;

  for(uint8_t i = angle_ss2; i >= angle2; i--) { 

    sv2.write(i);

    angle_ss2 = i;

    delay(3);

  }

  sv2.write(angle2);

  delay(100);

}

void pause() { 

  uint8_t angle2 = 107;

  for(uint8_t i = angle_ss2; i <= angle2; i++) { 

    sv2.write(i);

    angle_ss2 = i;

    delay(3);

  }

  sv2.write(angle2);

  delay(100);

}

///////////////////////// เลี้ยวซ้าย ///////////////////////////////////////////

void L() { L0(); L1(); L2(); delay(100); } 

void L0() { while (1) { analogs(); run(-150, 150); if(s1 < a1) { run(0, 0); break; } } } 

void L1() { while (1) { analogs(); run(-150, 150); if(s2 < a2) { run(0, 0); break; } } } 

void L2() { while (1) { analogs(); run(-150, 150); if(s4 < a4) { run(0, 0); break; } } } 

////////////////////// เลี้ยวขวา //////////////////////////////////////////

void R() { R0(); R1(); R2(); delay(100); } 

void R0() { while (1) { analogs(); run(150, -150); if(s8 < a8) { run(0, 0); break; } } } 

void R1() { while (1) { analogs(); run(150, -150); if(s7 < a7) { run(0, 0); break; } } } 

void R2() { while (1) { analogs(); run(150, -150); if(s5 < a5) { run(0, 0); break; } } } 

void waitfor(int t) {

  int time_sec1 = 0;

  int time_sec2 = 0;

  byte time_detect = 5;

  while(1) {

    analogColor = analogRead(A6);

    if((time_sec2 == time_detect) or (time_sec1 == t)) {

      break;

    }

    else if(analogColor > 200) {

      analogColor = analogRead(A6);

      delay(1000);

      time_sec2++;

      OLED.clearDisplay();

      OLED.setTextColor(WHITE, BLACK);  

      OLED.setCursor(0, 0);     

      OLED.setTextSize(1);      

      OLED.print(time_sec2);     

      OLED.println(F(" sec.")); 

      OLED.print("Color : ");     

      OLED.print(analogColor);     

      OLED.display();           

    }

    else {

      analogColor = analogRead(A6);

      delay(1000);

      time_sec1++;

      OLED.clearDisplay();

      OLED.setTextColor(WHITE, BLACK);  

      OLED.setCursor(0, 0);     

      OLED.setTextSize(1);      

      OLED.print(time_sec1);     

      OLED.println(F(" sec.")); 

      OLED.print("Color : ");     

      OLED.print(analogColor);     

      OLED.display();           

    }

  }

}

uint8_t count_color_red = 0;

uint8_t count_color_yellow = 0;

bool colorDetected = false;

void color() {

  if(analogColor > ac6) {        

    if(colorDetected) return;

    colorDetected = true;

    count_color_yellow++;

    if(count_color_yellow > 4) return;

    else if(count_color_yellow == 1) { yellow_1(); colorDetected = false; }

    else if(count_color_yellow == 2) { yellow_2(); colorDetected = false; }

    else if(count_color_yellow == 3) { yellow_3(); colorDetected = false; }

    else if(count_color_yellow == 4) { yellow_4(); colorDetected = false; }

  }

  else if(analogColor < ac6) {   

    if(colorDetected) return;

    colorDetected = true;

    count_color_red++;

    if(count_color_red > 4) return;

    else if(count_color_red == 1) { red_1(); colorDetected = false; }

    else if(count_color_red == 2) { red_2(); colorDetected = false; }

    else if(count_color_red == 3) { red_3(); colorDetected = false; }

    else if(count_color_red == 4) { red_4(); colorDetected = false; }

  }

  else {

    backtostart();

  }

}

///////////////////////////// ฟังก์ชันมอเตอร์ ///////////////////////////////////

void run(int spl, int spr) {

  if (spl > 0) {

    spl = abs(spl);

    digitalWrite(DL1, LOW);

    digitalWrite(DL2, HIGH);

    analogWrite(PWML, spl);

  }

  else if (spl < 0) {

    spl = abs(spl);

    digitalWrite(DL1, HIGH);

    digitalWrite(DL2, LOW);

    analogWrite(PWML, spl);

  }

  else {

    digitalWrite(DL1, HIGH);

    digitalWrite(DL2, HIGH);

  }

  if (spr > 0) {

    spr = abs(spr);

    digitalWrite(DR1, LOW);

    digitalWrite(DR2, HIGH);

    analogWrite(PWMR, spr);

  }

  else if (spr < 0) {

    spr = abs(spr);

    digitalWrite(DR1, HIGH);

    digitalWrite(DR2, LOW);

    analogWrite(PWMR, spr);

  }

  else {

    digitalWrite(DR1, HIGH);

    digitalWrite(DR2, HIGH);

  }

}

void motorTest() {

  run(100, 100); delay(500); run(0, 0); delay(500);

  run(-100, 100); delay(500); run(0, 0); delay(500);

  run(100, -100); delay(500); run(0, 0); delay(500);

  run(-100, -100); delay(500); run(0, 0); delay(500);

}

//////////////////////////////// เมนูการทำงาน ////////////////////////////////////////

void menu2() { motorTest(); }

void menu3() { 

  P(); R(); PP(100, 3); PS(); kom(); pause(); start();

}

void menu4() {}

void menu5() { kom(); }

void menu6() { PS(); }

void menu7() { R(); }

void menu8() { keep(); up(); }

void menu9() { down(); pause(); }

//////////////////////////////// การคัดแยกสี ////////////////////////////////////////

void yellow_1() {

  keep(); up(); BT(300); R(); P(); L(); PP(130, 2); P(); R(); P(); LD(150); down(); pause(); up(); keep();

  R(); R(); P(); L(); P(); R(); PP(130); P(); R(); PP(130, 2); PS(); pause(); kom(); 

}

void yellow_2() {

  keep(); up(); BT(300); R(); PP(130, 2); P(); L(); PP(130, 2); PS(); LD(150); down(); pause(); up(); keep();

  R(); L(); PP(120, 2); P(); R(); PP(130, 2); PS(); pause(); kom();   

}

void yellow_3() {

  keep(); up(); BT(300); R(); PP(130, 3); P(); R(); P(); LD(250); down(); pause(); up(); keep();

  R(); R(); PP(130, 2); P(); R(); P(); L(); PS(); pause(); kom();

}

void yellow_4() {

  keep(); up(); BT(300); R(); PP(130, 2); P(); R(); P(); L(); P(); LD(70); down(); pause(); up(); keep(); 

  R(); R(); PP(130, 2); P(); R(); P(); L(); PS(); pause(); kom();

}

void red_1() {

  keep(); up(); BT(300); R(); P(); R(); P(); ST(300); LD(70); down(); pause(); up(); keep(); 

  BT(300); R(); R(); PP(); L(); PS(); pause(); kom();

}

void red_2() {

  keep(); up(); BT(300); R(); PP(); P(); R(); P(); ST(300); RD(150); down(); pause(); up(); keep();

  BT(300); R(); R(); P(); L(); PP(); PS(); pause(); kom();

}

void red_3() {

  keep(); up(); BT(300); R(); P(); L(); PP(130, 2); P(); R(); PP(130, 2); P(); R(); LD(70); down(); pause(); up(); keep();

  R(); P(); L(); PP(130, 2); P(); R(); PP(130, 2); PS(); pause(); kom();

}

void red_4() {

  keep(); up(); BT(300); R(); P(); L(); PP(130, 2); P(); R(); PP(); P(); R(); P(); L(); P(); LD(70); down(); pause(); up(); keep();

  L(); L(); P(); L(); PP(); P(); R(); PP(130, 2); PS(); pause(); kom();

}

void start() {

  while(1) {

    if(count_color_red + count_color_yellow == 8) {

      backtostart();

      break;

    }

    else {

      waitfor(5);

      color();

    }

  }

}

void backtostart() {

  up();

  BT(100); R(); PP(120, 2); P(120); L(); P(); R(); P(); ST(100);

}