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

  signed int servoPos, servoPos2;
  signed int quickPos2;
  int batteryVoltage;
  nI2CBytesReady[kSc8Port] = 0;
	int direction;
	int i;

	buttonValue = 0;
	direction = 1;

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

	while(1)
  {
		batteryVoltage = Get_Batt_V();
		nxtDisplayTextLine(2, "V batt= %d mV", batteryVoltage);

	  if ( servoPos >= 2500 ) {
		  direction = -1; // move towards right
		}
		if ( servoPos <= 500 ) {
		  direction = 1;  // move torwards left
		}
		servoPos += direction * 20;
		nxtDisplayTextLine(3, "Servo = %d uS", servoPos);
		NXTServo_SetPosition(1, servoPos);

/*
	  if ( quickPos1 >= 255 ) {direction = -1;}
    if ( quickPos1 <= 0 ) {direction = 1;}
    quickPos1 += direction*5;
    if ( quickPos1 < 0 ) quickPos1 = 0;
		if ( quickPos1 > 255 ) quickPos1 = 255;
    nxtDisplayTextLine(4, "Qpos1 = %d", quickPos1);
    NXTServo_Quick_Servo_Setup(1, quickPos1);
*/

		wait1Msec(2);

	 /*
	  * based on the button pressed move the servo2 to
		* new position
		*/

    nxtDisplayTextLine(5, "button = %d", buttonValue);
    servoPos2 = 1500 + (buttonValue * 100);
 	  NXTServo_SetPosition(2, servoPos2);

 	  for ( i = 3; i<=8; i++) {
 	    NXTServo_SetPosition(i, servoPos2);
 	  }

    quickPos2 = 127 + buttonValue*5;
		if ( quickPos2 < 0 ) quickPos2 = 0;
		if ( quickPos2 > 255 ) quickPos2 = 255;
    nxtDisplayTextLine(6, "QPos2 = %d", quickPos2);
//    NXTServo_Quick_Servo_Setup(2, quickPos2);
 }

 StopAllTasks();
}