Askeleista koostuva ohjelma aliohjelmana

#pragma config(Sensor, S4,     RGB,                 sensorCOLORFULL)
#pragma config(Sensor, S2,     kosketus1,           sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//esitellaan funktio, jolla kysellaan halusta ajella robotilla
int OnkoRobottiStartattuKosketusanturista();
void RobotinAjo();

//paaohjelma, jonka nimi on aina main
task main()
{
  ClearTimer(T1);
    while(1)//ikuinen silmukka alkaa tasta
    {
      //wait1Msec(1);//odotellaan vahan aikaa, talla ei taida olla mitaan merkitysta
      if(1==OnkoRobottiStartattuKosketusanturista())//testataan koko ajan kayttajan halua ajaa robottia
      {
        RobotinAjo();
      }
      else//tanne tullaan kun flipflop==0 ja pysaytetaan robotti, eli jos painetaan toisen kerran kosketusanturia
      {
          motor[motorC] = 0;
            motor[motorB] = 0;
      }
    }//ikuinen silmukka loppuu tahan
}

void RobotinAjo()
{
  static bool viimeksiAjettiinValkoisellaAlueella;
  static bool viimeksiAjettiinMustallaAlueella;
  static int askel;//0 = aja suoraan, 1 = kaanny teipin paassa, staattiset muuttujat alustuvat ohjelman käynnistyessä automaattisesti nolliksi  
  //siksi askeleet alkavat nollasta eivätkä ykkosesta
   if(askel==0)
  {
        if(SensorValue(RGB)== 6) //WHITECOLOR) valkoinen havaittu, yritetaan takaisin mustalle
            {
              if(viimeksiAjettiinMustallaAlueella==true)
              {
                ClearTimer(T1);
              }
                viimeksiAjettiinMustallaAlueella=false;//nollataan toisen suunnan laskuri
                viimeksiAjettiinValkoisellaAlueella=true;
                  motor[motorC] = 0;
                  motor[motorB] = 80;
            }
            if(SensorValue(RGB) == 1) //BLACKCOLOR) musta havaittu, yritetaan takaisin valkoiselle
            {
              if(viimeksiAjettiinValkoisellaAlueella==true)
              {
                ClearTimer(T1);
              }
                viimeksiAjettiinValkoisellaAlueella=false;//nollataan toisen suunnan laskuri
                  viimeksiAjettiinMustallaAlueella=true;
                  motor[motorC] = 80;
                  motor[motorB] = 0;
            }
            //testataan: paattyiko teippi eli jos maaraaikaan mennessa ei loytynyt teipin paa niin vaihdetaan askeleelle kaksi eli kaannytaan takaisin
            if(time1[T1]>500)
            {
              viimeksiAjettiinValkoisellaAlueella=false;
              viimeksiAjettiinMustallaAlueella=false;
              askel=1;
              motor[motorC] = -40;
                motor[motorB] = 40;//moottori peruuttaa
            }
        }
        if(askel==1)//kaannytaan paikoillaan ympari
        {
            if(SensorValue(RGB) == 1) //BLACKCOLOR) tultiinko takaisin mustalle alueella
            {
              askel=0;//kun musta teippi loytyi niin vaihdetaan takaisin askeleelle yksi
              ClearTimer(T1); // nollataan ajastin seuraavaa suoraan ajoa varten
            }
        }
}

//Talla funktiolla tutkitqaan takimmaisen kosketusanturin toimintaa
//joka toisella panalluksella funktio palauttaa nollan ja robotin pitaisi pysahtya
int OnkoRobottiStartattuKosketusanturista()
{
    static int flipflop;//esitellaan muuttuja staattisena, jotta se muistaa edellisen kerran tilansa
    static int flipAskel;//0=odotellaan napin painallusta 1=nappi painettu alas 2=nappi paastetty ylos 3=nappi toisen kerran alas
    int kosketus2Value=0;
    kosketus2Value = SensorValue(kosketus1);
    if(kosketus2Value==1&&flipAskel==0)//eka painallus alas
    {
      flipflop=1;
      flipAskel=1;
    }
    if(kosketus2Value==0&&flipflop==1)//eka paasto
    {
      flipAskel=2;
    }
    if(kosketus2Value==1&&flipAskel==2)//toka kerta alas
    {
      flipflop=0;
      flipAskel=3;
    }
    if(kosketus2Value==0&&flipAskel==3)//toka kerta ylos
    {
      flipAskel=0;
    }
    return flipflop;
}