// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Domino Bot
// By Dave Astolfo
// www.astolfo.com/bots
// The idea:
// Build a robot that has the ability to place domino's in a row as defined
// by a black line placed on a white background. Because of the robots rotation
// axis, the line should not be tight and curved, but rather gradually curving.
// Too tight a curve will cause the robot to turn so much that the next domino
// placed will not be lined up within reach of the others.
// If you do not wish it to follow a line, just comment out the
// start checklight function call in the main() function.

// Program flow:
// the robot first starts by calibrating the light sensor. It gets values for both
// dark and light (essentially black and white). Then uses these values to follow the
// line.
// Once complete, it begins driving forward while following the line AND checking
// distance so it knows when to place a domino.
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

#define MAX_DISTANCE 4
int dominocheck=0;
int calibration=0;
int l_limit;
int u_limit;
int val;
int i;

void fwd(int x)
{
  OnRev(OUT_A); OnFwd(OUT_C);Wait(x);
}

void rev(int x)
{
  OnFwd(OUT_A); OnRev(OUT_C); Wait(x);
}

void left(int x)
{
  OnRev(OUT_A); OnRev(OUT_C); Wait(x);
}

void right(int x)
{
  OnFwd(OUT_A); OnFwd(OUT_C); Wait(x);
}

void freeze()
{
  Off(OUT_A + OUT_C);
}


void calibrate()
{
  l_limit=9999;
  u_limit=0;
  //reduce the power of the drive motors temporarily
  SetPower(OUT_A + OUT_C, 1);

  //calibrate
  left(100);
  right(100);
  ClearTimer(0);
  while (Timer(0)<10)
  {
    val=SENSOR_1;
    if (val<l_limit) l_limit=val;
    if (val>u_limit) u_limit=val;
  }

  //compute parameters
  val=(u_limit+l_limit)/2;
  l_limit=val-5;
  u_limit=val-5;
  
  //bring drive motors back up to full power
  SetPower(OUT_A + OUT_C, 7);
}


task checklight()
{
  //begin moving forward to find the line
  fwd(1);
  while (SENSOR_1 > u_limit || SENSOR_1<l_limit);
  //freeze();
  left(100);
  while(true)
  {
    if (SENSOR_1<l_limit)
      {
      right(30);
      fwd(50);
      }
    else if (SENSOR_1>u_limit)
      {
      left(30);
      fwd(50);
      }
    else
      freeze();
  }
}


void dodomino()
 //the assumption is that the robot will start with the domino box loaded and
 //the domino "hand" will be in the back position. That meaning, the next
 //time the touch sensor (SENSOR_2) is tripped, a domino has been pushed out and placed
 //on the surface. This should cause the motor to reverse until the sensor is
 //tripped again. Then there will be a pause. The bot will drive fwd a bit,
 //and the process will repeat.
  //first step is to stop the bot
{

//first step - after the first domino is dropped. The loading unit will reatract
//to load another domino. This will cause SENSOR_2 to remain tripped with a value
//of 1. Since the first time through does not yield this, it has to be compensated
//for.

  if (SENSOR_2 == 1)
    {
     until(SENSOR_2 == 0)
     {
       OnRev(OUT_B);
     }
     Off(OUT_B);
    }

//Now perform the domino place and retract routines
   until(SENSOR_2 == 1)
   {
     OnRev(OUT_B);
   }
   Off(OUT_B);
   //go just a bit futher to ensure domino was dropped.
   OnRev(OUT_B);Wait(50);Off(OUT_B);
   
   //shift the domino back a bit to ensure that it will be close to the last one.
   rev(1);Wait(50);freeze();
   
   Wait(100);
   OnFwd(OUT_B);
   //move arm so sensor is no longer tripped or below code wont work.
   Wait(150);


   //move arm back and load another domino
   until(SENSOR_2 == 1)
   {
     OnFwd(OUT_B);
   }
   Off(OUT_B);
   PlaySound(SOUND_CLICK);
   dominocheck = 1;
}

task checkdistance()
{
  while(true)
  {
  //using a sensor mode of PULSE allows a check to be done each time the sensor
  //is tripped. This increments the counter by 1.   Doing some basic ground testing,
  //a count of 3 (MAX_DISTANCE) allows for the bot to move a reasonable distance between each domino.
  //Note a different sized domino will require this value to be re-calibrated
  
    if (SENSOR_3 == MAX_DISTANCE)
    {
      //stop moving forward
      freeze();
      PlaySound(SOUND_DOUBLE_BEEP);
      //move fwd just a tad to clear the last domino
      fwd(1);Wait(100);freeze();
      
      //clear the sensor.
      ClearSensor(SENSOR_3);
      //stop trying to follow the line while a domino is being placed
      stop checklight;
      //place a domino
      dodomino();
      //do not start following the line again until the domino is placed.
      //this is determined by dominocheck
      until(dominocheck == 1); Wait(1);
      //tone was used to check to make sure the variable was being set - not needed now
      //PlayTone(440, 200);
      start checklight;
      //set the check var back to 0
      dominocheck = 0;
    }
  }
}

task main()
{
  SetSensorType(SENSOR_1, SENSOR_TYPE_LIGHT);
  SetSensorType(SENSOR_2, SENSOR_TYPE_TOUCH);
  SetSensorMode(SENSOR_2, SENSOR_MODE_BOOL);

  //the distance sensor is set to pulse so that we can count the number of
  //turns.
  SetSensorType(SENSOR_3, SENSOR_TYPE_TOUCH);
  SetSensorMode(SENSOR_3, SENSOR_MODE_PULSE);
  SetPower(OUT_A + OUT_B + OUT_C, 7);

  //calibrate the light sensor
  //calibrate();

  //drive forward to begin
  fwd(1);

  //a domino will be placed every 3 clicks of the sensor.
  start checkdistance;
  
  //follow a black line - this will be roughly the path the domino's are placed on
  start checklight;
}