/*	PONTECH Copyright 1996
 *	Date: 1/23/1996
 *  SV_LIB.C
 *  	SV200 move functions use by all the examples
 *
 *  Global:
 *		MasterDelay - Delay (in ms) after each move
 *  Functions:
 * 		Move (Servo, Postion)
 *			i.e.  move(3, 146);
 *						   move servo #3 to postion 146
 * 		MoveAll (S1Pos, S2Pos, S3Pos, S4Pos, S5Pos, S6Pos, S7P7s, S8Pos)
 *			i.e.  moveAll(10,10,NC,NC,NC,NC,NC,10);
 *						   move servo #1, #2 & #8 to postion 10
 *							 note: NC is a macro for No Change
 */

#include <stdio.h>
#include <stdlib.h>
#include <dos.h>
#include "comm.h"
#include "SV_LIB.h"

#define rcvWait 10    /* 9600 baud */
											/* increase to 30 if use 2400 */
											/* increase to 20 if use 4800 */
											/* decrease to 6 if use 19200 */

void svCmmd(char *cmmd, int param)
{
	serlXmitStr(cmmd);
	itoa(param, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
}

void svCmmd2(char *cmmd, int param1, int param2)
{
	serlXmitStr(cmmd);
	itoa(param1, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr(" ");
	itoa(param2, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
}

void selectBoard(int board)
{
	serlXmitStr("BD");
	itoa(board, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
}

void selectServo(int servo)
{
	serlXmitStr("SV");
	itoa(servo, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
	svSelect = servo;
}

void moveSelect(int position)
{
	serlXmitStr("M");
	itoa(position, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
	savePosition[svSelect] = position;

	delay(MasterDelay);
}

void move(int servo, int position)
{
	serlXmitStr("SV");
	itoa(servo, numStr, 10);
	serlXmitStr(numStr);

	serlXmitStr("M");
	itoa(position, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
	savePosition[servo-1] = servo;

	delay(MasterDelay);
}

void moveAll(int S1, int S2, int S3, int S4, int S5, int S6, int S7, int S8)
{
	if ((S1 >= 0) && (S1 <= 255))
	{
		serlXmitStr("SV1M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[0] = S1;
	}
	if ((S2 >= 0) && (S2 <= 255))
	{
		serlXmitStr("SV2M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[1] = S2;
	}
	if ((S3 >= 0) && (S3 <= 255))
	{
		serlXmitStr("SV3M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[2] = S3;
	}
	if ((S4 >= 0) && (S4 <= 255))
	{
		serlXmitStr("SV4M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[3] = S4;
	}
	if ((S5 >= 0) && (S5 <= 255))
	{
		serlXmitStr("SV5M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[4] = S5;
	}
	if ((S6 >= 0) && (S6 <= 255))
	{
		serlXmitStr("SV6M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[5] = S6;
	}
	if ((S7 >= 0) && (S7 <= 255))
	{
		serlXmitStr("SV7M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[6] = S7;
	}
	if ((S8 >= 0) && (S8 <= 255))
	{
		serlXmitStr("SV8M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[7] = S8;
	}

	serlXmitStr("\n");
	delay(MasterDelay);
}

void slowMove(int servo, int position,int speed)
{
	int i, change;

	serlXmitStr("SV");
	itoa(servo, numStr, 10);
	serlXmitStr(numStr);

	change = savePosition[servo-1] - position;
	if (change > 0)
	{
		for (i = 0; i < change; i++)
		{
			serlXmitStr("I-1\n");
			delay(speed);
		}
	}
	else
	{
		for (i = change; i < 0; i++)
		{
			serlXmitStr("I1\n");
			delay(speed);
		}
	}

	savePosition[servo-1] = position;

	delay(MasterDelay);
}

int getNumFromSV200(void)
{
	char ch, tempStr[10];
	int i=0;

	while(serlAvl())
	{
		ch = serlRcv();
		if (ch == 13)
			tempStr[i++] = '\0';
		else
			tempStr[i++] = ch;
	}
	return atoi(tempStr);
}

unsigned char getAD(int channel)
{
	serlXmitStr("AD");
	itoa(channel, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
	delay(rcvWait);

	return (unsigned char) getNumFromSV200();
}

int adjADJoystick(int position)
{
	return (((position-128) * 2) + 128);
}