//BallSorter -- Originally built by Mr. McCluskey 
//   Can be used by students to build their projects
//   Provided they understand the code they are modifying
//
//   ** Your ownership/partnership information should go here. **
//
//
// *** Additional Libraries to extend the "c" language
#include   // requred by ColorSensor (communication protocol)
#include   // Adds servo commands
#include "Adafruit_TCS34725.h" // Color Sensor Library
/* *** Wiring 
 
   ** Color sensor **
   Connect SCL    to analog 5   -- Blue Wire 
   Connect SDA    to analog 4   -- Orange Wire
   Connect VIN    to 5V DC      -- Red Wire
   Connect GROUND to common ground  --  Green Wire
   Connect LED    to Digital 3  -- Yellow Wire (allows LED on sensor to be turned on and off)

   ** Routing servo **
   Connect Red to 5V
   Connect Red to GND 
   Connect Yellow to 7 (Signal)

    ** Escapement servo - allows only one ball at a time**
   Connect Red to 5V
   Connect Red to GND 
   Connect Yellow to 9 (Signal)
*/

// ***  Pin Assignemnts ***

int LED_ControlLine= 3;  //Wiring to Control LED

// ***  Create control objects ***
// Initialise with default values (int time = 2.4ms, gain = 1x)Not used
// Adafruit_TCS34725 tcs = Adafruit_TCS34725();   

/* Initialise with specific int time and gain values */
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_700MS, TCS34725_GAIN_1X);


Servo EscapeServo;  // create servo object to control Escapement servo
Servo RouterServo;  // create servo object to control Router servo

// *** Global cariables **
int pos = 0;    // variable to store the servo current position
int Red = 0;    // Boolean to store detected ball color
 uint16_t r, g, b, c, colorTemp, lux;  // Output vars from Color sensor

// *** Constants ***
int EscapeCenterPos = 130;  
int EscapeReleasePos = 160;  // +30 typical from Center
int EscapePickupPos = 100;  // - 30 typ[ical from center

int RouterCenterPos = 89;
int RouterRedPos = 135;
int RouterBluePos = 45;


void setup() {
   Serial.begin(9600);
   delay(20);

   Serial.println("Setup - Begin");
   EscapeServo.attach(9);  // attaches the servo on pin 9 to the servo object
   RouterServo.attach(7);  // attaches the servo on pin 9 to the servo object.
   Serial.println("Setup - Servos Attached");

   pinMode(LED_ControlLine, OUTPUT);  // Set Up Control Line
   digitalWrite(LED_ControlLine,LOW); // Turn off 

   // Check Color Sensor
   Serial.println("Setup - Checking Color Sensor");
   if (tcs.begin()) 
      { Serial.println("Found sensor");}
      else 
      { Serial.println("No TCS34725 found ... check your connections");
         while (1);  // Waits forever if sensor not found
      }  
    EscapeServo.write(EscapeCenterPos);
    Serial.println("Setup - Escape Servo Centered ");
    RouterServo.write(RouterCenterPos);
    Serial.println("Setup - Router Servo Centered ");
    Serial.println("Setup - End");
}

void Route() {
   Serial.println("Route - Begin");
   if (Red) 
      {
      // Release Ball to REd Side
      Serial.println("Route - Routing to Red Side");
      for (pos = RouterCenterPos; pos <= RouterRedPos; pos += 1) 
          { 
             Serial.print(pos);
             Serial.print(",");
             RouterServo.write(pos); 
             delay(15); 
          }  // end for
        Serial.println("");  
        delay(200); // Settle time  
      // Re-Center
      Serial.println("Route - Recentering from red side");
      for (pos = RouterRedPos; pos >= RouterCenterPos; pos -= 1) 
          {
             Serial.print(pos);
             Serial.print(",");
             RouterServo.write(pos);         
             delay(15);                     
           }
         Serial.println("");  
      } // end if(Red)
  else
     {
     // Release Ball to Blue Side
     Serial.println("Route - Routing to Blue Side");
     for (pos = RouterCenterPos; pos >= RouterBluePos; pos -= 1) 
         { 
             Serial.print(pos);
             Serial.print(",");
             RouterServo.write(pos); 
             delay(15); 
          }//  end for
     Serial.println("");       
     delay(200); // Settle time
     // Re-Center
     Serial.println("Route - Recentering from Blue side");
     for (pos = RouterBluePos; pos <= RouterCenterPos; pos += 1) 
         {
             Serial.print(pos);
             Serial.print(",");
             RouterServo.write(pos);         
             delay(15);                     
         }  // end for
     Serial.println("");
     delay(200); // Settle time
     }  // End-else ( Red/BLue)
    Serial .println("Route - End");
} // end-function

void Escape() {
   Serial.println("Escape - Begin");
   Serial.println("Escape - Release Ball");
   // Release Ball
   for (pos = EscapeCenterPos; pos <= EscapeReleasePos; pos += 1) 
      { 
          EscapeServo.write(pos);          // tell servo to go to position in variable 'pos'
          delay(15);                       // waits  for the servo to reach the position
      }  
   // Pick Up New Ball
  Serial.println("Escape - Pick-up new Ball");
  for (pos = EscapeReleasePos; pos >= EscapePickupPos; pos -= 1) 
      { 
          EscapeServo.write(pos);         // tell servo to go to position in variable 'pos'
         delay(15);                       // waits 1for the servo to reach the position
      }
  // Recenter
  Serial.println("Escape - Re-center");
  for (pos = EscapePickupPos; pos <= EscapeCenterPos; pos += 1) 
      { 
         EscapeServo.write(pos);              // tell servo to go to position in variable 'pos'
        delay(15);   
      }
  Serial.println("Escape - End");
}  // end function


void ReadColor()
{
   Serial.println("ReadColor - Begin");
   digitalWrite(LED_ControlLine,HIGH); // Turn ON LED 
   Serial.println("ReadColor - White LED On"); 
   delay(50);  // allow LED to stabilize

   tcs.getRawData(&r, &g, &b, &c);
   colorTemp = tcs.calculateColorTemperature(r, g, b);
   lux = tcs.calculateLux(r, g, b);
  
   if (true)  // set to false to reduce information on logfile
      {
          Serial.print("Color Temp: "); Serial.print(colorTemp, DEC); Serial.print(" K - ");
          Serial.print("Lux: "); Serial.print(lux, DEC); Serial.print(" - ");
          Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
          Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
          Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
          Serial.print("C: "); Serial.print(c, DEC); Serial.print(" ");
          Serial.println(" ");
      }
  digitalWrite(LED_ControlLine,LOW); // Turn OFF LED 
  Serial.println("ReadColor - White LED Off");
  // Set variable Red to True or false
  if ( r > b) { Red=true; Serial.println("ReadColor - Red=true ");}
  else {Red=false;Serial.println("ReadColor - Red=false");};
  Serial.println("ReadColor - End");
  }// end function ReadColor()

void loop() // Main Loop
   {   
    Escape();     // Feed a single ball into chute
    ReadColor();  // Determine the color of the ball
    Route();      // Move the ball to the approariate Bin
 }