/*
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"

int buttonValue;

task button_handler()
{
	while (true) {
		while (nNxtButtonPressed == -1){
			wait10Msec(10); // don't hog the cpu.
		}
		if (nNxtButtonPressed == 2) {
				buttonValue--;
		} else if (nNxtButtonPressed == 1) {
				buttonValue++;
		}
		while (nNxtButtonPressed != -1) {
			wait10Msec(10); // don't hog the cpu.
	  }
	}
}

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

task main()
{

  int batteryVoltage;
  nI2CBytesReady[kSc8Port] = 0;
	int pos;

	buttonValue = 0;

	// 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.
	 */
		batteryVoltage = Get_Batt_V();
		nxtDisplayTextLine(2, "V batt= %d mV", batteryVoltage);

	while(1)
  {
		pos = 1000;
		nxtDisplayTextLine(3, "Position: %d   ", pos);
		NXTServo_SetPosition(1, pos);
		wait10Msec(100);
		pos = 2000;
		nxtDisplayTextLine(3, "Position: %d ", pos);
		NXTServo_SetPosition(1, pos);
		wait10Msec(100);

		/*
		 * code below is same functionality as above
		 * - using different API.
		 */
		/*
		pos = 100;
		nxtDisplayTextLine(3, "Position: %d   ", pos);
    NXTServo_Quick_Servo_Setup(1, pos);
		wait10Msec(100);
		pos = 200;
		nxtDisplayTextLine(3, "Position: %d ", pos);
    NXTServo_Quick_Servo_Setup(1, pos);
		wait10Msec(100);
		*/
 }

 StopAllTasks();
}