#include "RCX_RTE.h"
#include "OutputPorts.h"

/* Buttons */
#define port4_addr  0xffb7
#define port4       *((volatile byte *) port4_addr)

#define OnOff       ! ( port4 & ( 1 << 1 ) )
#define msCount     500

void delay(int16 ms) 
{
  int16 i;
  while( ms > 0){
    for(i=0; i<msCount; i++);
     ms--;
  }
}

void _start(void) 
{ 
  int16 i; int16 t;
  i=0; t=0;
  lcd_show_icon(LCD_WALKING);
  
  while ( ! OnOff ) {
    delay(1000);
    i++;t++;
    if (t==3) {t=0;}
    if (i==4) {i=0;}
    if (i==0 || i==2)
      {PortA(OnPos);} else
	{PortA(Float);}
    if (t==0) {PortB(OnPos);} else
      if (t==2) {PortB(Float);}
    if (i==0) {PortC(OnPos);} else
      PortC(Float);
  }
  
  RCX_Reset();
}