#!/usr/bin/env python
# -*- coding: iso-8859-15 -*-

# serial.py, Serial Module
# Copyright (C) 2006 por Rafael Treviño Menéndez
# Autor: Rafael Treviño Menéndez <skasi.7@gmail.com>

# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU Library General Public
# License as published by the Free Software Foundation; either
# version 2 of the License, or (at your option) any later version.

# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
# Library General Public License for more details.

# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

import os, termios, sys

class servos:

	def __init__ (self, term):
		self.fd = os.open (term, os.O_RDWR | os.O_NOCTTY)
		self.old = None
		self.last = None
		self.configurePort ()

	def configurePort (self):
		# Struct -> iflag, oflag, cflag, lflag, ispeed, ospeed, cc
		self.old = termios.tcgetattr (self.fd)
		
		iflag = termios.IGNPAR
		oflag = 0
		cflag = termios.CBAUD | termios.CS8 | termios.CLOCAL | termios.CREAD
		lflag = 0
		ispeed = termios.B9600
		ospeed = termios.B9600
		cc = self.old [-1]
		cc [termios.VMIN] = 1
		cc [termios.VTIME] = 0

		## Clear the input and output buffers
		termios.tcflush (self.fd, termios.TCIFLUSH)
		termios.tcflush (self.fd, termios.TCOFLUSH)

		termios.tcsetattr (self.fd, termios.TCSANOW,
				[iflag, oflag, cflag, lflag, ispeed, ospeed, cc])

	def write (self, data):
		return os.write (self.fd, data)

	#####################
	#        API        #
	#####################

	def setServos (self, servos):
		"""Send positioning data.
		Positions are in degrees."""
		result = True
		## The servos numeration in the Stargate goes from 1 to 8
		## For that reason the frame is created using i + 1
		if self.last:
			frame = [(i + 1, int (-p * 1.28 + 115)) for i, p in enumerate (servos) if p != self.last [i]]
		else:
			frame = [(i + 1, int (-p * 1.28 + 115)) for i, p in enumerate (servos)]
		self.last = servos
		for i, p in frame:
			result = result and (self.write ('%c%c%c' % ('\x57', i, p)) == 3)
		return result

	def setMask (self, mask):
		"""Send masking data."""
		return (self.write ('%c%c' % ('\x45', mask)) == 2)

if __name__ == '__main__':
	import sys
	print 'Autotest of "%s"' % sys.argv [0]
	try:
		dev = sys.argv [1]
	except IndexError:
		dev = '/dev/ttyS0'
	print 'Using "%s" as dev' % dev
	s = servos (dev)
	s.setMask (0xff)
	s.setServos ([45, 45]);
	del s