#define ArmMotor OUT_B
#define RaisedArm SENSOR_2
#define InitialArmTime 200

#define Laser SENSOR_1

#define R_Motor OUT_C
#define L_Motor OUT_A
#define Both_Motors OUT_C+OUT_A

#define Right 1
#define Left 0

#define CylDetect SENSOR_3
#define Threshold 4

#define MaxRecenterTime 200
#define MaxRotationTime 1400

int CylInClaw = false;
int ArmTime = InitialArmTime;

task main ()
{
  SetSensor (RaisedArm, SENSOR_TOUCH);
  SetSensor (Laser, SENSOR_LIGHT);
  SetSensorMode (Laser, SENSOR_MODE_BOOL);

  start CheckForCylinder;
  LowerArm();

  int Direction;
  int TimeOut;

  while (true)
  {
    start CheckCylinderRemaining;
    Direction = Right;
    SetPower(Both_Motors,7);
    OnFwd (R_Motor);
    Off(L_Motor);
    until (Laser);
    stop CheckCylinderRemaining;
    PlaySound (SOUND_CLICK);
    while (Laser);
    PlaySound (SOUND_DOUBLE_BEEP);
    until (CylInClaw)
    {
      Direction = 1 - Direction;
      OnFwd (Both_Motors);
      if (Direction == Right)
      {
        SetPower(L_Motor,1);
        SetPower(R_Motor,7);
      }
      else
      {
        SetPower(R_Motor,1);
        SetPower(L_Motor,7);
      }
      ClearTimer (2);
      TimeOut = false;
      until (Laser)
      {
        if (FastTimer(2) > MaxRecenterTime)
        {
          TimeOut = true;
          break;
        }
        if (CylInClaw)
        {
          break;
        }
      }
      if (TimeOut) break;
      while (Laser)
      {
        if (CylInClaw)
        {
          break;
        }
      }
    }
    if (CylInClaw)
    {
      Wait (20);
      Off (Both_Motors);
      RaiseArm();
      LowerArm();
    }
  }
}



sub RaiseArm ()
{
  SetPower (ArmMotor,7);
  OnFwd (ArmMotor);
  ClearTimer (1);
  until (RaisedArm)
  {
    if (FastTimer (1) > ArmTime + 50) break;
  }
  Wait(20);
  ArmTime = FastTimer (1);
  Off (ArmMotor);
}

sub LowerArm ()
{
  SetPower (ArmMotor,3);
  OnRev (ArmMotor);
  Wait(ArmTime*13/10);
  Off (ArmMotor);
  CylInClaw = false;
}


task CheckForCylinder ()
{
  int AverageLight;

  SetSensor (CylDetect, SENSOR_TOUCH);

  while (true)
  {
    if (CylDetect)
    {
      CylInClaw = true;
    }
  }
}

task CheckCylinderRemaining ()
{
  ClearTimer (2);

  while ( FastTimer (2) < MaxRotationTime );
  Off (Both_Motors);
  PlaySound (SOUND_DOWN);
  StopAllTasks ();
}