IPST-SE STEM FESTIVAL 2016 Second Round

#include <ipst.h>   // IPST-SE Board
word L, R, A, B, C,BAR, i;
word refL=350,refR=450;
int N=50,E=122,S=168,W=300;
void setup() {
   rdSensor();
  OK();
  barCode();
  fd(30);delay(500);
  if(A){ R_Fire();}
  else{  L_Fire();}
  trackUntilBall();ao();L180();ao();FFF();FFF();FFF();trackTime(2000);bk(30);delay(500);R180();
}

void loop() {
if(BAR==2){Ball1();Ball2();}
else if(BAR==3){Ball1();Ball3();}
else if(BAR==4){Ball1();Ball4();}
else if(BAR==5){Ball2();Ball3();}
else if(BAR==6){Ball2();Ball4();}
else if(BAR==7){Ball3();Ball4();}

}
void Ball3(){
FFF();LLL();trackUntilBall();L180();LLL();L180();L180();FFF();trackTime(2000);bk(30);delay(500);R180();

}
void Ball1(){
LLL();trackUntilBall();L180();LLL();L180();L180();trackTime(2000);bk(30);delay(500);R180();

}
void Ball2(){
RRR();trackUntilBall();L180();LLL();trackTime(2000);bk(30);delay(500);R180();

}
void Ball4(){
FFF();RRR();trackUntilBall();L180();LLL();FFF();trackTime(2000);bk(30);delay(500);R180();
}

word whiteBrown=500,blackBrown=290;  // 700 //400// 200
void barCode() {
  setTextSize(2);
  setTextColor(GLCD_BLACK);
  setTextBackgroundColor(GLCD_WHITE);
  while (analog(1)>100){fd(20);} ao(); delay(400);
  while (analog(1)<100){fd(20);delay(12);}delay(50); ao(); delay(400);
  while (analog(1)>blackBrown&&analog(1)<whiteBrown){fd(30);} ao(); delay(400);
  if (analog(1)<blackBrown){
    glcd(2,0,"BLACK"); A=1;
    while (analog(1)<blackBrown){fd(30);}delay(50); ao(); delay(400);
  }
  else if (analog(1)>whiteBrown){
    glcd(2, 0, "WHITE"); A = 0;
    while (analog(1)>whiteBrown){fd(30);}delay(50); ao(); delay(400);
  }

  while(analog(1)>blackBrown && analog(1)<whiteBrown){fd(30);}ao(); delay(400);
    if (analog(1)<blackBrown) {
      glcd(3,0,"BLACK"); B=2;
      while(analog(1)<blackBrown){fd(30);}delay(50); ao(); delay(400);
    }
    else if(analog(1)>whiteBrown){
      glcd(3,0,"WHITE"); B=0;
      while(analog(1)>whiteBrown){fd(30);}delay(50);   ao(); delay(400);
    }

  while (analog(1)>blackBrown&&analog(1)<whiteBrown){fd(30);} ao(); delay(400);
  if (analog(1)<blackBrown){
    glcd(4,0,"BLACK");C=4;
    while (analog(1)<blackBrown){fd(30);}delay(50);ao(); delay(400);
  }
  else if (analog(1)>whiteBrown) {
    glcd(4,0,"WHITE"); C=0;
    while (analog(1)>whiteBrown) {fd(30);}delay(50);ao();delay(400);
  }

  BAR = A + B + C;
  glcdFillScreen(GLCD_BLUE);
  setTextSize(10);
  setTextBackgroundColor(GLCD_BLUE);
  setTextColor(GLCD_YELLOW);
  glcd(0, 1, "%d ", A + B + C);

}

void fire(int t) {
  out(12, 1);
  delay(t);
  out(12, 0);
}


void L_Fire(){
FFF();FFF();LLL();trackTime(600);fire(2000);bk(30);delay(50);R180();LLL();
}
void R_Fire(){
FFF();FFF();RRR();trackTime(500);fire(2000);bk(30);delay(50);R180();RRR();

}


void FFF() {
  while (1) {
    L=analog(0); R=analog(1);
    if (L<refL&&R<refR){fd2(30,33);}
    else if(L>refL&&R<refR){sl(40);}
    else if (L<refL&&R>refR){sr(40);}
    else if (L>refL&&R>refR){
      fd(30);delay(200);
      break;
    }
 }
}
void LLL() {
  while (1) {
    L = analog(0);R = analog(1);
    if (L<refL&&R<refR){fd2(30,33);}
    else if (L>refL&&R<refR){sl(40);}
    else if (L<refL&&R>refR){sr(40);}
    else if (L>refL&&R>refR) {
      fd(30); delay(200);
      while (analog(0)<refL){sl(40);}
      while (analog(0)>refL){sl(40);}
      break;
    }
  }
}
void RRR() {
  while(1) {
    L=analog(0);R=analog(1);
    if (L<refL&&R<refR) {fd2(30,33);}
    else if(L>refL&&R<refR) {sl(40);}
    else if (L<refL&&R>refR){sr(40);}
    else if (L>refL&&R>refR){
      fd(30); delay(200);
      while (analog(1)<refR) {sr(40);}
      while (analog(1)>refR) {sr(40);}
      break;
    }
  }
}
void trackTime(long t) {
  long m=millis();
  while (millis()<(m+t)) {
    L=analog(0);R=analog(1);
    if (L<refL&&R<refR) {fd2(30,33);}
    else if(L>refL&&R<refR){sl(40);}
    else if (L<refL&&R>refR){sr(40);}
  }
  ao();
}
void trackUntilWall(){
   while(in(27)){
      L=analog(0);
      R=analog(1);
      if(L<refL&&R<refR){fd2(30,33);}
      else if(L>refL&&R<refR){sl(40);}
      else if(L<refL&&R>refR){sr(40);}
   }
   ao();
}
void trackUntilBall(){
  while(analog(5)<150){
      L=analog(0);R=analog(1);
      if(L<refL&&R<refR){fd2(30,30);}
      else if(L>refL&&R<refR){sl(40);}
      else if(L<refL&&R>refR){sr(40);}
   }ao();
}
void R180(){
    while (analog(1)<refR){sr(30);}
    while (analog(1)>refR){sr(30);}
    ao();
}
void L180(){
    while (analog(0)<refL){sl(30);}
    while (analog(0)>refL){sl(30);}
    ao();
}

void rdSensor() {
  setTextSize(2); glcdMode(1);
  while (sw_1() == 0) {
    L = analog(0); R = analog(1);
    glcd(0, 0, "L=%d   ", L);
    glcd(1, 0, "R=%d   ", R);
    glcd(2, 0, "R=%d   ", analog(5));
    glcd(3, 0, "%d  ", compass_read());
  }
}