/************************************************************************/
/*                                                                      */
/* Program Name: NXTServo-test.nxc                                      */
/* ===========================                                          */
/*                                                                      */
/* Copyright (c) 2008 by mindsensors.com                                */
/* Email: info (<at>) mindsensors (<dot>) com                           */
/*                                                                      */
/* This program is free software. You can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published by */
/* the Free Software Foundation; version 3 of the License.              */
/* Read the license at: http://www.gnu.org/licenses/gpl.txt             */
/*                                                                      */
/************************************************************************/

/*
 * History
 * ------------------------------------------------
 * Author     Date      Comments
 * Deepak     08/27/08  Initial Authoring.
 */

#include "NXTServo-lib.nxc"

#define SensorPort S1
const byte ServoAddr = 0xb0;

task main()           // main task:
{

  string msg, v;
  int i;
	int voltage;

  SetSensorLowspeed(SensorPort);

	while (true) {
		ShowSensorInfo(ServoAddr, SensorPort);

		/*
		 *  display battery voltage on LCD.
		 */
		msg = "v:             ";
		voltage = NXTServo_GetBattVoltage(SensorPort, ServoAddr);
		v = NumToStr(voltage);
		msg = StrReplace(msg, 3, v);
		TextOut(0, LCD_LINE4, msg, false);

		/*
		 * move all motors to one side.
		 * wait for short while and move them
		 * all to other side.
		 */
		for ( i=1; i <= 8; i++) {
			NXTServo_SetSpeed(SensorPort, ServoAddr, i, 100);
			NXTServo_Quick_Servo_Setup(SensorPort, ServoAddr, i, 50);
		}
		Wait(800);
		for ( i=1; i <= 8; i++) {
			NXTServo_Quick_Servo_Setup(SensorPort, ServoAddr, i, 200);
		}
		Wait(800);
	}
	/*
  NXTServo_Quick_Servo_Setup(SensorPort, ServoAddr, 4, 50);
  Wait(3000);
  NXTServo_Quick_Servo_Setup(SensorPort, ServoAddr, 4, 200);
	*/

  i = 0;
  while (i == 0 ) {
    if ( ButtonPressed(BTNCENTER, true) ) {
      i = 1;
    }
  }

  Wait(100);

}