Robot Contest Code [Extends]

#include <ipst.h>

//CLASS-----------------------------------------------------------------------------------------

class LightSensor
{
  public:

  int port;

  int LightLimit;

  LightSensor(int port_,int LightLimit_)
  {
    port = port_;
    LightLimit = LightLimit_;
  };

  int input()
  {
     return analog(port);
  };

  int digital()
  {
    return in(port);
  }

  boolean isWhite()
  {
    return input() > LightLimit;
  }

  boolean isBlack()
  {
    return !isWhite();
  }

};

//VARIABLE---------------------------------------------------------------------------------------

//Motion

  //Normal Moving Speed
  const int normalSpeed = 30;
  const int fastSpeed = 40;
  const int slowSpeed = 25;

  //Rotation Speed
  const int turnSpeed = 20;
  const int turningOffsetTime = 400;
  const int rotationFixSpeed = 100;
  const int rotationFixTime = 75;

  //caning Speed
  const int canHittingSpeed = (fastSpeed * 2) + 10;
  const int canHittingTime = 10;
  const int canBackwardSpeed = slowSpeed;

  //Error Of Balance (relative to left)
  const int balanceError = 0;

  //motor port
  const int motorLeft = 2;
  const int motorRight = 1;

//Sensoring

  //LightSensor
  LightSensor leftSensor = LightSensor(4,500);
  LightSensor midLeftSensor = LightSensor(1,100);
  LightSensor midRightSensor = LightSensor(2,100);
  LightSensor rightSensor = LightSensor(5,500);

//CODE----------------------------------------------------------------------------------

void fw2(int left, int right)
{
  motor(motorLeft, left + balanceError);
  motor(motorRight, right);
}

void bw2(int left, int right)
{
  motor(motorLeft, left + balanceError);
  motor(motorRight, right);
}

void turnR90()
{
  fw2(turnSpeed,0);
  delay(turningOffsetTime);
  while(midLeftSensor.isWhite() || midRightSensor.isWhite());

  /*fw2(rotationFixSpeed,-rotationFixSpeed);
  delay(rotationFixTime);*/
  motor_stop(ALL);
}

void turnL90()
{

  fw2(0,turnSpeed);
  delay(turningOffsetTime);
  while(midLeftSensor.isWhite() || midRightSensor.isWhite());

  /*fw2(rotationFixSpeed,-rotationFixSpeed);
  delay(rotationFixTime);*/
  motor_stop(ALL);
}

int i = 5;

//General Along-Path Motion Controller
void track(int code)
{
  if(midLeftSensor.isBlack() && midRightSensor.isBlack())
  {
    //glcd(2,1,"Straight                    ");
      if(code == 2)
        fw2(slowSpeed - 7 , slowSpeed - 7);
      else if(code == 3)
        fw2(slowSpeed - 15 , slowSpeed - 15);
      else if(code == -1)
        fw2(-slowSpeed,-slowSpeed);
      else if(code == 4)
        fw2(slowSpeed - 15 , slowSpeed - 15);
       else
        fw2(slowSpeed, slowSpeed);
      i = 5;
  }
  else if(midLeftSensor.isWhite() && midRightSensor.isBlack())// curve on Right
  {
     // glcd(2,1,"Turn Right Bit             ");
     if(code == 2)
       fw2(slowSpeed,0);
     else if(code == 3)
        fw2(slowSpeed + 35,-slowSpeed);
     else if(code == -1)
       fw2(-slowSpeed, -normalSpeed);
     else if(code == 4)
     {
       motor_stop(ALL);
       while(!(midLeftSensor.isBlack() && midRightSensor.isBlack()))
         fw2(slowSpeed,0);
     }
     else
       fw2(normalSpeed, slowSpeed);
      i = 1;
  }
  else if(midLeftSensor.isBlack() && midRightSensor.isWhite())//curve on left
  {
     // glcd(2,1,"Turn Left Bit            ");
     if(code == 2)
       fw2(0,slowSpeed);
     else if(code == 3)
       fw2(0,slowSpeed + 5);
     else if(code == -1)
       fw2(-normalSpeed,-slowSpeed);
     else if(code == 4)
     {
       motor_stop(ALL);
       while(!(midLeftSensor.isBlack() && midRightSensor.isBlack()))
         fw2(0,slowSpeed);
     }
     else
        fw2(slowSpeed, normalSpeed);
     i = 2;
  }
  else if(code != -1)
  {
      switch(i)
      {
        case 1: fw2(fastSpeed, slowSpeed); break;
        case 2: fw2(slowSpeed , fastSpeed); break;
        case 3: fw2(-normalSpeed, normalSpeed); break;
        case 4: fw2(normalSpeed, -normalSpeed); break;
        case 5: fw2(fastSpeed,fastSpeed);break;
      }
  }
  else
  {
        fw2(-slowSpeed,-slowSpeed);
  }
}

//A Common Part From Methods trackCan L,R
void trackCan()
{
  //Go on at full speed for 1 second hit a can
   //glcd(1,1,"Bumping!                               ");
   /*fw2(canHittingSpeed,canHittingSpeed);
   while(rightSensor.isWhite());*/

   fw2(-slowSpeed,-slowSpeed);
   delay(700);

   while(rightSensor.isWhite())
    track(-1);
  turnR90();
}

void allignLine()
{
  motor_stop(ALL);
  if(midLeftSensor.isBlack() && midRightSensor.isBlack())
    return;
  while(!( midLeftSensor.isBlack() && midRightSensor.isBlack() ))
  {
    if(leftSensor.isBlack())
      fw2(-10,10);
    else if(rightSensor.isBlack())
      fw2(10,-10);
  }
}

void trackCanL()
{
  while(rightSensor.isWhite())
    track(1);
  //glcd(2,1,"Turn Left 90");
  turnL90();
  while(rightSensor.isWhite())
    track(1);
  //glcd(2,1,"Attack Can                ");
  allignLine();
  fw2(slowSpeed,slowSpeed);
  delay(500);
  trackCan();
}

void trackCanL2()
{
  while(rightSensor.isWhite())
    track(2);
  //glcd(2,1,"Turn Left 90");
  turnL90();
  fw2(normalSpeed,normalSpeed);
  delay(1000);// Gap DElay
  while(rightSensor.isWhite())
    track(1);
  //glcd(2,1,"Attack Can                ");
  allignLine();
  fw2(slowSpeed,slowSpeed);
  delay(500);
  trackCan();
}

void trackCanL3()
{
  while(leftSensor.isWhite() || rightSensor.isWhite())
    track(3);
  //glcd(2,1,"Turn Left 90");
  turnL90();

  while(rightSensor.isWhite())
    track(4);
  //glcd(2,1,"Attack Can                ");

  while(midLeftSensor.isBlack() || midRightSensor.isBlack())
  {
    fw2(slowSpeed,slowSpeed);
  }
  motor_stop(ALL);
  while(midLeftSensor.isWhite() || midRightSensor.isWhite())
  {
    fw2(slowSpeed,0);
  }
  motor_stop(ALL);
  delay(500);
  fw2(100,100);
  while(rightSensor.isWhite());
  motor_stop(ALL);
}

void setup()
{
  setTextSize(2);
  glcd(4,1,"Press OK!");
  sw_OK_press();
  glcdClear();
  setTextSize(1);
  glcdMode(1);
  fw2(slowSpeed,slowSpeed);
  delay(750);
  trackCanL();
  trackCanL2();
  trackCanL3();
}

int speeds = 0;

void loop()
{
  //debug();
  //fw2(slowSpeed,slowSpeed);
}

void debug()
{
  glcd(6,1,">DEBUG MODE");
  glcd(1,1,"%d  %d  %d  %d   ",leftSensor.input(),midLeftSensor.input(),midRightSensor.input(),rightSensor.input());
  glcd(3,1,"%d  ",speeds);
  glcd(2,1,"%d   %d   %d   %d  ",leftSensor.isBlack(),midLeftSensor.isBlack(),midRightSensor.isBlack(),rightSensor.isBlack());
  if(sw_OK())
    speeds++;
  else if(sw1())
    speeds--;
  fw2(speeds,speeds);
}