/********************************************************
*
* drive.c	Drive R4
*
* Author:	Gary Hoggard	26 Apr 1998
*
* Change History:
*	01 Jan 1999	added 'a'rm commands
********************************************************/

#include "r4.h"

/* bits to turn motors on and off */
#define R4_STOP		0x00
#define R4_FORWARD	0x05
#define R4_REVERSE	0x0a
#define R4_LEFT_SPIN	0x06
#define R4_RIGHT_SPIN	0x09
#define R4_LEFT_FWD	0x04
#define R4_RIGHT_FWD	0x01
#define R4_LEFT_REV	0x08
#define R4_RIGHT_REV	0x02

main()
	{
	unsigned char r4;	/* byte to write to I/O card */
	char pipe[128];		/* pathname of named pipe */
	FILE *PIPE;		/* file pointer for named pipe */
	unsigned x,y,z;		/* 3D arm co-ords */
	char buffer[BUFSIZ];

	if (getenv("DEBUG"))
		{
		debug=YES;
		dlog = fopen(DEBUGFILE, "a");
		}

	/* setup I/O card */
	portinit();

	/* open the named pipe for reading */
	if (getenv("R4_LIB"))
		strcpy(pipe, getenv("R4_LIB"));
	else
		strcpy(pipe, LIB);
	strcat(pipe, "/drive_pipe");
	if ((PIPE = fopen(pipe, "r")) == NULL)
		{
		perror(pipe);
		exit(1);
		}

	while (YES)
		{
		if ((fgets(buffer, BUFSIZ, PIPE)) == 0)
			{
			fclose(PIPE);
			exit(0);
			}
		if (debug)
			{
			fprintf(dlog, "drive: read '%s' from drive_pipe\n", buffer);
			fflush(dlog);
			}
		if (*buffer == 'm')	/* 'm'ove commmands are to move R4 */
			switch (buffer[1])
				{
				case '0': r4 = R4_STOP;		break;
				case 'f': r4 = R4_FORWARD;	break;
				case 'b': r4 = R4_REVERSE;	break;
				case 'l': r4 = R4_LEFT_SPIN;	break;
				case 'r': r4 = R4_RIGHT_SPIN;	break;
				case '1': r4 = R4_LEFT_FWD;	break;
				case '2': r4 = R4_RIGHT_FWD;	break;
				case '3': r4 = R4_LEFT_REV;	break;
				case '4': r4 = R4_RIGHT_REV;	break;
				}
		else if (*buffer == 'a')	/* 'a'rm commands are to move the arm */
			{
			if (buffer[1] == 'p')
				{
				sscanf(&buffer[3], "%u %u %u", &x, &y, &z);
				/* move to 3D co-ord (x,y,z) */
				}
			else if (! strncmp(&buffer[1], "open", 4))
				{
				/* open the claw */ ;
				}
			else if (! strncmp(&buffer[1], "close", 5))
				{
				/* close the claw */ ;
				}
			}
		else
			continue;
		portout(IO_C, r4, 0, NO);
		}
	}