// puls sensor 3 // start
// motor a tot sensor 1 uit door tot sensor 1 aan
// motor b tot sensor 2 uit door tot sensor 2 aan
// ff wachten
// motor b terug tot sensor 2 uit door tot sensor 2 aan
// motor c tijdje aan
// ff wachten
// motor c tijdje aan
// motor a tot sensor 1 uit door tot sensor 1 aan

int a;
int b;
int c;

  sub init()
  {
  SetSensor(SENSOR_1,SENSOR_TOUCH);
  SetSensor(SENSOR_2,SENSOR_TOUCH);
  SetSensor(SENSOR_3,SENSOR_TOUCH);
  a=0;
  b=0;
  c=0;
  return;
  }

  sub wagen_heen()
  {
  a=1;
  OnFwd(OUT_A);
  while (SENSOR_1==1);
  while (SENSOR_1==0);
  Wait(20);
  a=0;
  Off(OUT_A);
  return;
  }
  
  sub wagen_terug()
  {
  a=-1;
  OnRev(OUT_A);
  while (SENSOR_1==1);
  while (SENSOR_1==0);
  a=0;
  Off(OUT_A);
  return;
  }

  sub pers_in()
  {
  b=1;
  OnFwd(OUT_B);
  while (SENSOR_2==1);
  while (SENSOR_2==0);
  Wait(30);
  b=0;
  Off(OUT_B);
  return;
  }

  sub pers_reset()
  {
  b=-1;
  OnRev(OUT_B);
  while (SENSOR_2==0);
  b=0;
  Off(OUT_B);
  return;
  }

  sub pers_uit()
  {
  b=-1;
  OnRev(OUT_B);
  while (SENSOR_2==1);
  while (SENSOR_2==0);
  b=0;
  Off(OUT_B);
  return;
  }

  sub wiel_in()
  {
  c=1;
  OnFwd(OUT_C);
  Wait(300);
  c=0;
  Off(OUT_C);
  return;
  }

  sub wiel_uit()
  {
  c=-1;
  OnRev(OUT_C);
  Wait(300);
  c=0;
  Off(OUT_C);
  return;
  }

  task hoho()
  {
  SetSensor(SENSOR_3,SENSOR_TOUCH);
  while (true)
  {
  
  while (SENSOR_3==1)
  {
  if (a!=0) Off(OUT_A);
  if (b!=0) Off(OUT_B);
  if (c!=0) Off(OUT_C);
  }

  if (a==1)  OnFwd(OUT_A);
  if (a==-1) OnRev(OUT_A);

  if (b==1) OnFwd(OUT_B);
  if (b==-1) OnRev(OUT_B);

  if (c==1)  OnFwd(OUT_C);
  if (c==-1) OnRev(OUT_C);
  
  } //while forever
  } //hoho


  task main()
  {
   init();
   pers_reset();
   wiel_uit();

   while (true)
   {
   while (SENSOR_3==1);
   while (SENSOR_3==0);
//   start hoho;
   wagen_heen();
   pers_in();
   pers_uit();
   wagen_terug();
   wiel_in();
   wiel_uit();
   stop hoho;
   };
  }