/* My first try at a NQC program - 1999-01-04, Anders Isaksson

   Objective:
     To turn when hitting something with one sensor,
     to go backwards a little, and then turn 90 degrees, when hitting both front sensors
     to go backwards a little, and then turn 90 degrees, when hitting back sensor

   Sensors used:  
     IN_L - detect hit on left front
     IN_M - back, detecting a 'climb'
     IN_R - detect hit on right front

     tacho counters from motor 0 and 1 (later)

   Motors used:
     OUT_L - left drive
     OUT_R - right drive

   Version 1.0       First try

*/

#define BOTH         OUT_L+OUT_R

#define TURN_CONTROL 1

#define LEFT         1
#define RIGHT        2
#define FORWARD      3
#define REVERSE      4

#define BACK_DIST    10
#define TURN_AMT     7

int     
  TurnDir,     //Direction to turn 
  TurnAmt,     //Amount to turn (time to turn for the moment)
  MoveDir,     //Direction to move
  MoveAmt,     //Amount to move (time to move for the moment)
  Speed,       //Drive speed
  TimeLimit;  

sub Move
{
  if (MoveDir == FORWARD)
  {
    Fwd(BOTH, Speed);
  }
  else
  {
    Rev(BOTH, Speed);
  }
  TimeLimit = MoveAmt;
  ClearTimer(TURN_CONTROL);
//  Sleep(MoveAmt);
//  Off(BOTH);
}

sub Turn
{
  if (TurnDir == LEFT)
  {
    Fwd(OUT_R, Speed);
    Rev(OUT_L, Speed);
  }
  else
  {
    Fwd(OUT_L, Speed);
    Rev(OUT_R, Speed);
  }
  TimeLimit = TurnAmt;
  ClearTimer(TURN_CONTROL);
//  Sleep(TurnAmt);
//  Off(BOTH);
}

task main
{
  Speed = OUT_FULL;
  Speed /= 2;

  while (1==1)     
  {
    if (IN_L == 1)  
    {
      if (IN_R == 1)  
      {
        MoveDir = REVERSE;
        MoveAmt = BACK_DIST;  
        Move();
        while (Timer(TURN_CONTROL) < TimeLimit)
          ;  
        TurnAmt = TURN_AMT;
        TurnDir = LEFT;   
        Turn();
      }
      else
      {
        TurnAmt = TURN_AMT;
        TurnDir = RIGHT;  
        Turn();
      }
    }
    else if (IN_R == 1)
    {
      if (IN_L == 1)
      {
        MoveDir = REVERSE;
        MoveAmt = BACK_DIST;  
        Move();
        while (Timer(TURN_CONTROL) < TimeLimit)
          ;  
        TurnAmt = TURN_AMT;
        TurnDir = LEFT;   
        Turn();
      }
      else
      {
        TurnAmt = TURN_AMT;
        TurnDir = LEFT;   
        Turn();
      }  
    }
    else if (IN_M == 1)
    {
      MoveDir = REVERSE;
      MoveAmt = BACK_DIST;  
      Move();
      while (Timer(TURN_CONTROL) < TimeLimit)
        ;  
      TurnAmt = TURN_AMT;
      TurnDir = LEFT;   
      Turn();
    }
    else
    {
      if (Timer(TURN_CONTROL) >= TimeLimit)
        Fwd(BOTH, Speed);
    }
  }
}