รถดั๊มตรวจจับสี

รถดั๊มตรวจจับสี

อุปกรณ์

1.รถดั้มสิบล้อ

2.servo motor

3.color sensor

4.กล่องกระดาษสีแดง สีเขียว สีน้ำเงิน

5.ลูกแก้ว

#include <Servo.h>

#define S0 2

#define S1 3

#define S2 4

#define S3 5

#define Out 6

Servo topServo;

int frequency = 0;

int color=0;

void setup() {

  pinMode(S0, OUTPUT);

  pinMode(S1, OUTPUT);

  pinMode(S2, OUTPUT);

  pinMode(S3, OUTPUT);

  pinMode(Out, INPUT);

  digitalWrite(S0, HIGH);

  digitalWrite(S1, LOW);

  topServo.attach(7);

  Serial.begin(9600);

}

void loop() {

  color = readColor();

  delay(10);  

  switch (color) {

    case 1:

    topServo.write(0);

    delay(5000);

    topServo.write(180);

    }

  color=0;

}

int readColor() {

  digitalWrite(S2, LOW);

  digitalWrite(S3, LOW);

  frequency = pulseIn(Out, LOW);

  int R = frequency;

  Serial.print("R= ");

  Serial.print(frequency);

  Serial.print("  ");

  delay(50);

  digitalWrite(S2, HIGH);

  digitalWrite(S3, HIGH);

  frequency = pulseIn(Out, LOW);

  int G = frequency;

  Serial.print("G= ");

  Serial.print(frequency);

  Serial.print("  ");

  delay(500);

  digitalWrite(S2, LOW);

  digitalWrite(S3, HIGH);

  frequency = pulseIn(Out, LOW);

  int B = frequency;

  Serial.print("B= ");

  Serial.print(frequency);

  Serial.println("  ");

  delay(50);

  if(R<35 & R>25 & G<104 & G>94){

    color = 1; // Red

  }

  return color;

}

หลักการทำงาน

   เมื่อ color sensor ตรวจสีจับสีที่กำหนดไว้ได้ servor motor จะทำงาน