/************************************************************************/
/*                                                                      */
/* Program Name: NXTServo-macro-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);

	/*
	 *  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);

  NXTServo_SendCommand(SensorPort, 0xb0, 'G');
  NXTServo_SendCommand(SensorPort, 0xb0, 0x30);

	Wait (200);
	TextOut(0, LCD_LINE6, "Press Btn.");
	until( ButtonPressed(BTNCENTER, true));

  NXTServo_SendCommand(SensorPort, 0xb0, 'P');

	Wait (200);
	TextOut(0, LCD_LINE6, "Press Btn.");
	until( ButtonPressed(BTNCENTER, true));

  Wait(100);

}