#include <eZ8.h>

// Servo Controller Data Comm

// Clock in data to async port
void clockin(void)
{
	PBOUT &=0xDF;		//clock low
	while((PBIN &0x10)==0x10);	// wait until ack received
	PBOUT |= 0x20;
	while((PBIN &0x10)==0);		//wait until ack is high
}

void servo_out(int mot,int com,int param)
{


char	motcom;
int	i;

	// select servo driver IC
	
	if(mot<9) PBOUT &= 0xFE;

	if(mot>=9)
		if(mot<17) 
			{
			PBOUT &=0xFD;
			mot=mot-8;
			}
	if(mot>=17)
		if(mot<25) 
			{
			PBOUT &=0xFB;
			mot=mot-16;
			}
	if(mot>= 25) 
		{
		PBOUT &=0xF7;
		mot=mot-24;
		}

	//merge command and motor number
	motcom=mot;
	motcom <<=4;		// transfer to high nibble
	motcom &= 0xF0;
	motcom = motcom | com; // add command

	for(i=1;i<9;++i)
	{
		if((param & 0x01)==1)
			PDOUT |= 0x01;
			else
			PDOUT &= 0xFE;
		param >>=1;
		clockin();

	}

	for(i=1;i<9;++i)
	{	
		if((motcom &0x01)==0x01) 
			PDOUT |= 0x01;
			else
			PDOUT &= 0xFE;

		motcom >>=1;
		clockin();
	}

		PBOUT |= 0x0F;
}

//// move to position ////

void	move(int mot,int pos,int speed)
{
	speed=speed+128;		//speed 129 minimum

	pos=(pos*100/90)+150;	// convert to degrees

	servo_out(mot,2,pos);
	servo_out(mot,1,speed);
}