## File: psc.py Version: v1.0
## Last Updated: Aug 5th 2006
## Created by: Will Smith Email: bugnthecode@gmail.com
## slightly modified by olsen wolf:sesselastronaut@gmail.com
## Allows easy control over a Parallax servo controller. Created with the usb version
## Check out parallax at http://www.parallax.com

import serial
import struct

class Psc:
    """This class allows you to send commands to a Parallax servo controller through a serial port.
    This version was fully tested on linux. If you're on windows you can make this work by supplying
    the com port -1. 
    Ex: myPsc = Psc(2) #for com port 1. 
    There are no gaurauntees that it will work
    though.
    For linux users, just supply the device's file you would like to use.
    Ex: myPsc = Psc('/dev/ttyUSB0') #This works on my system with a usb psc (ftdi driver)"""
    
    ATSTRING = '!SC'
    VERSION = 'VER?'
    TIMEOUT = 1
    
    def __init__(self, comPort):
        """Supply the port you would like to open. Operates at 2400 baud
        Ex: '/dev/ttyUSB0'"""
        self.psc = serial.Serial((comPort), 2400, timeout =Psc.TIMEOUT)
        
    def cleanup(self):
        """Must be called before destroying object to close out the serial port."""
        self.psc.close()
     
    def __del__(self):
        """In case you forgot to call cleanup() this will attempt to close the serial port
        upon object destruction. Remember the call to this method is implementation speceifc
        which means DO NOT RELY ON THIS TO CLEANUP FOR YOU!!!"""
        self.psc.close()
        
    def getVer(self):
        """Gets the version from the PSC. Mainly just to verify a connection"""
        
        self.psc.write(Psc.ATSTRING+Psc.VERSION+'\r')
        self.psc.read(8) #Discard the echo!
        return self.psc.read(3)
            
    def moveServo(self, ch, rate, position):
        """Takes in the desired channel, the ramp rate, and
        the desired position of ther servo. Moves the servo to the desired postion.
        Acceptable values (per the parallax docs):
        channel: 0 >= ch <= 15
        rate: 0 >= rate <= 63
        position: 250 >= position <= 1250"""
        
        #Ensure values are within spec. Note: this only supports the first 16
        #channels, so if the psc is daisy chained this will need modified!
        assert position >= 360 and position <=1150  #regular servo f.e. graupner C505
        #assert position >= 800 and position <=880    #360 Deg Rotation with modified Futaba S3003
        assert ch >= 0 and ch <= 15
        assert rate >= 0 and rate <= 63
        
        #Following line from John Machin on comp.lang.python
        self.psc.write( Psc.ATSTRING+struct.pack('<BBH', ch, rate, position)+'\r')
        self.psc.flushInput() #discard the echo

if __name__ == '__main__':
    ## The following tests for proper operation of a servo on ch 0 with ramp rate of 5.
    ## Moves the servo full swing one way, then full swing the other way, then center.
    ## Use this as an example for controlling the psc.
    
    import time #used for pausing between servo movements
    import sys
    mypsc = Psc('/dev/ttyUSB1') #Create new instance of the PSC on the desired port
#   print 'PSC Version: ' + mypsc.getVer()
#   input from stdin
    input=sys.argv[1]
    #split input & convert strings to int
    array=input.split(",")
    var1 = int(array[0]) #channel
    var2 = int(array[1]) #rate
    var3 = int(array[2]) #position
#    print var1,  var2, var3 
    mypsc.moveServo (var1, var2, var3) 
#   mypsc.moveServo(0, 5, 0) #Max one way
#   time.sleep(1)
#   mypsc.moveServo(0, 5, 1250) #Max the other way
    #time.sleep(1)
    #mypsc.moveServo(0, 5, 750) #Center
    #time.sleep(1)
    mypsc.cleanup() #let psc do any needed cleanup.