//
// Program used by "myIrAsuro" in qualifying for competition
// "Parallel Slalom" of RobotChallenge 2007 in Vienna (24th march):
// http://www.robotchallenge.at
//
// Time 10.1s for 354cm, that is speed 35.05cm/s (best was 60.00 cm/s).
//
// myIrAsuro has reduction of 1:12.5 (Asuro standard is 1:25), see:
// http://www.roboternetz.de/phpBB2/zeigebeitrag.php?t=28876
//
// The entries 0xFF for both SPEED values below will work on
// a standard Asuro with speed < 0.5m/s.
//
// The value for THRESHOLD must be changed if no IR-FrontLED
// with white shield is used, see:
// http://www.roboternetz.de/phpBB2/zeigebeitrag.php?t=29109
//
//
// Robot will have to be placed on the line for starting!
//
// After starting the StatusLED of Asuro will blink [ready].
// After (any) switch is pressed both BackLED's turn on [steady].
// Releasing the switch will start the Robot [go].
//
#include <asuro.h>

#define OFF_LEFT  -1
#define ON_LINE   0
#define OFF_RIGHT +1

#define SPEED  0xdF // 0xFF
#define SPEED1 0xbF // 0xFF
#define SPEED2 0x00

#define THRESHOLD  1100

void RaceStart(void); // blink until any switch is pressed,
                      // then wait until switch is released


int main(void)
{
  unsigned int sum,data[2];

  int position = ON_LINE;

  Init();

  RaceStart();
 
  FrontLED(ON);   
  MotorDir(FWD,FWD);
  MotorSpeed(SPEED,SPEED);

  while (1)                  // state machine: position on/off the line
  {
    LineData(data);
    sum = data[LEFT] + data[RIGHT];


    if (sum>=THRESHOLD)      // not on the line
    {
      if (position==ON_LINE) // switch state if previously on the line
      {
        position = (data[LEFT]<data[RIGHT]) ? OFF_RIGHT : OFF_LEFT;

        BackLED( (position==OFF_LEFT)?ON:OFF, (position==OFF_RIGHT)?ON:OFF );
      }
    }
    else                     // on the line
    {
      if (position!=ON_LINE) // switch state if previously not on the line
      {
        position = ON_LINE;

        BackLED(OFF,OFF);
      }
    }
    

         if (position==ON_LINE)   { MotorSpeed(SPEED,SPEED);   } // full FWD
    else if (position==OFF_RIGHT) { MotorSpeed(SPEED2,SPEED1); } // turn left
    else if (position==OFF_LEFT)  { MotorSpeed(SPEED1,SPEED2); } // turn right
  }


  while (1);                 // code unreachable

  return 0;
}


void RaceStart(void)    // blink until any switch is pressed,
{                       // then wait until switch is released
  uint8_t t1, t2;
  unsigned int col=OFF;

  while (1)             // blinking StatusLED until any switch is pressed
  {
    t1 = PollSwitch();
    t2 = PollSwitch();
    if (t1==t2)
    {
      if (t1)
      {
        break;
      }
      else
      {
        col ^= GREEN;
        StatusLED(col);
      }
    }
    Msleep(50);
  }

  StatusLED(OFF);       // turn off StatusLED and ...
  BackLED(ON,ON);       // ... turn on both BackLED's

  while (1)             // wait until switch is released
  {
    t1 = PollSwitch();
    t2 = PollSwitch();
    if (t1==t2)
    {
      if (!t1)
      {
        break;
      }
    }
    Msleep(50);
  }

  BackLED(OFF,OFF);     // turn off BackLED's indication start of race
}

...
  while (1)                  // state machine: position on/off the line
  {
    LineData(data);
    sum = data[LEFT] + data[RIGHT];


    if (sum>=THRESHOLD)      // not on the line
    {
...
    }
    else                     // on the line
    {
...
    }
...
  }
...