/*
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()
{

  signed int servoPos;
  int batteryVoltage;
  nI2CBytesReady[kSc8Port] = 0;

  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.
	 */
  servoPos=1500;

	while(1)
  {
    while(servoPos<2500)
    {
		 /*
		  * Pull the servo to left extreme
		  * position in increments of 10mS.
		  */
      batteryVoltage = Get_Batt_V();
      nxtDisplayTextLine(2, "V batt= %d mV", batteryVoltage);
      NXTServo_SetPosition(1, servoPos);
      nxtDisplayTextLine(3, "Servo = %d uS", servoPos);
      wait1Msec(3);
      servoPos  += 10;
    }
    while(servoPos>500)
    {
		 /*
		  * Pull the servo to right extreme
		  * position in increments of 10mS.
		  */
      batteryVoltage = Get_Batt_V();
      nxtDisplayTextLine(2, "V batt= %d mV", batteryVoltage);
      NXTServo_SetPosition(1, servoPos);
      nxtDisplayTextLine(3, "Servo = %d uS", servoPos);
      wait1Msec(3);
      servoPos -= 10;
    }
 }

 StopAllTasks();
}