// servo.c
// GPSNav
//
// Routines for driving servos over a parallel port.
//
// Copyright (C) 2004 Joshua Wise. All rights reserved.

#include <stdio.h>
#include <fcntl.h>
#include <sys/types.h>
#include <unistd.h>
#include <linux/ioctl.h>
#include <linux/ppdev.h>
#include <sched.h>
#include <time.h>
#include <sys/time.h>
#ifndef TEST_DRIVER
#  include "gpsnav.h"
#endif

#define ST_TRIM -10.0f
#define TH_TRIM 38.0f
#define LAPSE_TIME 20000

#define STEERING_SERVO	(1 << 0)
#define SPEED_SERVO	(1 << 1)
#define POWER_SERVO	(~(STEERING_SERVO | SPEED_SERVO))
static struct timeval last;

static float direction = 0;
static float speed = 0;
static int pfd;

void servo_turn(int xdirection)
{
	direction = xdirection + ST_TRIM;
}

void servo_speed(int xspeed)
{
	speed = -xspeed + TH_TRIM;
}

void servo_init()
{
	unsigned char data;
	struct sched_param p;
	
	p.sched_priority=5;
	
	if (sched_setscheduler(getpid(), SCHED_FIFO, &p))
		perror("servo_init: sched_setscheduler");

	pfd = open("/dev/parport0", O_RDWR);
	if (pfd < 0)
	{
		perror("servo_init: open");
		exit(1);				// This is an unrecoverable error.
	}
	
	if ((ioctl(pfd, PPEXCL) < 0) || (ioctl(pfd, PPCLAIM) < 0))
	{
		perror("servo_init: ioctl");
		close(pfd);
		exit(1);				// This is an unrecoverable error.
	}
	
	gettimeofday(&last, NULL);
	
	speed = direction = 0;
}

void servo_update()
{
	unsigned char data;
	float directionmsec;
	float speedmsec;
	struct timespec sleeptimer;
	struct timeval now;
	
	gettimeofday(&now, NULL);
	
	if ((now.tv_sec == last.tv_sec) && ((now.tv_usec - last.tv_usec) < LAPSE_TIME))
		return;
	if ((now.tv_sec == (last.tv_sec+1)) && (((now.tv_usec + 1000000) - last.tv_usec) < LAPSE_TIME))
		return;

	last = now;
	
	directionmsec = (direction + 127) / 127.0f * 0.5f + 1.0f;
	speedmsec     = (speed     + 127) / 127.0f * 0.4f + 1.0f;
	
	data = STEERING_SERVO | POWER_SERVO;
	if (ioctl(pfd, PPWDATA, &data) < 0)
	{
		perror("servo_update: ioctl");
		close(pfd);
		exit(1);
	}
	
	sleeptimer.tv_sec = 0;
	sleeptimer.tv_nsec = directionmsec * 1000000;
	nanosleep(&sleeptimer, NULL);
	

	data = SPEED_SERVO | POWER_SERVO;
	if (ioctl(pfd, PPWDATA, &data) < 0)
	{
		perror("servo_update: ioctl");
		close(pfd);
		exit(1);
	}
	
	sleeptimer.tv_sec = 0;
	sleeptimer.tv_nsec = speedmsec * 1000000;
	nanosleep(&sleeptimer, NULL);
	
	data = POWER_SERVO;
	if (ioctl(pfd, PPWDATA, &data) < 0)
	{
		perror("servo_update: ioctl");
		close(pfd);
		exit(1);
	}
}

#ifdef TEST_DRIVER
#include <stdio.h>
int main(int argc, char** argv)
{
	struct timeval seconds;
	struct timeval now;
	
	printf("Initializing servos... ");
	fflush(stdout);
	servo_init();
	printf("done\n");
	
	printf("Synchronizing seconds... ");
	fflush(stdout);
	gettimeofday(&seconds, NULL);
	gettimeofday(&now, NULL);
	while (now.tv_sec == seconds.tv_sec)
		gettimeofday(&now, NULL);
	printf("done\n");

	if (argc == 3)
	{
		int st, th;
		
		st = atoi(argv[1]);
		th = atoi(argv[2]);
		
		printf("Servo position: Steering: %d, Throttle: %d... ", st, th);
		fflush(stdout);
		servo_speed(th);
		servo_turn(st);
		seconds = now;
		while (now.tv_sec < (seconds.tv_sec+5))
		{
			gettimeofday(&now, NULL);
			servo_update();
		}
		printf("done\n");
	}
	else
	{	
		printf("Servo position: neutral... ");
		fflush(stdout);
		servo_turn(0);
		servo_speed(0);
		seconds = now;
		while (now.tv_sec == seconds.tv_sec)
		{
			gettimeofday(&now, NULL);
			servo_update();
		}
		printf("done\n");
		
		printf("Servo position: full left... ");
		fflush(stdout);
		servo_turn(-127);
		seconds = now;
		while (now.tv_sec == seconds.tv_sec)
		{
			gettimeofday(&now, NULL);
			servo_update();
		}
		printf("done\n");
		
		printf("Servo position: full right... ");
		fflush(stdout);
		servo_turn(127);
		seconds = now;
		while (now.tv_sec == seconds.tv_sec)
		{
			gettimeofday(&now, NULL);
			servo_update();
		}
		printf("done\n");
		
		printf("Servo position: full throttle... ");
		fflush(stdout);
		servo_turn(0);
		servo_speed(127);
		seconds = now;
		while (now.tv_sec == seconds.tv_sec)
		{
			gettimeofday(&now, NULL);
			servo_update();
		}
		printf("done\n");
		
		printf("Servo position: full reverse... ");
		fflush(stdout);
		servo_speed(-127);
		seconds = now;
		while (now.tv_sec == seconds.tv_sec)
		{
			gettimeofday(&now, NULL);
			servo_update();
		}
		printf("done\n");
		
		printf("Servo position: neutral... ");
		fflush(stdout);
		servo_speed(0);
		seconds = now;
		while (now.tv_sec == seconds.tv_sec)
		{
			gettimeofday(&now, NULL);
			servo_update();
		}
		printf("done\n");
	}
 	return 0;
}
#endif