// Program for the grabberarm robot
// Thomas Waadeland  12. November 2001

int lastlevel = 0;
int something = 0;

task send_signal()
{
  while (0 == something)
    { SendMessage(0); Wait(10); }
}

task check_signal()
{
  while (0 == something)
  {
    lastlevel = SENSOR_2;
    if (SENSOR_2 > lastlevel+150)
    { 
      something = 1;
    }
  }
}

task close_grab()
{
  OnRev(OUT_A); // Close
  Wait(300);
  Off(OUT_A);
}

task open_grab()
{
  OnFwd(OUT_A); // Open
  Wait(300);
  Off(OUT_A);
}

sub set_down()
{
  // lower arm
  OnRev(OUT_B); // Down
  until (1 == SENSOR_1);
  start open_grab;
  until (0 == SENSOR_1);
  // raise arm
  OnFwd(OUT_B); // Up
  until (1 == SENSOR_3);
  Off(OUT_B);
}

sub pick_up()
{
  // lower arm
  OnRev(OUT_B); // Down
  until (1 == SENSOR_1);
  Wait(5);
  start close_grab;
  until (0 == SENSOR_1);
  // raise arm
  OnFwd(OUT_B); // Up
  until (1 == SENSOR_3);
  Off(OUT_B);
}

task main()
{
  SetSensorType(SENSOR_2, SENSOR_TYPE_LIGHT);
  SetSensorMode(SENSOR_2, SENSOR_MODE_RAW);
  SetSensor(SENSOR_1,SENSOR_TOUCH);
  SetSensor(SENSOR_3,SENSOR_TOUCH);

  SetTxPower(TX_POWER_HI);

  SetPower(OUT_A+OUT_B+OUT_C, 7);

  Off(OUT_A+OUT_B+OUT_C);

  // init
  if (1 != SENSOR_3)
  {
    start open_grab; 
    // raise arm
    OnFwd(OUT_B); // Up
    until (1 == SENSOR_3);
    Off(OUT_B);
  }
  
  start send_signal;
  start check_signal;

  OnRev(OUT_C); // Turn clockwise
  until (1 == something);
  Wait(20);
  Off(OUT_C);
      
  pick_up();
  PlaySound(5);
  Wait(200);

  OnFwd(OUT_C); // Turn anti-clockwise
  Wait(500);
  Off(OUT_C);
  set_down();

  Float(OUT_A+OUT_B+OUT_C);    
}