Motion interpreter（ コード ）

//**** command : Execute sequence ****
// eN[enter]
// e : command / Execute
// N : Sequence No.
void procMIexecSq( String szInput )
{
	int		lenBuf	= szInput.length();
	String	azArg1	= szInput.substring(1, lenBuf);		// sequence numbern
	int		seqNo	= azArg1.toInt();

	startSqMS( seqNo );		// subfunction
}

//**** command : Move servomotor ****
// mCA[enter]
// m : command / Move
// C : extremities abbreviation
//     HD, AR, AL, LR, LL, FR, FL
// A : position / expressive angle
//     -x - +x, 0 - 180
void procMImoveSv( String szInput )
{
	struct MI_M_EXTERMITIES {
		String	abbr;
		int		svNo;
	};

	struct MI_M_EXTERMITIES miTable[] = {
		{ "hd", SV_HD },		// Head
		{ "ar", SV_AR },		// Arm Right
		{ "al", SV_AL },		// Arm Left
		{ "lr", SV_LR },		// Leg Right
		{ "ll", SV_LL },		// Leg Left
		{ "fr", SV_FR },		// Foot Right
		{ "fl", SV_FL }			// Foot Left
	};

	int	lenBuf		= szInput.length();
	String	azArg1	= szInput.substring(1, 3);		// extremities abbreviation
	String	azArg2	= szInput.substring(3, lenBuf);	// position / expressive angle
	int	nPos		= azArg2.toInt();

	int	svNo = 0;
	for ( int i = 0; i < 8; i++ ){
		if( azArg1.compareTo( miTable[i].abbr ) == 0 ){
			svNo = miTable[i].svNo;
		}
	}

	if( svNo != 0 ){
		setSVposition( svNo, nPos );
	}
}

//**** command : Set speed servomotor ****
// sL[enter]
// s : command / Set angle speed
// M : speed
//     1 - 10
void procMIsetSpeedSV( String szInput )
{
	int		lenBuf		= szInput.length();
	String	azArg1		= szInput.substring(1, lenBuf);	// sequence numbern
	float	speedDev	= azArg1.toInt();

	setSpeedSV( speedDev );
}

//**** command : Write sequence ****
// wXXX[enter]
// w : command / Write sequence
// X : Element of motions ( unique )
//     CAAW
//     C  : extremities abbreviation ( char )
//         HD, AR, AL, LR, LL, FR, FL
//     AA : position / expressive angle ( hex. )
//         -x - +x, 0 - 180
//     W  : wait time by next execute ( hex. )
// retun : TRUE = success / FALSE = error <<<add***
int procMIwriteSq( String szInput )
{
	char	buf[SV_SQ_WRITABLE_CMDMAX]; <<<add***
	char	tmp[3];
	int		nPos;

	// Elements of extermities and commands
	int miTableSq[] = {
		0,			// 0 : Dummy
		SV_AR,		// 1 : Arm Right
		SV_FR,		// 2 : Foot Right
		SV_LR,		// 3 : Leg Right
		SV_HD,		// 4 : Head
		SV_LL,		// 5 : Leg Left
		SV_FL,		// 6 : Foot Left
		SV_AL		// 7 : Arm Left
	};

	struct MI_W_COMMAND {
		char	com;	// command for communication
		int		mi;		// command for motion interpreter 
	};

	struct MI_W_COMMAND miTableSqCom[] = {
		{ 'F', SV_CMD_FINISH    },		// F : End of sequence
		{ 'L', SV_CMD_LOOP      },		// L : Loop next
		{ 'N', SV_CMD_NEXT      },		// N : Execute next command
		{ 'S', SV_CMD_SET_SPEED } 		// S : Set speed
	};

	int	lenBuf	= szInput.length();
	szInput.toCharArray ( buf, lenBuf );

	int	nNumChar = ( szInput.indexOf('/') - 1 ) / 4;
	if( nNumChar > SV_SQ_WRITABLE_CMDMAX ) return FALSE; <<<add***

	int j = 1;
	for( int i = 0; i < nNumChar; ++i ){
		
		//.... Target Servomotor ....
		if( buf[j] < '8' ){
			svSqWritable[i].svNo = buf[j] - '0';
		} else {
			for ( int k = 0; k < 4; ++k ){
				if( miTableSqCom[k].com == buf[j] ){
					svSqWritable[i].svNo = miTableSqCom[k].mi;
				}
			}
		}

		//.... Target Position ....
		int c1, c2, c;

		if( buf[j+1] <= '9' ){
			 c1 = buf[j+1] - '0';
		} else {
			 c1 = buf[j+1] - 55;
		}

		if( buf[j+2] <= '9' ){
			 c2 = buf[j+2] - '0';
		} else {
			 c2 = buf[j+2] - 55;
		}

		nPos = c1 * 16 + c2;

		if( !(	svSqWritable[i].svNo == SV_AR || 
				svSqWritable[i].svNo == SV_AL || 
				svSqWritable[i].svNo == SV_CMD_SET_SPEED
		)) {
			nPos -= 90;
		}

		svSqWritable[i].svPos = nPos;

		//.... Period by next motion ....
		if( buf[j+3] <= '9' ){
			 svSqWritable[i].period = buf[j+3] - '0';
		} else {
			 svSqWritable[i].period = buf[j+3] - 'F';
		}

		j+=4;
	}

	return TRUE; <<<add***
}