// Use an RS232 port to control a RC servo
// For more details, see http://www.fareham.org/simple-servo.shtml
#include <termios.h>
#include <unistd.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

// Servo requires a +ve pulse of between 1 and 2 ms to set it's position
//
// Wiring, RS232 TX goes via a current limiting resistor to the servo control pin
// We use a pair of diode to limit the RS232 signal to the 5V and 0V rails. 
//
// 0V -----|>|----+
//                +
// 5V -----|<|----+
//                +   +----+
// Control -------+---+    +--- TX data from RS232
//                    +----+
//
// On a 9-pin RS232 connector: TX is Pin 2, 0V is Pin 5.
// To avoid handshaking issues, tied 7+8 together, then 1+4+6 together.
// 

// Set baud rate, and send a start bit + a number of 0 bits
static int send_one(char const *pszDevName,int baud_id,int bits)
{	struct termios options;
	char const cToSend=(0xff<<bits)&0xFF;
	int fd;
	int err;

	// Open the serial pport
	if ((fd = open(pszDevName, O_RDWR|O_NOCTTY)) == -1)
		return -1;

	// Setup options
	fcntl(fd, F_SETFL, 0);
	tcgetattr(fd, &options);
	cfsetispeed(&options, baud_id);
	cfsetospeed(&options, baud_id);
	cfmakeraw(&options);
	options.c_cflag |= (CLOCAL | CREAD);
	options.c_cflag |= CRTSCTS;
	// Set the options
	if (tcsetattr(fd, TCSANOW, &options) != 0)
		return -1;
	// sleep 10ms to let any glitches pass, also limits max rate to 100Hz
	usleep(10*1000);
	// Send the requested number of 0's, RS232 inverts of 0 = +ve, that we clip to 5V
	err=(write(fd,&cToSend,1)-1);
	// Close port
	close(fd);
	return err;
}

// Send a byte at the correct baud rate to select one of the possible positions.
static int set_pos(int pos)
{	static const struct
	{	int baud_id, bits;
	} sPos[8]=
	{	{ B4800, 3 },	// 0.83333 ms
		{ B4800, 4 },	// 1.04166 ms
		{ B4800, 5 },	// 1.25000 ms
		{ B4800, 6 },	// 1.45833 ms
		{ B4800, 7 },	// 1.66666 ms
		{ B4800, 8 },	// 1.87500 ms
		{ B2400, 4 },	// 2.08333 ms
		{ B2400, 5 }	// 2.50000 ms
	};
	if (pos<0 || pos>=8)
		return 8;
	return send_one("/dev/ttyS0",sPos[pos].baud_id,sPos[pos].bits);
}

// Simple test prog for above.
// Either run as root, or chmod 777 /dev/ttyS0 beforehand
int main(int argc,char *argv[])
{	if (argc>=2 && toupper(argv[1][0])=='R')
	{	int i;
		int last=99;
		int n=(argc>=3) ? atoi(argv[2]) : 1;
		for (i=1 ; i<n ; i++)
		{	int new;
			do new=(rand()%8);
			while (new==last);
			last=new;
			if (set_pos(new%8)!=0)
				return -1;
			sleep(1);
		}
		return 0;
	} else
	return set_pos(atoi(argv[1]));
}