#include <aCore.tea>
#include <aPrint.tea>
#include <aServo.tea>

void move(unsigned char p)
{
  aServo_SetReflex(0, 0xE400);
  aServo_SetAbsolute(0,p);
  asm
  {
    pushms	aPortSemaphore
    pops
  }
}

void main ()
{
  aServo_SetAbsolute(0,0);
  aCore_Sleep(5000);
  aServo_SetConfig(0, (char)(SRV_ENA | 4));
  while (1)
  {  
    aPrint_String("Back...\n");
    move((unsigned char)255);
    aPrint_String("and forth...\n");
    move((unsigned char)0);
  }
}