#include <inttypes.h>
#include "C3024_motion.h"
#include "C3024_io.h"
#include <avr/pgmspace.h>

int main(void)

{
	uint8_t count;
	uint8_t i,j,k;

	prog_uint8_t motorset[32] = {1,1,1,1,1,0,1,1,1,0,0,0,1,1,1,0,0,0,1,1,1,1,1,0};
	prog_uint8_t motor_zero[32] ={101,102,98,104,102,0,100,104,99,0,0,0,99,97,97,0,0,0,97,100,104,103,117,0};
//	prog_uint8_t move_data1[32] = { 10,50,100,50,100,0};
//	prog_uint8_t move_data2[32] = { 190,50,100,50,100,0};

	prog_uint8_t stdA[32] = { 100,76,145,93,100,100 };
	prog_uint8_t stdB[32] = { 100,30,80,100,100,100 };
	prog_uint8_t stdC[32] = { 100,30,80,100,100,100 };
	prog_uint8_t stdD[32] = { 100,76,145,93,100,100 };

	for(i=0;i<2;i++)
	{
		for(j=0;j<255;j++)
		{
			for(k=0;k<220;k++)
			{
//
			}
		}
	}
	

	c3024_init();

	motion_init();

	zero(G24, motor_zero);

	led1_off();

	ptp(1);
	servo_ctl(PTP_ALL_ON);

	dir(G6B,0x3F);
	dir(G6C,0x00);
	dir(G6D,0x16);
	dir(G6A,0x09);

	getmotorset(G24, motorset);

	speed(5);

	for (count = 0; count < 24; count++) motor(count);
	
	for(i=0;i<255;i++);
	for(i=0;i<255;i++);
	for(i=0;i<255;i++);
	move(G6A, stdA);
	for(i=0;i<255;i++);
	for(i=0;i<255;i++);
	for(i=0;i<255;i++);
	move(G6D, stdD);
	for(i=0;i<255;i++);
	for(i=0;i<255;i++);
	for(i=0;i<255;i++);
	move(G6B, stdB);
	for(i=0;i<255;i++);
	for(i=0;i<255;i++);
	for(i=0;i<255;i++);
	move(G6C, stdC);

	led1_on();

	while(1)
	{
		for(count=0;count<255;count++);
	}
}