/************************************************************************/
/*                                                                      */
/* Program Name: NXTServo-disp-macro.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     11/10/08  Initial Authoring.
 */


#define PORT S1
#include "NXTServo-lib.nxc"
const byte ServoAddr = 0xb0;

void NXTServo_WriteMyMacro (byte port)
{

  byte mDataAt0x21[];
  byte cmdbuf[];
  byte nByteReady = 0;
	byte loop, c;
	int  n, i, j, k, aLen;
	string msg, x;

	ArrayBuild(mDataAt0x21,

/* =========================================================
Macro below moves servo motors 1 throu 4 one after the other
Use this macro as an example. And modify it to suit your needs.
=======================================================*/
/* 0x21 */ 'J', '3', '0',                // jump to location 0x30
/* 0x24 */ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
/* 0x30 */ 'S', '1', '0', '2', 'E', 'E', // set servo 1 to position 0x02EE(750)
/* 0x36 */ 'W', '0', '3', 'E', '8',      // wait 1000ms
/* 0x3B */ 'S', '2', '0', '2', 'E', 'E', // set servo 2 to position 0x02EE(750)
/* 0x41 */ 'W', '0', '3', 'E', '8',      // wait 1000ms
/* 0x46 */ 'S', '3', '0', '2', 'E', 'E', // set servo 3 to position 0x02EE(750)
/* 0x4C */ 'W', '0', '3', 'E', '8',      // wait 1000ms
/* 0x51 */ 'S', '4', '0', '2', 'E', 'E', // set servo 4 to position 0x02EE(750)
/* 0x57 */ 'W', '0', '3', 'E', '8',      // wait 1000ms
/* 0x5C */ 'S', '1', '0', '8', 'F', 'F', // set servo 1 to position 0x08FF(2303)
/* 0x62 */ 'W', '0', '3', 'E', '8',      // wait 1000ms
/* 0x67 */ 'S', '2', '0', '8', 'F', 'F', // set servo 2 to position 0x08FF(2303)
/* 0x6D */ 'W', '0', '3', 'E', '8',      // wait 1000ms
/* 0x72 */ 'S', '3', '0', '8', 'F', 'F', // set servo 3 to position 0x08FF(2303)
/* 0x78 */ 'W', '0', '3', 'E', '8',      // wait 1000ms
/* 0x7D */ 'S', '4', '0', '8', 'F', 'F', // set servo 4 to position 0x08FF(2303)
/* 0x83 */ 'W', '0', '3', 'E', '8',      // wait 1000ms
/* 0x88 */ 'J', '3', '0',               // jump to 0x30
/* 0x8B */ 0x00

	);
	aLen = ArrayLen(mDataAt0x21);

	for ( i = 0; i < aLen; i++ ) {
		j = 0x21 + i;
		c = mDataAt0x21[i];
		ArrayBuild(cmdbuf, 0x40, j, c);
		loop = STAT_COMM_PENDING;
		while ( loop == STAT_COMM_PENDING ) {
			loop = I2CStatus(port, nByteReady);
		}
		n = I2CWrite(port, 1, cmdbuf);
		Wait(10);

		// display progress
		msg = "Writing loc:        .";
		x = NXTServo_format_hex(j);
		msg = StrReplace(msg, 13, x);
		TextOut(0, LCD_LINE6, msg, false);

	}

}

void NXTServo_ExitMacroEditMode(byte port) {
  byte cmdBuf[];
  ArrayBuild(cmdBuf, 0x40, 0x00, 'Q');
  I2CWrite(port, 0, cmdBuf);
  int status = I2CCheckStatus(port);
  while (status > NO_ERR)
    status = I2CCheckStatus(port);
  Wait(200);
}

task main()           // main task:
{
  byte Address=0x02;
  SetSensorLowspeed(PORT);

	ShowSensorInfo(PORT,0xb0);

	TextOut(0, LCD_LINE5, "changing mode....");
	TextOut(0, LCD_LINE7, "Press Btn.");
	until(ButtonPressed(BTNCENTER, true));
	Wait (200);
  NXTServo_SendCommand(PORT, 0xb0, 'E');
  NXTServo_SendCommand(PORT, 0xb0, 'M');

	TextOut(0, LCD_LINE5, "writing macro....");
  NXTServo_WriteMyMacro (PORT);
	TextOut(0, LCD_LINE7, "done.            ");

	NXTServo_ExitMacroEditMode(PORT);
  TextOut(0, LCD_LINE8, "press btn.", false);
  until(ButtonPressed(BTNCENTER, true));

}