Neljästä askeleesta koostuva sekvenssiohjelma

#pragma config(Sensor, S1,     kosketus2,           sensorTouch)
#pragma config(Sensor, S2,     starttinappi,        sensorTouch)
#pragma config(Sensor, S3,     ultra,               sensorNone)
#pragma config(Sensor, S4,     RGB,                 sensorCOLORFULL)
#pragma config(Motor,  motorB,          motorB,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorC,        tmotorNormal, PIDControl, encoder)
//*!!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()
{
  while(1)//ikuinen silmukka alkaa tasta
	{
	    RobotinAjo();
	}//ikuinen silmukka loppuu tahan
}

void RobotinAjo()
{
  //esitellaan muuttuja static maareella, jotta funktiossa sailyy edelliskerran arvo
  static int askel;//0 =odotusaskel 1 = aja mustalle, 2 = aja valkoiselle 3= kaanny teipin paassa
  if(askel==0)//odotellaan starttausta
  {
     ClearTimer(T1);
	   eraseDisplay();
     nxtDisplayBigStringAt(0, 31,"odotellaan");
	   motor[motorC] = 0;
	   motor[motorB] = 0;
	   if(1==OnkoRobottiStartattuKosketusanturista())//testataan koko ajan kayttajan halua ajaa robottia
	   {
	     askel=1;
	     ClearTimer(T1);
		   eraseDisplay();
	     nxtDisplayBigStringAt(0, 31,"a1 valkoinen");
		   motor[motorC] = 0;
		   motor[motorB] = 80;
	   }
  }
  if(askel==1)//ollaan valkoisella ja yritetaan mustalle
  {
		if(SensorValue(RGB) == 1)
		{
		  askel=2;
		  ClearTimer(T1);
	    eraseDisplay();
	    nxtDisplayBigStringAt(0, 31,"a2 musta");
		  motor[motorC] = 80;
		  motor[motorB] = 0;
		}
		if(0==OnkoRobottiStartattuKosketusanturista()){askel=0;}
	  if(time1[T1]>500)//ei loytynyt mustaa 0,5 sekunnissa
	  {
	    eraseDisplay();
	    nxtDisplayBigStringAt(0, 31,"a3 kaanto");
	    askel=3;
	  }
	}
	if(askel==2)//ollaan mustalla ja yritetaan valkoiselle
	{
	  if(0==OnkoRobottiStartattuKosketusanturista()){askel=0;}
		if(SensorValue(RGB) != 1) //Poistuttiinko mustalta alueella
		{
		  askel=0;//kun musta teippi loytyi niin vaihdetaan takaisin askeleelle yksi
		  ClearTimer(T1); // nollataan ajastin seuraavaa suoraan ajoa varten
		  motor[motorC] = 0;
		  motor[motorB] = 80;
		}
	}
	if(askel==3)//kaannytaan paikoillaan ympari
	{
		motor[motorC] = -40;
		motor[motorB] = 40;//moottori peruuttaa
		if(SensorValue(RGB) == 1) //BLACKCOLOR) tultiinko takaisin mustalle alueella
		{
		  askel=1;//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(starttinappi);
    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;
}