 #include "Adafruit_TCS34725.h"  // Color Sensor
 
 // Initialize the display with the numbers of the interface pins
-LiquidCrystal lcd(pinLcdRS, pinLcdEnable, pinLcdD4, pinLcdD5, pinLcdD6, pinLcdD7);
+LiquidCrystal lcd(PIN_LCD_RS, PIN_LCD_ENABLE, PIN_LCD_D4, PIN_LCD_D5, PIN_LCD_D6, PIN_LCD_D7);
 // Create the object for the top continuous rotation servo
 Servo servoTop;
 // Create the object for the bottom standard servo
 Servo servoBtm;
 // Create the color sensor object
-Adafruit_TCS34725 tcs = Adafruit_TCS34725();
-
-// Variables
-int skittleCount = 0; // The number of skittles sorted
-unsigned long lastSkittleTime = 0; // Time when the last skittle was being sorted
-int topServoSpeed = 0; // The speed in which the top servo moves
-boolean isColorBeingMeasured = false; // Is the color sensor measuring the Skittle right now
-
+Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_1X);
+
+// The number of skittles sorted
+int skittleCount = 0;
+// Time when the last skittle was being sorted
+unsigned long lastSkittleTime = 0;
+// The speed in which the top servo moves
+int topServoSpeed = 0;
+// Is the color sensor measuring the Skittle right now
+boolean isColorBeingMeasured = false;
 
 void setup() {
   // Set up the serial for debugging purposes

// The circuit:
// * Top Servo
//    Digital pin 13
const int pinTopServo = 13;
// * Bottom Servo (Arm)
//    Digital pin 10
const int pinBtmServo = 10;
// * Color Sensor
//    SDA pin to analog pin 4
//    SCL pin to analog pin 5

//    Color View Blue pin to diginal pin 6
// * LCD
//    RS pin to digital pin 12
#define PIN_LCD_RS 12
//    Enable pin to digital pin 11
#define PIN_LCD_ENABLE 11
//    LCD D4 pin to digital pin 5
#define PIN_LCD_D4 5
//    LCD D5 pin to digital pin 4
#define PIN_LCD_D5 4
//    LCD D6 pin to digital pin 3
#define PIN_LCD_D6 3
//    LCD D7 pin to digital pin 2
#define PIN_LCD_D7 2
//    LCD R/W pin to ground


// Constants
#define C_ATIME TCS34725_INTEGRATIONTIME_50MS // Integration time 50ms, 20 cycles, Max Count: 20480
#define C_CYCLES (256-C_ATIME) // The cycles according to the integration time

const boolean topServoTimeoutEnable = true; // Enable the timeout for the top servo
const int topServoTimeout = 6000; // Timeout to determine if the top servo is stuck


// Include libraries
#include <LiquidCrystal.h>      // LCD display
