/*
This is sample program to use with mindsensors.com NXTServo module.
V1.0
2008(c) mindsensors.com
for more info visit www.mindsensors.com

This is a sample program to move Servo motor.
In a loop, it pulls servo from left to right

History:
When           Ahthor/Editor          Comments
02/15/08       Dr. Nitin Patil        Initial authoring of test program
03/01/08       Deepak Patil           modified to support recent changes.
*/

#include "NXTServo-lib.c"

/////////////////////////////////////////////////////////////////////////////
//
//   move the servo 1 and show battery voltage and position on NXT display
//
/////////////////////////////////////////////////////////////////////////////

task main()
{
  int batteryVoltage;
  nI2CBytesReady[kSc8Port] = 0;
	int i;

	// Setup button handler
//	nNxtButtonTask = -2;
//	StartTask(button_handler);

  nxtDisplayTextLine(0, "mindsensors.com");
  nxtDisplayTextLine(1, "NXTServo Module");
  SensorType[kSc8Port] = sensorI2CCustom9V;

	/*
	 * When the servoPos is set to 1500 mS the
	 * servo will be in neutral position.
	 *
	 * The extreme servo positions lie between
	 * 500 mS to 2500 mS on both sides of the neutral position.
	 */
	while(1)
  {
		batteryVoltage = Get_Batt_V();
		nxtDisplayTextLine(2, "V batt= %d mV", batteryVoltage);

		for ( i=1; i<=8; i++ ) {
				NXTServo_SetPosition(i, 500);
		}
		wait10Msec(100);
		for ( i=1; i<=8; i++ ) {
				NXTServo_SetPosition(i, 2500);
		}
		wait10Msec(100);
	}

  StopAllTasks();
}