#include <Servo.h>
#define motorder 2
#define motorizq 3
Servo servo1;
Servo servo2;
Servo servo3;
// Define color sensor pins
#define S0 4
#define S1 5
#define S2 6
#define S3 7
#define sensorOut 8
// Calibration Values
// *Get these from Calibration Sketch
int redMin = 0; // Red minimum value
int redMax = 0; // Red maximum value
int greenMin = 0; // Green minimum value
int greenMax = 0; // Green maximum value
int blueMin = 0; // Blue minimum value
int blueMax = 0; // Blue maximum value
// Variables for Color Pulse Width Measurements
int redPW = 0;
int greenPW = 0;
int bluePW = 0;
// Variables for final Color values
int redValue;
int greenValue;
int blueValue;
int trigPin = 11; // Disparador (Trigger)
int echoPin = 12; // Eco (Echo)
long duration, cm;
const int DISTANCIA = 15;
void setup(){
pinMode(motorder, OUTPUT);
pinMode(motorizq, OUTPUT);
servo1.attach(4);
servo2.attach(5);
servo3.attach(6);
// Set S0 - S3 as outputs
pinMode(S0, OUTPUT);
pinMode(S1, OUTPUT);
pinMode(S2, OUTPUT);
pinMode(S3, OUTPUT);
// Set Sensor output as input
pinMode(sensorOut, INPUT);
// Set Frequency scaling to 20%
digitalWrite(S0,HIGH);
digitalWrite(S1,LOW);
// Setup Serial Monitor
Serial.begin(9600);
//Define entradas y salidas
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}
void loop(){
servo1.write(0);
servo2.write(0);
servo3.write(0);
// El sensor se activa con un pulso ALTO de 10 microsegundos o más
// Emite un breve pulso BAJO antes para asegurar un pulso ALTO limpio:
digitalWrite(trigPin, LOW);
delayMicroseconds(5);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Lee la señal del sensor: un pulso ALTO cuya duración
// es el tiempo (en microsegundos) desde el envío del
// ping hasta la recepción de su eco de un objeto.
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
// Convierte el tiempo en distancia
cm = (duration/2) / 29.1; // Divide entre 29.1 o multiplica por 0.0343
Serial.print(cm);
Serial.print("cm");
Serial.println();
delay(250);
if (cm < DISTANCIA){
Serial.print(">OBJETO DETECTADO A ");
Serial.print(cm);
Serial.println("<");
digitalWrite(motorder, HIGH);
digitalWrite(motorizq, LOW);
delay(1500);
digitalWrite(motorder, LOW);
digitalWrite(motorizq, LOW);
}
// Read Red value
redPW = getRedPW();
// Map to value from 0-255
redValue = map(redPW, redMin,redMax,255,0);
// Delay to stabilize sensor
delay(200);
// Read Green value
greenPW = getGreenPW();
// Map to value from 0-255
greenValue = map(greenPW, greenMin,greenMax,255,0);
// Delay to stabilize sensor
delay(200);
// Read Blue value
bluePW = getBluePW();
// Map to value from 0-255
blueValue = map(bluePW, blueMin,blueMax,255,0);
// Delay to stabilize sensor
delay(200);
// Print output to Serial Monitor
Serial.print("Red = ");
Serial.print(redValue);
Serial.print(" - Green = ");
Serial.print(greenValue);
Serial.print(" - Blue = ");
Serial.println(blueValue);
}
// Function to read Red Pulse Widths
int getRedPW() {
// Set sensor to read Red only
digitalWrite(S2,LOW);
digitalWrite(S3,LOW);
// Define integer to represent Pulse Width
int PW;
// Read the output Pulse Width
PW = pulseIn(sensorOut, LOW);
// Return the value
return PW;
}
// Function to read Green Pulse Widths
int getGreenPW() {
// Set sensor to read Green only
digitalWrite(S2,HIGH);
digitalWrite(S3,HIGH);
// Define integer to represent Pulse Width
int PW;
// Read the output Pulse Width
PW = pulseIn(sensorOut, LOW);
// Return the value
return PW;
}
// Function to read Blue Pulse Widths
int getBluePW() {
// Set sensor to read Blue only
digitalWrite(S2,LOW);
digitalWrite(S3,HIGH);
// Define integer to represent Pulse Width
int PW;
// Read the output Pulse Width
PW = pulseIn(sensorOut, LOW);
// Return the value
return PW;
}
if (redValue >240 && redValue <250 && greenValue >240 && greenValue <250 && blueValue > 240 && blueValue < 250){
digitalWrite(motorder, HIGH);
digitalWrite(motorizq, LOW);
servo1.write(45);
delay(3000);
digitalWrite(motorder, LOW);
digitalWrite(motorizq, LOW);
servo1.write(0);
}
if (redValue >250 && redValue <260 && greenValue >250 && greenValue <260 && blueValue > 250 && blueValue < 260){
digitalWrite(motorder, HIGH);
digitalWrite(motorizq, LOW);
servo2.write(50);
delay(5000);
digitalWrite(motorder, LOW);
digitalWrite(motorizq, LOW);
servo2.write(0);
}
if (redValue >260 && redValue <270 && greenValue >260 && greenValue <270 && blueValue > 260 && blueValue < 270){
digitalWrite(motorder, HIGH);
digitalWrite(motorizq, LOW);
servo3.write(40);
delay(7000);
digitalWrite(motorder, LOW);
digitalWrite(motorizq, LOW);
servo3.write(0);
}

