Automatic Shiny Hunnter

#include <Servo.h>
#include "Adafruit_TCS34725.h"

#define shinyLED 12
#define switchDetector 13

const int NUM_MEASUREMENTS = 6;

int numLoops = 0;

Servo servoAX, servoBY, servoUL, servoDR, servoStart;
bool useAX = false;
bool useBY = false;
bool useUL = false;
bool useDR = false;
bool useStart = false;

bool DEBUG = true;

int posAX[] = {90, 36, 165}; //A <-> X
int posBY[] = {90, 30, 155};
int posUL[] = {90, 70, 115};
int posDR[] = {90, 65, 105};
int posStart[] = {90, 55};

int delayDefault = 250;
int delayScreenTransition = 2500;

#define commonAnode true
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);

void resetServos() {
  if (useAX) {servoAX.write(posAX[0]);}
  if (useBY) {servoBY.write(posBY[0]);}
  if (useUL) {servoUL.write(posUL[0]);}
  if (useDR) {servoDR.write(posDR[0]);}
  if (useStart) {servoStart.write(posStart[0]);}

  delay(delayDefault);
}

void setup() {
  if (DEBUG) {
    Serial.begin(9600);
  }

  pinMode(switchDetector, INPUT);
  pinMode(shinyLED, OUTPUT);
  digitalWrite(shinyLED, LOW);

  if (tcs.begin()) {
    Serial.println("Found sensor");
  } else {
    Serial.println("No TCS34725 found ... check your connections");
    while (1); // halt!
  }

//  if (useAX) {servoAX.attach(5);}
//  if (useBY) {servoBY.attach(6);}
//  if (useUL) {servoUL.attach(8);}
//  if (useDR) {servoDR.attach(7);}
//  if (useStart) {servoStart.attach(9);}
  servoAX.attach(5);
  servoBY.attach(6);
  servoUL.attach(8);
  servoDR.attach(7);
  servoStart.attach(9);
  resetServos();

  //switchToTester();

  waitForStart();

  firstLoop();
}

void firstLoop() {
  loop();
  detachUnused();
}

void loop() {
  numLoops++;
  if (DEBUG) {
    Serial.println(numLoops + " Loops Completed");
  }
  hyperspace_default();
}

//=========================HUNTING LOOPS=========================//
void horde_default() { //repeatedly running horde encounters, no ability on my end
  x();
  a();
  delay(1600);
  down();
  a();
  down();
  a();
  a();
  delay(8000);
  float startR, startG, startB;
  measureLight(&startR, &startG, &startB);
  delay(10750); //8500 for less laggy, bit of an issue, 9100 for laggier like vulpix
  float endR, endG, endB;
  measureLight(&endR, &endG, &endB);
  if (!detectTransition(startR, startG, startB, endR, endG, endB)) {
    digitalWrite(shinyLED, HIGH);
    disengageServos();
    while(1); //shiny omg!!!
  }
  left();
  right();
  a();
  delay(7500); //6500 for lower levels, implement run away pokemon
}

void latias_default() { //latias run/re-encounter, my pokemon has no ability
  a();
  delay(1500);
  a();
  delay(500);
  float startR, startG, startB;
  measureLight(&startR, &startG, &startB);
  delay(8500);
  float endR, endG, endB;
  measureLight(&endR, &endG, &endB);
  if (!detectTransition(startR, startG, startB, endR, endG, endB)) {
    digitalWrite(shinyLED, HIGH);
    disengageServos();
    while(1); //shiny omg!!!
  }
  left();
  right();
  a();
  delay(6500);
  a();
}

void latios_default() { //latias run/re-encounter, my pokemon has no ability
  a();
  delay(1500);
  a();
  delay(500);
  float startR, startG, startB;
  measureLight(&startR, &startG, &startB);
  delay(9000);
  float endR, endG, endB;
  measureLight(&endR, &endG, &endB);
  if (!detectTransition(startR, startG, startB, endR, endG, endB)) {
    digitalWrite(shinyLED, HIGH);
    disengageServos();
    while(1); //shiny omg!!!
  }
  left();
  right();
  a();
  delay(7000);
  a();
}

//approx 13.84 hrs per shiny
void hyperspace_default() { //hyperspace hole SR, no ability on my end or on enemy end
  a();
  delay(2250);
  a();
  delay(4250);
  a();
  delay(3250);
  a();
  delay(750);
  a();
  delay(50);
  a();
  delay(50);
  a();
  delay(2000);
  float startR, startG, startB;
  measureLight(&startR, &startG, &startB);
  delay(8300);
  float endR, endG, endB;
  measureLight(&endR, &endG, &endB);
  if (!detectTransition(startR, startG, startB, endR, endG, endB)) {
    digitalWrite(shinyLED, HIGH);
    disengageServos();
    while(1); //shiny omg!!!
  }
  softReset();
  delay(11000);
}

void hyperspace_ability() { //reshiram hyperspace hole SR, no ability on my end
  a();
  delay(2250);
  a();
  delay(4250);
  a();
  delay(3250);
  a();
  delay(750);
  a();
  delay(500);
  a();
  delay(500);
  a();
  delay(2000);
  float startR, startG, startB;
  measureLight(&startR, &startG, &startB);
  delay(11500);
  float endR, endG, endB;
  measureLight(&endR, &endG, &endB);
  if (!detectTransition(startR, startG, startB, endR, endG, endB)) {
    digitalWrite(shinyLED, HIGH);
    disengageServos();
    while(1); //shiny omg!!!
  }
  softReset();
  delay(11000);
}

//=========================UTILITY=========================//
void waitForStart() {
  while (digitalRead(switchDetector) != HIGH) {}
}

void switchToTester() {
  waitForStart();
  a();
  delay(2250);
  a();
  delay(4250);
  a();
  delay(4250);
  x();
  delay(1500);
  a();
  delay(1500);
  a();
  up();
  up();
  a();
  right();
  a();
  b();
  delay(1500);
  up();
  a();
  delay(2000);
  a();
  delay(8000);
  softReset();
}

bool detectTransition(float r, float g, float b, float _r, float _g, float _b) {
  Serial.println();
  float diffR = abs(r-_r)/((r+_r)/2);
  float diffG = abs(g-_g)/((g+_g)/2);
  float diffB = abs(b-_b)/((b+_b)/2);
  int total = 0;
  if (diffR  >= .1) {total++;}
  if (diffG  >= .1) {total++;}
  if (diffB  >= .1) {total++;}
  return total > 1;
}

float measureLight(float *totalR, float *totalG, float *totalB) {
  *totalR = 0;
  *totalG = 0;
  *totalB = 0;
  for (int i = 0; i < NUM_MEASUREMENTS; i++) {
    uint16_t clr, red, green, blue;
    tcs.setInterrupt(false);      // turn on LED
    delay(60);  // takes 50ms to read
    tcs.getRawData(&red, &green, &blue, &clr);
    tcs.setInterrupt(true);  // turn off LED
    float r, g, b;
    uint32_t sum = clr;
    r = red; r /= sum;
    g = green; g /= sum;
    b = blue; b /= sum;
    r *= 256; g *= 256; b *= 256;

    *totalR += r;
    *totalG += g;
    *totalB += b;

    if (DEBUG) {
      Serial.print("R: "); Serial.print(r); Serial.print(" G: ");Serial.print(g); Serial.print(" B: "); Serial.print(b); Serial.println();
    }
  }
  *totalR /= NUM_MEASUREMENTS;
  *totalG /= NUM_MEASUREMENTS;
  *totalB /= NUM_MEASUREMENTS;

  if (*totalR != *totalR) {*totalR = 0;}
  if (*totalG != *totalG) {*totalG = 0;}
  if (*totalB != *totalB) {*totalB = 0;}

  if (DEBUG) {
      Serial.print("AvgR: "); Serial.print(*totalR); Serial.print(" AvgG: ");Serial.print(*totalG); Serial.print(" AvgB: "); Serial.print(*totalB); Serial.println();
  }
}

void detachUnused() {
  resetServos();
  if (!useAX) {servoAX.detach();}
  if (!useBY) {servoBY.detach();}
  if (!useUL) {servoUL.detach();}
  if (!useDR) {servoDR.detach();}
  if (!useStart) {servoStart.detach();}
}

void softReset() {
  useStart = true;
  servoStart.write(posStart[1]);
  delay(delayDefault);
  servoStart.write(posStart[0]);
  delay(delayDefault);
}

void disengageServos() {
  resetServos();
//  if (useAX) {servoAX.detach();}
//  if (useBY) {servoBY.detach();}
//  if (useUL) {servoUL.detach();}
//  if (useDR) {servoDR.detach();}
//  if (useStart) {servoStart.detach();}
  servoAX.detach();
  servoBY.detach();
  servoUL.detach();
  servoDR.detach();
  servoStart.detach();
}

//=========================ABXY=========================//
void b() {
  useBY = true;
  servoBY.write(posBY[2]);
  delay(delayDefault);
  servoBY.write(posBY[0]);
  delay(delayDefault);
}

void y() {
  useBY = true;
  servoBY.write(posBY[1]);
  delay(delayDefault);
  servoBY.write(posBY[0]);
  delay(delayDefault);
}

void x() {
  useAX = true;
  servoAX.write(posAX[2]);
  delay(delayDefault);
  servoAX.write(posAX[0]);
  delay(delayDefault);
}

void a() {
  useAX = true;
  servoAX.write(posAX[1]);
  delay(delayDefault);
  servoAX.write(posAX[0]);
  delay(delayDefault);
}

//=========================D PAD=========================//
void left() {
  useUL = true;
  servoUL.write(posUL[2]);
  delay(delayDefault);
  servoUL.write(posUL[0]);
  delay(delayDefault);
}

void down() {
  useDR = true;
  servoDR.write(posDR[1]);
  delay(delayDefault);
  servoDR.write(posDR[0]);
  delay(delayDefault);
}

void right() {
  useDR = true;
  servoDR.write(posDR[2]);
  delay(delayDefault);
  servoDR.write(posDR[0]);
  delay(delayDefault);
}

void up() {
  useUL = true;
  servoUL.write(posUL[1]);
  delay(delayDefault);
  servoUL.write(posUL[0]);
  delay(delayDefault);
}