// Block Stacker 3 - Runamok Design Team
// Rick De Haan, David Koudys
// September 22, 2000


//drive definitions
#define off         SetOutput(OUT_A+OUT_C,OUT_OFF);
#define drift       SetOutput(OUT_A+OUT_C,OUT_FLOAT);
#define speed(s)    SetPower(OUT_A+OUT_C,s);
#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_A,OUT_REV);SetDirection(OUT_C,OUT_FWD);SetOutput(OUT_A+OUT_C,OUT_ON);
#define right       SetDirection(OUT_A,OUT_FWD);SetDirection(OUT_C,OUT_REV);SetOutput(OUT_A+OUT_C,OUT_ON);

//grabber definitions
#define handoff     SetOutput(OUT_B,OUT_FLOAT);
#define handstop    SetOutput(OUT_B,OUT_OFF);
#define strength(t) SetPower(OUT_B,t);
#define handup      SetDirection(OUT_B,OUT_FWD);SetOutput(OUT_B,OUT_ON);
#define handdown    SetDirection(OUT_B,OUT_REV);SetOutput(OUT_B,OUT_ON);

int num_blocks_pickedup; // current # of blocks we have picked up.
int max_num_blocks;      // maximum number of blocks to pick up
int working_mode;        // 1:searching  2:hunting  3:finishup
int performing_action;   // 0:not doing anything    1:doing something
int light_value;         // sets the value of the light sensor
int saw_a_block_in_chute;// 0:did not see a block   1:saw a block
int time_to_find_wall;
int time_to_go_and_dump; //
int max_drive_time;
int num_retrys;          // number of time it tried to grab the block

//start program
task main()
{
  //defining sensor inputs
  SetSensorType(SENSOR_1, SENSOR_TYPE_LIGHT); SetSensorMode(SENSOR_1, SENSOR_MODE_PERCENT);
  SetSensorType(SENSOR_2, SENSOR_TYPE_ROTATION); SetSensorMode(SENSOR_2, SENSOR_MODE_ROTATION);ClearSensor(SENSOR_2);
  SetSensorType(SENSOR_3, SENSOR_TYPE_LIGHT); SetSensorMode(SENSOR_3, SENSOR_MODE_PERCENT);

  // RESET and stop
  ClearTimer(0);    //reset timer (competition is 5 min)
  time_to_find_wall = 2300;    //2300
  time_to_go_and_dump = 2700;  //2700
  max_drive_time = 250;
  light_value = 60;
  performing_action = 0;
  working_mode = 0;
  max_num_blocks = 7;
  num_blocks_pickedup = 0; // check here
  strength(1);  handdown;  Wait(200);  handoff; Wait(300);
  ClearSensor(SENSOR_2);
  speed(7);
  strength(7);
  SelectDisplay(2); //display rotation values on LCD

  // Main loop
start major;
//start count_check;
start catch_all;
while (true)
{
}
}


task count_check()
{
while (true)
  {
    // Late addition to code. If the chute sensor is not blocked, then we do 
    // not have 6 blocks. If it is blocked then we do have 6 blocks
    // Note that these checks occur within the 'performing action' semaphore - 
    // cause if it did not - this check and adjustment would occur as a block
    // slides past the light.
    if (working_mode != 1)
    {
    if ((num_blocks_pickedup >= max_num_blocks - 1) && (SENSOR_3 > light_value))
       {
       PlaySound(0);
       PlaySound(4);
       PlaySound(0);
       num_blocks_pickedup--;
       }
    if ((num_blocks_pickedup < max_num_blocks - 1) && (SENSOR_3 < light_value))
       {
       PlaySound(0);
       PlaySound(4);
       PlaySound(0);
       num_blocks_pickedup = 6;
       }
    if (num_blocks_pickedup >= max_num_blocks - 1)
       {
       stop chute_sensor;
       }
    }
}
}



task catch_all()
{
 while (true)
    {
    if ((Timer (0) >= time_to_go_and_dump + 50) && (working_mode != 4))
      {
      drift;
      handoff;
      PlayTone(29,30);
      stop find_wall;
      stop hunt_sensor;
      stop searching;
      stop chute_sensor;
      stop major;

      // if hand in motion then we must put it down!
      if (performing_action == 1)
         {
         until (SENSOR_2 <= 5)
          {
          strength(1);
          handdown;
          }
         }
      
      working_mode = 4;
      // dump the blocks
      dump_block ();
      }
   }
}


task major()
{
  while (true)
    {

    if ((Timer (0) >= time_to_go_and_dump) && (working_mode != 4))
      {
      drift;
      PlayTone(29,30);
      stop find_wall;
      stop hunt_sensor;
      stop searching;
      stop chute_sensor;

      // if hand in motion then we must put it down!
      if (performing_action == 1)
         {
         until (SENSOR_2 <= 5)
          {
          strength(1);
          handdown;
          }
         }
      
      working_mode = 4;
      // dump the blocks
      dump_block ();
      }
    else if ((Timer (0) >= time_to_find_wall) &&  (Timer(0) <= time_to_go_and_dump) && (num_blocks_pickedup >= max_num_blocks) &&(working_mode != 3))
      // If we are at the time to go find wall
      //    and we are before the time to stand up
      //    and we have max blocks in chute
      //    and we are not in 'find wall' mode
      //    then go find wall
      {
      // If we have the hand in action - we don't switch
      working_mode = 3;
      stop searching;
      stop chute_sensor;
      stop hunt_sensor;
      start find_wall;
      }
    else if ((Timer(0) < time_to_go_and_dump) && (num_blocks_pickedup < max_num_blocks) && (working_mode != 1))
       {
       working_mode = 1;
       PlayTone(29,10);
       speed(7);forward;
       start searching;
       start chute_sensor;
       }
    else if ((Timer (0) < time_to_find_wall) && (num_blocks_pickedup >= max_num_blocks) && (working_mode != 2)) 
      // If we are BEFORE the time to find the wall
      //    and we have picked up MAX number of blocks
      //    and we are not in 'hunt' mode 
      //    then go hunt.
      {
      // If we have the hand in action - we don't switch
      if (performing_action != 1 )
         {
         working_mode = 2;
         stop searching;
         stop chute_sensor;
         speed(7);backward;
         start hunt_sensor;
         }
      }
    else if (Timer (0) >= 31000)
      {
      handoff;
      off;
      }
   }
}



task searching()
{
  //drive around 
  //if we run into something - then turn
  //if we find a block - then lift it up.
  //The main() will let us continue to do this until time is up, or 
  //           until we have 7 blocks
  ClearTimer(1);    
  while (true)
  {
    if ( performing_action == 0 ) 
       {
        if (SENSOR_1 < light_value)
          {
          // Do the claw
          off;
          PlaySound(5);
          lift_block();
          ClearTimer(1);    
          // we tried 2 times, and the sensor is still blocked - then we have a jam
          if ((num_retrys >= 2) && (SENSOR_1 < light_value))
             {
             unjam();
             }
          forward;
          }
       else if (SENSOR_1 > 99)                          //check if left bumper is pressed
          {       
          PlaySound(0);
          speed(7);backward;Wait(50);right;Wait(300);forward;
          ClearTimer(1);    
          }
       else if (SENSOR_3 > 99)                     //check if right bumper is pressed
          {
          PlaySound(0);
          speed(7);backward;Wait(50);left;Wait(300);forward;
          ClearTimer(1);    
          }
       else if (Timer(1) > max_drive_time)
          {
          speed(7);backward;Wait(50);left;Wait(400);forward;
          ClearTimer(1);
          }
      }
   }
}



task chute_sensor()
{
  // Keep looping. Everytime we see a block we 
  // set the variable to 1 indicating that we 
  // saw a block.
  while (true)
    {
    if (SENSOR_3 < light_value)
       {
       saw_a_block_in_chute = 1 ;
       PlayTone(104,2);
       }
    }
}


sub lift_block()
  {

  // move forward just a hair to make sure block is there
  performing_action = 1;
  speed(1);
  forward;Wait(40);
  drift;
  strength(7);

  saw_a_block_in_chute = 0;
  num_retrys = 0;

  if (num_blocks_pickedup < max_num_blocks - 1)
     {
     //While the claw is blocked, up to 2 times, try to lift it.
     while (num_retrys < 2 && SENSOR_1 < light_value)
        {
         num_retrys ++ ;
         // On second and 3rd retry, move forward a hair.
         if (num_retrys > 1) 
            {
            speed(1);
            forward;
            Wait(30);
            off;
            speed(7);
            }
        //lift up
        until (SENSOR_2 > 39)
          {
          handup;
          } 
        handoff;
//        handdown;Wait(300);
//        handup;Wait(300);
        until (SENSOR_2 <= 5)
          {
          strength(1);
          handdown;
          }
        handdown;Wait(100);
        handoff; Wait(200); 
        ClearSensor(SENSOR_2);
        // if we saw a block dump - then count it.
        if (saw_a_block_in_chute == 1 )
           {
           num_blocks_pickedup ++;
           }
        }
     }
  else
     {
     // lift the block just a bit (for the last block)
     until (SENSOR_2 > 13)
       {
       handup;
       }
     handstop;
//     // backup a bit
//     backward;
//     Wait(100);
//     off;
//     // if the block is still there - then we cout it as 'grabbed'
//  //   if (SENSOR_1 < light_value) 
//        {
        num_blocks_pickedup ++;
//        }
     }
  performing_action = 0;
  }



task hunt_sensor ()
  {
  //drive around 
  //if we run into something - then turn
  //The main() will let us continue to do this until time is up
  ClearTimer(1);
  while (true)
     {
     if (SENSOR_1 > 99)                          //check if left bumper is pressed
        {       
        Wait(50);speed(7);forward;Wait(100);left;Wait(300);backward;
        ClearTimer(1);
        }
     else if (SENSOR_3 > 99)                     //check if right bumper is pressed
        {
        Wait(50);speed(7);forward;Wait(100);right;Wait(300);backward;
        ClearTimer(1);
        }
     else if (Timer(1) > max_drive_time)
        {
        speed(7);backward;Wait(50);left;Wait(400);forward;
        ClearTimer(1);
        }
     }
  }


task find_wall ()
{
      int found_wall;
      int which_side_touched;  // value of 1 for sensor 1, and 3 for sensor 3
      PlaySound(5);
      PlaySound(1);
      PlaySound(5);

      // head for a wall!
      speed(7);
      found_wall = 0;
      which_side_touched = 0;

      forward;
      until (which_side_touched != 0)
        {
        if ((SENSOR_1 > 99) && (SENSOR_3 < 99))
           {
           which_side_touched = 1;
           off;
           backward; Wait(50);
           left; Wait(50);
           off;
           }
  
        if ((SENSOR_1 < 99) && (SENSOR_3 > 99))
           {
           which_side_touched = 3;
           off;
           backward; Wait(50);
           right; Wait(50);
           off;
           }

        if ((SENSOR_1 > 99) && (SENSOR_3 > 99))
           {
           found_wall = 1;
           which_side_touched = 2;
           off;
           }
        }

      if (found_wall != 1)
         {
         forward;      
         }

      until (found_wall == 1)
          {
          if ((which_side_touched == 3) && (SENSOR_1 > 99))
             {
             found_wall = 1;
             }

          if ((which_side_touched == 1) && (SENSOR_3 > 99))
             {
             found_wall = 1;
             }
          if ((which_side_touched == 1) && (SENSOR_1 > 99))
             {
             off;
             backward; Wait(50);
             left; Wait(50);
             forward;
             }
          if ((which_side_touched == 3) && (SENSOR_3 > 99))
             {
             off;
             backward; Wait(50);
             right; Wait(50);
             forward;
             }
          }
      off;
}


sub dump_block()
{
      performing_action = 1;
      speed (7);
      backward;
      Wait(20);
      PlaySound(2);
      PlaySound(3);
      off;
      
      strength(7);

      // tower up
      until (SENSOR_2 > 67)
      {
        handup;
      }
      // open door
      until (SENSOR_2 <= 25)
      {
        handdown;
      }
      handoff;
      // move back
      speed(1);backward;Wait(70);off;
      performing_action = 0;
}


sub unjam()
{
   //lift up
   strength(7);
   until (SENSOR_2 > 39)
      {
      handup;
      } 
      handoff;

   speed(7);
   backward;
   Wait (40);
   off;

   // No choice to keep trying this until we get that block out of there!
   // (and if it already gone then we don't even have to go in.
   until (SENSOR_1 > light_value)
      {
      speed(5);
      // make sure the block is to the left
      forward;Wait(50);
      // turn left to get the block against the left side.
      left;Wait(300);
      //  right till it is centered
      right;Wait(70);
      off;
      // back it out
      speed(7);
      backward;
      Wait(100);
      off;
      }

   // put he hand down
   until (SENSOR_2 <= 5)
      {
      strength(1);
      handdown;
      }
      handdown;Wait(100);
      handoff; Wait(200); 
      ClearSensor(SENSOR_2);
}

/*
Rotation Values:
Grabber open, arm down, tower down -  0
Grabber closed, on block -            15
Grabber closed, arm up, tower down -  40
Grabber closed, arm up, tower up -    67
Grabber open, arm down, tower up -    30
*/