ATX2 Multi Function Demo (I-BOT)

#include <ipst.h>  // Include library for IPST Board
int REF = 100;  // Reference for Fire detected
void setup()
{
  glcdMode(3);  // Set direction display GLCD
  //glcdFillScreen(GLCD_GREEN);
}
void readCompass()
{
  glcdClear();
  setTextColor(GLCD_WHITE);
  setTextSize(1);
  glcd(12,0,"<SW1> Exit menu");

  setTextSize(2);
  setTextColor(GLCD_GREEN);
  while(1)
  {
    glcd(3,0,"compass = %d      ",compass_read());  // Read and display angle
    if(sw1())
    {
      glcdClear();
      sound(20,1500,100);
      break;
    }
  }
}
void readLight()
{
  glcdClear();
  setTextColor(GLCD_WHITE);
  setTextSize(1);
  glcd(12,0,"<SW1> Exit menu");
  setTextSize(2);
  while(1)
  {
    int left = 1023-analog(1);
    int right = 1023-analog(2);
    setTextColor(GLCD_BLUE);
    glcd(0,0,"Left    Right");
    setTextColor(GLCD_WHITE);
    glcd(1,0,"%d   ",left);
    glcd(1,8,"%d   ",right);
    if(sw1())
    {
      glcdClear();
      sound(20,1500,100);
      break;
    }
  }
}
void robotFindLight()
{
  glcdClear();
  setTextSize(2);
  setTextColor(GLCD_RED);
  glcd(2,0,"    Robot");
  glcd(3,0," find light.");
  setTextColor(GLCD_WHITE);
  setTextSize(1);
  glcd(12,0,"<SW1> Exit menu");
  while(1)
  {
    int left = 1023-analog(1);  // Get Analog form ZX-Photo left side
  int right = 1023-analog(2);  // Get Analog form ZX-Photo right side
  if(left>REF && right>REF)  // Robot detects fire?
  {
    fd(40);
//    ao();  // Robot stop
//    out(13,1);  // Rotor motor on
//    delay(3000);  // Delay 3 second
//    int l = 1023-analog(1);  // Get Analog form ZX-Photo left side
//    int r = 1023-analog(2);  // Get Analog form ZX-Photo right side
//    if(l<REF && r<REF)  // Recheck Fire off?
//    {
//      out(13,0);  // Rotor motor off
//      bk(40); delay(400);    // Robot backward few time
//      sr(40); delay(600);    // Robot spin right a few time
//      ao();  // Ensure Robot stop
//      glcdMode(1);
//      setTextSize(2);
//      glcdClear();
//      glcd(5,0,"  completed!");
//      while(1);  // Break program
//    }
  }
  else if(left<REF && right>REF)  // Fire were right?
  {
    sr(40); delay(20);    // Robot spin right a few time
    fd(40); delay(20);    // Robot forward a few time
  }
  else if(left>REF && right<REF)  // Fire were left?
  {
    sl(40); delay(20);    // Robot spin left a few time
    fd(40); delay(20);    // Robot forward a few time
  }
  else  // Fire not found?
  {
    ao();
//    if(left>right)   // Fire were left over?
//    {
//      sl(40); delay(20);  // Robot spin left a few time
//      fd(40); delay(20);  // Robot forward a few time
//    }
//    else
//    {
//      sr(40); delay(20);  // Robot spin right a few time
//      fd(40); delay(20);  // Robot forward a few time
//    }
  }
    if(sw1())
    {
      glcdClear();
      sound(20,1500,100);
      break;
    }
  }
}
void objectAvoider()
{
  glcdClear();
  setTextSize(2);
  setTextColor(GLCD_SKY);
  glcd(2,0,"    Robot");
  glcd(3,0,"ObjectAvoider");
  setTextColor(GLCD_WHITE);
  setTextSize(1);
  glcd(12,0,"<SW1> Exit menu");
  while(1)
  {
    fd(40);  // Robot backward
  if(!in(17))  // Left sensor pressed?
  {
    bk(40);  // Robot backward
    delay(800);  // Delay 0.8 second
    sr(40);  // Robot spin right
    delay(500);  // Delay 0.5 second
  }
  if(!in(16))  // Right sensor pressed?
  {
    bk(40);  // Robot backward
    delay(800);  // Delay 0.8 second
    sl(40);  // Robot spin left
    delay(500);  // Delay 0.5 second
  }
    if(sw1())
    {
      glcdClear();
      sound(20,1500,100);
      break;
    }
  }
}
void bleJoystickControl()
{
  Serial1.begin(9600);
  glcdClear();
  setTextSize(2);
  setTextColor(GLCD_BLUE);
  glcd(2,0,"    Robot");
  glcd(3,0," BLE Joystick");
  setTextColor(GLCD_WHITE);
  setTextSize(1);
  glcd(11,0,"<Triangle Button> Wind ON");
  glcd(12,0,"<SW1> Exit menu");
  while(1)
  {
    if(Serial1.available()>0)
  {
    char inbyte = Serial1.read();
    //Serial1.print(inbyte);
    //beep();
    //glcd(5,0,"receive: %c ",inbyte);
    if(inbyte=='a' || inbyte=='A')
    {
      fd(40);
      //delay(50);
    }
    else if(inbyte=='b'|| inbyte=='B')
    {
      sr(40);
      //delay(50);
    }
    else if(inbyte=='c'|| inbyte=='C')
    {
      bk(40);
      //delay(50);
    }
    else if(inbyte=='d' || inbyte=='D')
    {
      sl(40);
      //delay(50);
    }
    else if(inbyte=='e' || inbyte=='E')
    {
      out(13,1);
      //sound(20,1200,100);
      //delay(50);
    }
    else if(inbyte=='F')
    {
      //out(13,1);
      sound(20,500,100);
      //delay(50);
    }
    else if(inbyte=='G')
    {
      //out(13,1);
      sound(20,1000,100);
      //delay(50);
    }
    else if(inbyte=='H')
    {
      //out(13,1);
      sound(20,2000,100);
      //delay(50);
    }
    else if(inbyte==0x00)  // When joystick release key from hold down
    {
      ao();
      out(13,0);
    }
  }
    if(sw1())
    {
      glcdClear();
      sound(20,1500,100);
      break;
    }
  }
}
void loop()
{
 int menu = map(analog(3),0,1023,1,6);

 setTextColor(GLCD_WHITE);
 setTextSize(1);
 glcd(12,0,"<SW1> Select menu");
 if(menu==1)
 {
   setTextSize(2);
   glcd(2,0,"  <Menu:1> ");
   glcd(3,0," Read Compass ");
   if(sw1())
  {
    sound(20,1500,100);
    readCompass();
  }
 }
 else if(menu==2)
 {
   setTextSize(2);
   glcd(2,0,"  <Menu:2> ");
   glcd(3,0," Read Light   ");
   if(sw1())
  {
    sound(20,1500,100);
    readLight();
  }
 }
 else if(menu==3)
 {
   setTextSize(2);
   glcd(2,0,"  <Menu:3> ");
   glcd(3,0," Find Light  ");
   if(sw1())
  {
    sound(20,1500,100);
    robotFindLight();
  }
 }
 else if(menu==4)
 {
   setTextSize(2);
   glcd(2,0,"  <Menu:4> ");
   glcd(3,0,"ObjectAvoider  ");
   if(sw1())
  {
    sound(20,1500,100);
    objectAvoider();
  }
 }
 else if(menu==5 || menu==6)
 {
   setTextSize(2);
   glcd(2,0,"  <Menu:5> ");
   glcd(3,0,"  Joystick    ");
   if(sw1())
  {
    sound(20,1500,100);
    bleJoystickControl();
  }
 }


}