Color Marble Sorter

Inspired by a $2.00 color detector by Gonzik on LetsMakeRobots my daughter created a color marble sorter for her elementary school science project.

 

#include  <Servo2>
 
Servo armServo; 
Servo gateServo; 
 
int pos = 0; 
int time = 300;
int redTest = 0;
int blueTest = 0;
int greenTest = 0;

#define BLUELIGHT   4
#define GREENLIGHT  3
#define REDLIGHT    2

#define ON 0
#define OFF 1

#define RedTestOrangeLow  30
#define RedTestOrangeHigh 90
 
#define OrangeArmPosition 0
#define BlackArmPosition 90
#define YellowArmPosition 180

#define OPEN 180 // LET GO (TO THE LEFT)
#define CLOSE 90 // PUSH IN
 
void setup() 
{ 
  Serial.begin(115200); 
  
  armServo.attach(9); 
  gateServo.attach(10); 
  
  pinMode(BLUELIGHT, HIGH);
  pinMode(GREENLIGHT, HIGH);
  pinMode(REDLIGHT, HIGH);
  
} 
 
boolean empty = true; 
 
void loop() 
{ 

  digitalWrite(REDLIGHT, ON);
  delay(time);
  redTest = analogRead(0);
  digitalWrite(REDLIGHT, OFF);

  digitalWrite(GREENLIGHT, ON);
  delay(time);
  greenTest = analogRead(0);
  digitalWrite(GREENLIGHT, OFF);

  digitalWrite(BLUELIGHT, ON);
  delay(time);
  blueTest = analogRead(0);

  digitalWrite(BLUELIGHT, OFF);

  Serial.print("R ");
  Serial.print(redTest);
  Serial.print(" G ");
  Serial.print(greenTest);
  Serial.print(" B ");
  Serial.print(blueTest);
  Serial.print("\n");

  if (greenTest < 30) // black
  {
    armServo.write (BlackArmPosition);
    Serial.print("black");
    empty = false;
  } else if ((redTest < 108) && (redTest > 80)) // orange
  {
    armServo.write (OrangeArmPosition);
    Serial.print("orange");
    empty = false;
//  } else if (redTest > 108) {
//    armServo.write (YellowArmPosition);
//    Serial.print("yellow");
//    empty = false;
  } else {
    Serial.print("empty");
    empty = true;
  }    

  if (empty == false)
  {  
    gateServo.write(OPEN);
    delay(2000);
    gateServo.write(CLOSE);
  }

}