#include "digitalout.h"
// newtest.c
// created Aug 5, 2008
#include "config.h"
#include "digitalin.h"
#include "appled.h"
#include "servo.h"

void mmmclock( int id );
void MyTask( void* p );
int IRLeft,IRCenter,IRRight,abclk;
int ServPos=600;

void Run( ) // this task gets called as soon as we boot up.
{
  TaskCreate( MyTask, "Me", 1500, 0, 6 );
}
  
void MyTask( void* p )
{
  (void)p;
	Servo_SetActive (1,1);
	FastTimer_SetActive (true);
	FastTimerEntry mmmtimer; // our microsecond clock timer  
			
	FastTimer_InitializeEntry( &mmmtimer, mmmclock, 1, 1000, true );
	FastTimer_Set (&mmmtimer);
	int direction = 0;


while( true )
  {
	if (direction == 1)				//oscillate servo
		{ServPos = ServPos + 20;}	
	else
		{ServPos = ServPos - 20;}
			
	if (ServPos >= 1320)				//set max range
		{
		ServPos = 1320;
		direction = 0;
		}
	if (ServPos <= -480)				//set min range
		{
		ServPos = -480;
		direction = 1;
		}
}
}

void mmmclock( int id )

{
  (void)id;
	if (abclk == 500 )
		{
		abclk = 0;
}
	abclk = abclk + 1;

Servo_SetPosition (1,ServPos);
}