import serial

class controller:
    def __init__(self,port="COM5:",servo_count=6,min=2496,max=9152, lmin=0,lmax=10000.0):
        self.servo_count=servo_count
        self.port="COM5:"
        self.serial = serial.Serial(self.port) # open the port to the servo controller
        self.cmdprefix = chr(0xAA) + chr(0x0C) # Pololu Maestro command prefix
        self.min=min  # servo physical minimun limit, value depends on controller
        self.max=max  # servo physical maximum limit, value depends on controller
        self.lmin=lmin # servo logical minimum limit, default set to 0
        self.lmax=lmax # servo logical maximun limit, default set to 1000
        self.lposition = [0] * servo_count
        self.ratio = (max-min)/(lmax-lmin) # the ratio from logical value to physical value

    def setPositionAbs(self,channel,p):
        l = p & 0x7F
        m = p >> 7 & 0x7F

        cmd = chr(0xAA) + chr(0x0C) + chr(0x04) + chr(channel) + chr(l) + chr(m)
        print "Debug:", p, ";", ord(cmd[0]),ord(cmd[1]),ord(cmd[2]),ord(cmd[3]),ord(cmd[4]),ord(cmd[5])

        self.serial.write(cmd)

    def setPosition(self,channel,lvalue): # l is the logical limit
        p = int(self.ratio * lvalue + self.min)
        self.setPositionAbs(channel,p)
        self.lposition[channel] = lvalue

    def getPosition(self,channel):
        cmd = self.cmdprefix + chr(0x10) + chr(channel)
        self.serial.write(cmd)
        l = ord(self.serial.read())
        m = ord(self.serial.read())

        #print "Hi",((m<<8) + l - self.min)/self.ratio
        return int(((m<<8) + l - self.min) /self.ratio)

    def getlPosition(self,channel):
        return self.lposition[channel]

    def close(self):
        self.serial.close()