sketch_sep11c

#include <Servo.h>

#define S0 4
#define S1 5
#define S2 6
#define S3 7
#define sensorOut 8

Servo servo_red;
Servo servo_green;
Servo servo_blue;

int frequency = 0;
int pos_red = 0;
int pos_green = 0;
int pos_blue = 0;

int led_red = 1;
int led_green = 2;
int led_blue = 3;

void setup() {
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);

  pinMode(led_red, OUTPUT);
  pinMode(led_green, OUTPUT);
  pinMode(led_blue, OUTPUT);

  servo_red.attach(9);
  servo_green.attach(10);
  servo_blue.attach(11);

  servo_red.write(0);
  servo_green.write(0);
  servo_blue.write(0);

  // Setting frequency-scaling to 20%
  digitalWrite(S0,HIGH);
  digitalWrite(S1,LOW);

  Serial.begin(9600);
}
void loop() {

  digitalWrite(led_red, LOW);
  digitalWrite(led_green, LOW);
  digitalWrite(led_blue, LOW);

  // Setting red filtered photodiodes to be read
  digitalWrite(S2,LOW);
  digitalWrite(S3,LOW);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  frequency = map(frequency, 25,72,255,0);
  // Printing the value on the serial monitor
  Serial.print("R= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  if(frequency >= 100){

    digitalWrite(led_red, HIGH);

    Serial.print("Red frequency < 100:");
    Serial.println(frequency);
    for (pos_red = 0; pos_red <= 180; pos_red += 1) { // goes from 0 degrees to 180 degrees
      // in steps of 1 degree
      servo_red.write(pos_red);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
    for (pos_red = 180; pos_red >= 0; pos_red -= 1) { // goes from 180 degrees to 0 degrees
      servo_red.write(pos_red);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
  }
  delay(100);
  // Setting Green filtered photodiodes to be read
  digitalWrite(S2,HIGH);
  digitalWrite(S3,HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  frequency = map(frequency, 30,90,255,0);
  // Printing the value on the serial monitor
  Serial.print("G= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  if(frequency >= 100){

    digitalWrite(led_green, HIGH);

    Serial.print("Green frequency < 100:");
    Serial.println(frequency);
    for (pos_green = 0; pos_green <= 180; pos_green += 1) { // goes from 0 degrees to 180 degrees
      // in steps of 1 degree
      servo_green.write(pos_green);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
    for (pos_green = 180; pos_green >= 0; pos_green -= 1) { // goes from 180 degrees to 0 degrees
      servo_green.write(pos_green);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
  }
  delay(100);
  // Setting Blue filtered photodiodes to be read
  digitalWrite(S2,LOW);
  digitalWrite(S3,HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  frequency = map(frequency, 25,70,255,0);
  // Printing the value on the serial monitor
  Serial.print("B= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.println("  ");
  if(frequency >= 100){

    digitalWrite(led_blue, HIGH);

    Serial.print("Blue frequency < 100:");
    Serial.println(frequency);
    for (pos_blue = 0; pos_blue <= 180; pos_blue += 1) { // goes from 0 degrees to 180 degrees
      // in steps of 1 degree
      servo_blue.write(pos_blue);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
    for (pos_blue = 180; pos_blue >= 0; pos_blue -= 1) { // goes from 180 degrees to 0 degrees
      servo_blue.write(pos_blue);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
  }
  delay(100);
}