/* the tube sorting software for Ria... */

#define DRIVE   OUT_C
#define GRAB    OUT_A

#define STOP_T  SENSOR_1

/* The dropper sensor */

#define SORT_L  SENSOR_3

/* the grab light sensor */

#define GREEN    45
#define YELLOW   55

/* value of a kinder-egg tube wrapped in either yellow or green duck paper. This allows me
   to use the indeterminate value as "no tube" */

/* Ria has a drive motor on the "rear" wheel, now geared six to one with an
   idler gear and layshaft.  The "grab" is at the same end. After trying
   a number of gear mechanisms I eventually decided on simplicity and retained
   the band drive from QuAD. */

task main()

{
SetSensor (STOP_T , SENSOR_TOUCH);
SetSensor (SORT_L , SENSOR_LIGHT);

SetPower (DRIVE , 1);
SetPower (GRAB , 1);

Fwd (DRIVE);

while (true)
   {
   On (DRIVE);
   Wait (50);
   while (true)
     {
     if (STOP_T == 1) break;
     }
   Wait (50);

   if (STOP_T == 1) break;
   // we are at the terminus, but have no tube...
   else if (SORT_L < GREEN)
      {
      get_tube ();
      OnFwd (DRIVE);
      move_end ();
      }
   else if (SORT_L > YELLOW)
      {
      get_tube ();
      OnRev (DRIVE);
      move_end ();
      }
   // else move right along...
   }      

Off (DRIVE);

}

sub get_tube ()

{
OnRev (GRAB);
Wait (30);
Off (GRAB);
}

sub move_end ()

{
while (true)
   {
   while (true)
         {
         if (STOP_T == 1) break;
         }
   Wait (50);

   if (STOP_T == 1) break;
   // still on a stop, so must be at the end...
   }

Off (DRIVE);
// stop, then lower the grab, releasing the tube...
OnFwd (GRAB);
Wait (30);
Off (GRAB);

// leave the grab !!down!!

}