#include <3067F.H>
#include "2l32a.h"

/**/
void action_decode(struct ACTION *action, struct SERVO_DATA *servo_data)
{
unsigned char	flg1;

	flg1 = 0;

	/******************************************************/
	/*直進　→　停止*/
	if(action->id0 == 0 && action->id1 == 1)
	{
		if( servo_data->motion_id1 == 0 || servo_data->motion_id1 == 4)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 3 || servo_data->motion_id1 == 2)/*最後の一歩*/
			servo_data->motion_id0 = 4;

		flg1 = 1;
	}

	/*直進　低速*/
	if(action->id0 == 1)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
		}
		if(action->id1 == 1)
		{
			if(servo_data->motion_id1 == 0)/*始めの一歩*/
					servo_data->motion_id0 = 3;
			if(servo_data->motion_id1 == 3 || servo_data->motion_id1 == 2)/*定常歩行*/
					servo_data->motion_id0 = 2;
		}
		action->id1 = 1;
		flg1 = 1;
	}

	/******************************************************/
	/*後進　→　停止*/
	if(action->id0 == 0 && action->id1 == 2)
	{
		if( servo_data->motion_id1 == 0 || servo_data->motion_id1 == 16)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 15 || servo_data->motion_id1 == 14)/*最後の一歩*/
			servo_data->motion_id0 = 16;

		flg1 = 1;
	}

	/*後進　低速*/
	if(action->id0 == 2)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
		}
		if(action->id1 == 2)
		{
			if(servo_data->motion_id1 == 0)/*始めの一歩*/
					servo_data->motion_id0 = 15;
			if(servo_data->motion_id1 == 15 || servo_data->motion_id1 == 14)/*定常歩行*/
					servo_data->motion_id0 = 14;
		}
		action->id1 = 2;
		flg1 = 1;
	}

	/******************************************************/
	/*右横進　停止*/
	if(action->id0 == 0 && action->id1 == 3)
	{
		if( servo_data->motion_id1 == 0 || servo_data->motion_id1 == 7)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 6 || servo_data->motion_id1 == 5)/*最後の一歩*/
			servo_data->motion_id0 = 7;

		servo_data->gyro_mode[0] = 1;
		servo_data->gyro_mode[1] = 1;
		flg1 = 1;
	}

	/*右横進*/
	if(action->id0 == 3)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
			servo_data->gyro_mode[0] = 1;
			servo_data->gyro_mode[1] = 1;
		}
		if(action->id1 == 3)
		{
			if(servo_data->motion_id1 == 0)/*始めの一歩*/
					servo_data->motion_id0 = 6;
			if(servo_data->motion_id1 == 6 || servo_data->motion_id1 == 5)/*定常歩行*/
					servo_data->motion_id0 = 5;
			servo_data->gyro_mode[0] = 0;
			servo_data->gyro_mode[1] = 1;
		}
		action->id1 = 3;
		flg1 = 1;
	}
	/******************************************************/
	/*左横進　停止*/
	if(action->id0 == 0 && action->id1 == 4)
	{
		if( servo_data->motion_id1 == 0 || servo_data->motion_id1 == 10)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 9 || servo_data->motion_id1 == 8)/*最後の一歩*/
			servo_data->motion_id0 = 10;

		servo_data->gyro_mode[0] = 1;
		servo_data->gyro_mode[1] = 1;
		flg1 = 1;
	}

	/*左横進*/
	if(action->id0 == 4)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
			servo_data->gyro_mode[0] = 1;
			servo_data->gyro_mode[1] = 1;
		}
		if(action->id1 == 4)
		{
			if(servo_data->motion_id1 == 0)/*始めの一歩*/
					servo_data->motion_id0 = 9;
			if(servo_data->motion_id1 == 9 || servo_data->motion_id1 == 8)/*定常歩行*/
					servo_data->motion_id0 = 8;
			servo_data->gyro_mode[0] = 0;
			servo_data->gyro_mode[1] = 1;
		}
		action->id1 = 4;
		flg1 = 1;
	}
	/******************************************************/
	/*超信地旋回（右）停止*/
	if(action->id0 == 0 && action->id1 == 5)
	{
		if( servo_data->motion_id1 == 0 || servo_data->motion_id1 == 20)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 19)/*最後の一歩*/
			servo_data->motion_id0 = 20;

		flg1 = 1;
	}

	/*超信地旋回（右）*/
	if(action->id0 == 5)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
		}
		if(action->id1 == 5)
		{
			if(servo_data->motion_id1 == 0 || servo_data->motion_id1 == 20)/*始めの一歩 & 定常歩行*/
					servo_data->motion_id0 = 19;
			if(servo_data->motion_id1 == 19)/*定常歩行*/
					servo_data->motion_id0 = 20;
		}
		action->id1 = 5;
		flg1 = 1;
	}
	/******************************************************/
	/*超信地旋回（左）停止*/
	if(action->id0 == 0 && action->id1 == 6)
	{
		if( servo_data->motion_id1 == 0 || servo_data->motion_id1 == 18)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 17)/*最後の一歩*/
			servo_data->motion_id0 = 18;

		flg1 = 1;
	}

	/*超信地旋回（左）*/
	if(action->id0 == 6)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
		}
		if(action->id1 == 6)
		{
			if(servo_data->motion_id1 == 0 || servo_data->motion_id1 == 18)/*始めの一歩 & 定常歩行*/
					servo_data->motion_id0 = 17;
			if(servo_data->motion_id1 == 17)/*定常歩行*/
					servo_data->motion_id0 = 18;
		}
		action->id1 = 6;
		flg1 = 1;
	}
	/******************************************************/
	/*超信地旋回（右−速）停止*/
	if(action->id0 == 0 && action->id1 == 7)
	{
		if( servo_data->motion_id1 == 0 || servo_data->motion_id1 == 30)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 29)/*最後の一歩*/
			servo_data->motion_id0 = 30;

		flg1 = 1;
	}

	/*超信地旋回（右−速）*/
	if(action->id0 == 7)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
		}
		if(action->id1 == 7)
		{
			if(servo_data->motion_id1 == 0 || servo_data->motion_id1 == 30)/*始めの一歩 & 定常歩行*/
					servo_data->motion_id0 = 29;
			if(servo_data->motion_id1 == 29)/*定常歩行*/
					servo_data->motion_id0 = 30;
		}
		action->id1 = 7;
		flg1 = 1;
	}
	/******************************************************/
	/*超信地旋回（左−速）停止*/
	if(action->id0 == 0 && action->id1 == 8)
	{
		if( servo_data->motion_id1 == 0 || servo_data->motion_id1 == 28)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 27)/*最後の一歩*/
			servo_data->motion_id0 = 28;

		flg1 = 1;
	}

	/*超信地旋回（左−速）*/
	if(action->id0 == 8)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
		}
		if(action->id1 == 8)
		{
			if(servo_data->motion_id1 == 0 || servo_data->motion_id1 == 28)/*始めの一歩 & 定常歩行*/
					servo_data->motion_id0 = 27;
			if(servo_data->motion_id1 == 27)/*定常歩行*/
					servo_data->motion_id0 = 28;
		}
		action->id1 = 8;
		flg1 = 1;
	}
	/******************************************************/
	/*直進（高速）停止*/
	if(action->id0 == 0 && action->id1 == 9)
	{
		if( servo_data->motion_id1 == 0 || servo_data->motion_id1 == 13)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 12 || servo_data->motion_id1 == 11)/*最後の一歩*/
			servo_data->motion_id0 = 13;

		flg1 = 1;
	}

	/*直進　高速*/
	if(action->id0 == 9)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
		}
		if(action->id1 == 9)
		{
			if(servo_data->motion_id1 == 0)/*始めの一歩*/
					servo_data->motion_id0 = 12;
			if(servo_data->motion_id1 == 12 || servo_data->motion_id1 == 11)/*定常歩行*/
					servo_data->motion_id0 = 11;
		}
		action->id1 = 9;
		flg1 = 1;
	}
	/******************************************************/
	/* id 10 後進（高速）*/
	/******************************************************/
	/* id 11 前進（低速　１／２速度）*/
	if(action->id0 == 0 && action->id1 == 11)
	{
		if( servo_data->motion_id1 == 0 
			|| servo_data->motion_id1 == 26 || servo_data->motion_id1 == 25)/*立つ*/
		{
			servo_data->motion_id0 = 1;
			action->id1 = 0;
		}

		if(servo_data->motion_id1 == 21 || servo_data->motion_id1 == 23)/*最後の一歩*/
			servo_data->motion_id0 = 26;

		if(servo_data->motion_id1 == 22 || servo_data->motion_id1 == 24)/*最後の一歩*/
			servo_data->motion_id0 = 25;

		flg1 = 1;
	}

	/*直進　低速*/
	if(action->id0 == 11)
	{
		if(action->id1 == 0)/*すわる*/
		{
			servo_data->motion_id0 = 0;
		}
		if(action->id1 == 11)
		{
			if(servo_data->motion_id1 == 0)/*始めの一歩*/
					servo_data->motion_id0 = 21;
			if(servo_data->motion_id1 == 21 || servo_data->motion_id1 == 23)/*定常歩行*/
					servo_data->motion_id0 = 24;
			if(servo_data->motion_id1 == 22 || servo_data->motion_id1 == 24)/*定常歩行*/
					servo_data->motion_id0 = 23;
		}
		action->id1 = 11;
		flg1 = 1;
	}
	/******************************************************/
	/* id 12 後進（低速　１／２速度）*/
	/******************************************************/
	/******************************************************/
	/*モーションセット*/
	if(servo_data->wave_datnum[servo_data->motion_id0] != 0 && flg1 == 1)
	{
		servo_data->ram1 = servo_data->wave_ptr[servo_data->motion_id0];
		servo_data->wave_play = servo_data->wave_datnum[servo_data->motion_id0] - 1;

		if(action->id0 == 3 || action->id0 == 4)/*横進のとき*/
		{
			servo_data->gyro_mode[0] = 0;
			servo_data->gyro_mode[1] = 1;
		}
		else
		{
			servo_data->gyro_mode[0] = 1;
			servo_data->gyro_mode[1] = 1;
		}

		servo_data->motion_id1 = servo_data->motion_id0;
	}
	else
		switch_LED(3);/*赤*/
}