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

//motion definitions
#define container_up    SetDirection(OUT_A,OUT_FWD);SetOutput(OUT_A,OUT_ON);
#define container_down  SetDirection(OUT_A,OUT_REV);SetOutput(OUT_A,OUT_ON);
#define container_off   SetOutput(OUT_A,OUT_OFF);
#define container_float SetDirection(OUT_A,OUT_REV);SetOutput(OUT_A,OUT_FLOAT);
#define hatch_open      SetDirection(OUT_B,OUT_REV);SetOutput(OUT_B,OUT_ON);
#define hatch_closed    SetDirection(OUT_B,OUT_FWD);SetOutput(OUT_B,OUT_ON);
#define hatch_off       SetOutput(OUT_B,OUT_OFF);
#define walls_open      SetDirection(OUT_C,OUT_FWD);SetOutput(OUT_C,OUT_ON);
#define walls_closed    SetDirection(OUT_C,OUT_REV);SetOutput(OUT_C,OUT_ON);
#define walls_off       SetOutput(OUT_C,OUT_OFF);

int drive_time;                       //driving time to search for blocks--approx 2 min 45 sec

//declare variables

task main()
{
ClearTimer(1);
drive_time=1600;
SetPower(OUT_A+OUT_B+OUT_C,7);
container_up;hatch_closed;walls_closed;Wait(800);
container_off;hatch_off;walls_off;
start dumping;
while (true)
  {
  }
}

task dumping()
{
  while(true)
  {
  if (Timer(1) > drive_time)          //check to see if time is up for driving around--2:45ish
    {
    container_down;Wait(50);container_float;
    start container_shake;
    hatch_open;Wait(800);hatch_off;
    walls_open;Wait(800);walls_off;
    stop container_shake;
    stop dumping;
    }
  }
}

task container_shake()
{
  while (true)
  {
    container_up;Wait(10);container_down;Wait(10);container_float;Wait(40);
  }
}