/*
Dave's Program for rtl12 - Block Stacking
*/

//motion definitions
#define forward     SetDirection(OUT_A+OUT_C,OUT_FWD);SetOutput(OUT_A+OUT_C,OUT_ON);
#define backward    SetDirection(OUT_A+OUT_C,OUT_REV);SetOutput(OUT_A+OUT_C,OUT_ON);
#define left        SetDirection(OUT_C,OUT_REV);SetDirection(OUT_A,OUT_FWD);
#define right       SetDirection(OUT_A,OUT_REV);SetDirection(OUT_C,OUT_FWD);
#define speed(s)    SetPower(OUT_A+OUT_C,s);
#define off         SetOutput(OUT_A+OUT_C,OUT_FLOAT);
#define up          SetDirection(OUT_B,OUT_REV);SetOutput(OUT_B,OUT_ON);
#define down        SetDirection(OUT_B,OUT_FWD);SetOutput(OUT_B,OUT_ON);
#define flbelt      SetOutput(OUT_B,OUT_FLOAT);

//declare variables
int sem;
int bumps;                            //5 bumps in 5 seconds--bot is trapped
int drive_time;                       //driving time to search for blocks--approx 2 min 45 sec
int belt_sensor;                      //rotation of belt

task main()
{
//set up initial values
SetSensorType (SENSOR_1, SENSOR_TYPE_TOUCH);SetSensorMode (SENSOR_1, SENSOR_MODE_BOOL);
SetSensorType (SENSOR_2, SENSOR_TYPE_ROTATION);SetSensorMode (SENSOR_2, SENSOR_MODE_ROTATION);
SetSensorType (SENSOR_3, SENSOR_TYPE_TOUCH);SetSensorMode (SENSOR_3, SENSOR_MODE_BOOL);

ClearTimer(0);
ClearTimer(1);
off;
speed(7);
sem=0;
bumps=0;
drive_time=1600;

//start can grabber

PlaySound(1);                   //inform user to put can in front of sensor

start timeline;
start driving;
start conveyor_belt;
while (true)
    {
    }
}

task conveyor_belt()
{
  while(true)
  {
    belt_sensor = SENSOR_2;
    up;Wait(60);
    if (abs(SENSOR_2) <= abs(belt_sensor))             //see if belt is jammed
    {
      down;Wait(50);
    }
  }
}

 task timeline()
{
  while(true)
  {
    if (Timer(1) > drive_time)          //check to see if time is up for driving around--2:45ish
    {
      stop driving;
      stop conveyor_belt;
      flbelt;
      off;Wait(1650);               //wait for blocks to be unloaded
      forward;Wait(200);
      off;
      stop timeline;
    }
  }
}

task driving()
{
  while(true)
  {
    if (SENSOR_1 == 1)                          //check if right bumper is pressed
    {
      until (sem == 0);sem = 1;
      backward; Wait(100);                    //back up from obstacle
      left; Wait(100);                //turn left
      forward;
      ClearTimer(0);
    }
    else if (SENSOR_3 == 1)                          //check if left bumper is pressed
    {
      until (sem == 0);sem = 1;
      backward; Wait(100);                    //back up from obstacle
      left; Wait(100);                //turn right
      forward;
      ClearTimer(0);
    }
    else
    sem = 0;
    forward;
    if (Timer(0) > 100)
    {
      backward;Wait(300);
      left;Wait(300);
      forward;
      ClearTimer(0);
    }
  }
}