/*   Bachelor task: Autonomous assembly using the UR5e robotic arm
 *   Popis: Color Sensor & Distance sensor code
 *   Autor: Martin Cesnek
 *   Pouzite Arduino: Arduino Mega 2560
 *   Ports: D2 - S0, D3 - S1, D4 - S2, D5 - S3, D6 - OUT, D7 - D0(sensor), A0 - A0(Sensor) 
 */ 
// Nastavenie pinov
#define S0 2
#define S1 3
#define S2 4
#define S3 5
#define sensorOut 6
#define D0 7 
int redfrequency = 0;
int greenfrequency = 0;
int bluefrequency = 0;
int color=0;
int RAW_signal,noise,clear_signal;
// Nastavenie Input/Output
void setup() {
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);
  pinMode(D0,OUTPUT);
  // Nastavenie scale frekvencie na 20 %
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);
  Serial.begin(9600);           // Seriova komunikacia
}
void loop() {
// SNIMAC FARBY
// Nastavenie Cerveneho filtra photodiody k citaniu
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  // Citanie vystupnej frekvencie
  redfrequency = pulseIn(sensorOut, LOW);
  int R = redfrequency;
  delay(250);
  // Nastavenie Zeleneho filtra photodiody k citaniu
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);
  // Citanie vystupnej frekvencie
  greenfrequency = pulseIn(sensorOut, LOW);
  int G = greenfrequency;
  delay(250);
  // Nastavenie Modreho filtra photodiody k citaniu
  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);
  // Citanie vystupnej frekvencie
  bluefrequency = pulseIn(sensorOut, LOW);
  int B = bluefrequency;
  // Printing the value on the serial monitor
  delay(250);

  // Identifikacia farby podla nastavenych podmienok
  if(R<B & B<G ){
    color = 1; // Red
  }
  if(G<R & B<R){
    color = 2; // Green
  }
  if (B<R & B<G & B<110){
    color = 3; // Blue;
  }
  if(R<G & G<B){
    color = 4; // Yellow
  }
   // SNIMAC VZDIALENOSTI
  digitalWrite(D0,HIGH);    // Zapnutie vysielacej LED
  delayMicroseconds(500);  
  RAW_signal=analogRead(A0);        // nacitanie dat z analog pinu(A0)

// Vypis dat do serial portu
   Serial.print(color);                //printing color on serial monitor
   Serial.print(" "); 
   Serial.print(RAW_signal);         // printing denoised signal on serial monitor
   Serial.print('\n');
}
