# python module "abs"
#
# for controlling the Acroname BrainStem
#
# this does not require any of Acroname's software 
# (once the board is in internal heartbeat mode!)
# (the freely available console.exe program is available for that -- see below)
#
# it talks to the board using its serial protocol
#
# you'll need pyserial, however...
# installing Mark Hammond's win32all takes care of that
#   and pyserial is also available form SourceForge for Mac OS X and Linux

#
# version 0.1: Zach Dodds 2/24/06
# dodds@cs.hmc.edu
#

# to do
#    - change sendAndWait so that it reads bytes until all
#      the expected bytes are read, checking for error messages
#      this will save time over the 
#    - set up conversions for pan/tilt unit from degrees to raw units
#    - add the rest of the BrainStem command interface
#    - add support for handling heartbeats
#    - create a sensor-checker on board that will gather all
#      of the latest data into a buffer and return it when asked...

# **********************************************************************
#
# NOTE:
#
# this code does not handle the BrainStem's "heartbeat"
# so you need to turn its heartbeat into "internal" mode
# 
# The following instructions come from 
# http://www.acroname.com/brainstem/examples/basicslave/basicslave.html
#
# To configure the BrainStem for internal heartbeat mode, 
# Enter the following commands from the Console: (console.exe)
#
#      2 18 5 1
#      2 19
#
# The first is the cmdVAL_SET command for switching to 
# internal heartbeat mode.  The second is the cmdVAL_SAV 
# command for saving the current system settings to the EEPROM.  
# When the Stem is turned off and on again, its heartbeat 
# light will blink automatically without being connected to the host.
#
# **********************************************************************
#
# Quick Guide to try things out
#
# >>> import abs     
# >>> abs.boardInit(PORT)  
#         
#     where PORT is taken from the control panel/device manager
#     this is the number of the serial port (1 = COM1, 2 = COM2, etc.)
#
#     see abs.BOARD_ADDRESS for the IIC address of the BrainStem (2 most likely)
#     if yours is not 2, you are too knowledgeable to be reading this...
#
# >>> abs.getHB()
#
#     should return the HB rate (1-255)
#
# >>> abs.setHB(40)
#
#     should be noticeably slower than the default HB rate of 10
#     try other values to speed up/slow down the heartbeat
#
# >>> abs.readA2D(<a2d port number>)
#
#     if you have a sensor plugged into the analog inputs, you'll get 
#     the 10-bit reading here
#
# >>> abs.readIR( port number )
#
#     is really the same thing, but happier :)
#
# >>> abs.sendAndWait([17 2])  
#
#     for sending raw commands to the brainstem.
#     All of the commands are documented at 
#     http://www.acroname.com/brainstem/ref/ref.html
#
#     The above just gets the heartbeat...
#
# >>> abs.setUpPanTiltServos() does the following
#
#     If you plus a panning servo into servo pin 1
#     and a tilting servo into servo pin 2
#     the above code will set them up...
#
# >>> abs.ptCenter()
#
#     centers them
#
# >>> abs.ptPos( pan, tilt )
#
#     0 <= pan,tilt <= 255 
#     this will place each motor into the specified position,
#     relative to that motor's range
#
# >>> abs.setPTvelocities( pan_vel, tilt_vel )
#
#     -1.0 <= pan_vel, tilt_vel <= 1.0
#     velocity control of the motors
#     Keep in mind how small their ranges are!
#
# >>> abs.close()
#
#     disables the servos and
#     closes the serial port
#

# modules we might need
import serial
import time
import math

# a global serial object for talking to the brainstem
ser = serial.Serial()

# this is the default address for the BrainStem
BOARD_ADDRESS = 2  

# here are all of the commands we've implemented thus far
cmdVAL_GET  = 17
cmdVAL_SET  = 18
cmdSRV_SAV  = 20
cmdA2D_RD   = 25
cmdIR02_RD  = 30
cmdSRV_CFG  = 31
cmdSRV_LMT  = 32
cmdSRV_ABS  = 33
cmdSRV_STOP = 36

# functions for doing different stuff...

# the default here is 15 - this is "one notch"
# slower than maximum speed (indicated as 0)
# for some reason, when set to 0, sometimes the servo
# bounces around crazily...
def servoConfig(servonumber,speed=15,enable=1):
    """ partial implementation of cmdSRV_CFG
    """
    # servo config: (2 3) 31 id# bitfield
    if enable != 1:
        enable = 0
    if speed < 0 or speed > 15:
        speed = 0    # max speed (1 is min)
    # cool - a chance to use shift and bitwise or
    bitfield = enable << 7 | speed
    if servonumber < 0 or servonumber > 3:
        print 'servonumber of',servonumber,'is out of bounds.'
	print 'Not sending a command.'
        return
    # ready to go... no response
    # we expect no reply bytes for this one
    sendAndWait([cmdSRV_CFG, servonumber, bitfield], 0) 

def saveServoParameters():
    """ this will save current servo parameters to EEPROM
        including enable bit, invert bit, speed, configuration
           offset and resolution, and current positions
        presumably, this means this will be the power-on state
           the next time the brainstem is used...
    """
    # expect no reply bytes for this one
    sendAndWait([cmdSRV_SAV], 0)
    print 'servo parameters saved!'

def servoStop( servo_motor_num ):
    """ this implements the cmdSRV_STOP command, which
        only works if speed is from 1-15.
        If speed == 0 for a particular servo, the motion
        will completeregardless of this cmdSRV_STOP command.
    """
    if 0 <= servo_motor_num <= 3:
        # expect no reply bytes for this one
        sendAndWait([cmdSRV_STOP, servo_motor_num], 0)
    else:
        print 'in servoStop,'
        print 'the servo motor', servo_motor_num, 'is out of bounds'

def setPTvelocities( pan_vel, tilt_vel ):
    """ runs a pan/tilt head as if it were a velocity-controlled
        device, even though servo motors are position-controlled
        FUN!
        The inputs are scaled by a single factor so
            that the larger (absolute) value is less than 1.0
        These are then mapped onto 1-10  out of the 1-15 range 
            that the abs provides us
        Simply changing the speed works, so we may not need
            to change the set point
        Simply sending a new set point also works - cool!
            We'll use that, as well
        Keep in mind that values < 0.05 and > -0.05 will be zero,
            and so will stop the motor!
    """
    pan_vel_abs = math.fabs(pan_vel)
    tilt_vel_abs = math.fabs(tilt_vel)

    # these "sign" values simply hold the destination
    # for the pan and tilt motors (which _do_ depend on the sign)
    pan_vel_sign = +255
    tilt_vel_sign = +255
    if pan_vel < 0.0:
        pan_vel_sign = 0
    if tilt_vel < 0.0:
        tilt_vel_sign = 0

    # now that we have all of that, let's scale
    maxVal = max( pan_vel_abs, tilt_vel_abs )
    if maxVal > 1.0:
        pan_vel *= 1.0/maxVal
        tilt_vel *= 1.0/maxVal
        pan_vel_abs *= 1.0/maxVal
        tilt_vel_abs *= 1.0/maxVal

    # set speeds 0-10 inclusive
    pan_speed = math.floor( (10.0*pan_vel_abs) + 0.5 )
    tilt_speed = math.floor( (10.0*tilt_vel_abs) + 0.5 )

    # set appropriate set points
    if pan_speed == 0:
        servoStop(0)
    else:
        servoConfig(0,int(pan_speed),1)
        setAbsPos(0,int(pan_vel_sign))

    if tilt_speed == 0:
        servoStop(1)
    else:
        servoConfig(1,int(tilt_speed),1)
        setAbsPos(1,int(tilt_vel_sign))

def disableServos():
    """ sets each servo's enable bit to off """
    print 'disabling servos'
    servoConfig(0, 0, 0)
    servoConfig(1, 0, 0)
    servoConfig(2, 0, 0)
    servoConfig(3, 0, 0)


def setUpPanTiltServos():
    """ this function 
        - disables all of the servos
	- sets the Servo Limits
	- enables the servos
	- puts the motors at a middle position
        NOTE that it assumes the PAN  MOTOR is at servo pin 0
                         and the TILT MOTOR is at servo pin 1
    """
    disableServos()
    print 'now enabling and setting pan/tilt parameters'
    setPanTiltServoLimits()   # hopefully OK
    servoConfig(0)   # enable motor #0
    servoConfig(1)   # ditto #1
    setAbsPos(0,100)    # set pan position - forward
    setAbsPos(1,70)    # set tilt position - forward

def ptPos( pan, tilt ):
    """ set a position between 0 and 255 for pan and tilt servos """
    setAbsPos( 0, pan )
    setAbsPos( 1, tilt )

def ptCenter():
    """ restore center positioning for pan and tilt servos """
    ptPos( 100, 70 )

def setPT_position_mode():
    """ restore to default (quick) velocities """
    servoConfig(0)
    servoConfig(1)

def setServoLimit( servoid, pos0, posres ):
    """ implementation of the cmdSRV_LMT command """
    if pos0 < 0:  pos0 = 0
    if pos0 > 70: pos0 = 70
    if posres < 1:  posres = 1
    if posres > 70: posres = 70
    if servoid<0 or servoid>3:
        print 'servoid of', servoid, 'in setServoLimit is out of bounds.'
        print 'not sending command'
        return
    # expect no reply bytes for this one
    sendAndWait([cmdSRV_LMT, servoid, pos0, posres], 0)

def setPanTiltServoLimits():
    """ sets the limits for a pan/tilt head
    """
    # servo limit: (2 4) 32 id# POS0 POSRES
    # ready to go... no response
    servonumber = 0 # set pan to be the whole range...
    setServoLimit( servonumber, 0, 70 )
    #sendNoWait([32, servonumber, 0, 70])
    servonumber = 1 # set tilt so that the arm does not hit anything
    setServoLimit( servonumber, 18, 35 )
    #sendNoWait([32, servonumber, 18, 35])

#def setServoLimits(...)

def setAbsPos(servonumber,position):
    """ sets the position of the indicated servo, implementing cmdSRV_ABS
    """
    # servo abs pos: (2 3) 33 id# ABSPOS
    if servonumber < 0 or servonumber > 3:
        print 'servonumber of',servonumber,'is out of bounds.'
        print 'Not sending a position from setAbsPos'
        return
    position = int(position)
    if position < 0: position = 0
    if position > 255: position = 255
    # we expect no reply bytes for this one
    sendAndWait([cmdSRV_ABS, servonumber, position], 0)


def getAbsPos(servonumber):
    """ gets the position of the indicated servo, implementing cmdSRV_ABS
        with no position parameter
    """
    # servo abs pos: (2 3) 33 id# ABSPOS <- last part omitted here
    if servonumber < 0 or servonumber > 3:
        print 'servonumber of',servonumber,'is out of bounds.'
        print 'Not getting a position from getAbsPos'
        return
    # we expect 5 bytes
    response = sendAndWait([cmdSRV_ABS, servonumber], 5)
    return response[-1]


def getHB():
    """ returns the heartbeat rate ranging from 1 (fastest)
        to 255 (slowest). 10 is the default.
    """
    # check heartbeat rate: (2 2) 17 2
    # the board address and length (parenthesized) are added by "packet"
    # we expect to get 5 bytes back here...
    response = sendAndWait([cmdVAL_GET, 2], 5)
    return response[-1]


def setHB(x):
    """ sets the heartbeat rate to x, ranging from 1 to 255
    """
    x = int(x)
    if x < 1 or x > 255:
        print 'x =', x, 'is out of range. Not setting the heartbeat.'
        return
    # set heartbeat rate: (2 3) 18 2 10 (or 50 or whatever)
    # we expect 0 bytes back here...
    sendAndWait([cmdVAL_SET, 2, x], 0)


def readIR02():
    """ reads the special Sharp GP2D02 sensor

        This is slow (~0.085 seconds), which is why
        the serial port timeout is 0.1 seconds.
    """
    # this sensor's address is 0
    # the HOST bit (7th) needs to be set to get a message back
    # 1 << 7 is 128
    #print 'before: time.clock() is', time.clock()
    # wait for 5 bytes to come back
    result = sendAndWait([cmdIR02_RD, 128], 5)
    #print 'after:  time.clock() is', time.clock()
    # the last byte is the range value...
    return result[-1]


def readIR( num ):
    """ a not-too-much-shorter name for readA2D
    """
    return readA2D( num )


def readA2D(a2dPortNum):
    """ reads the A2D port given as input
    """
    if a2dPortNum < 0 or a2dPortNum > 4:
        print 'The a2dPortNum of', a2dPortNum, 'is out of the range 0-4.'
        print 'Not reading the A2D port. Returning -1.'
        return -1
    # cmdA2D_RD is 25 and
    # 25 (128+a2dPortNum) chooses the appropriate port
    # we expect 6 bytes back
    #print 'before: time.clock() is', time.clock()
    result = sendAndWait([cmdA2D_RD, 128+a2dPortNum], 6)
    #print 'after:  time.clock() is', time.clock()
    # now we have our list, the last two
    # bytes are the 10-digit reading and so are in
    # the range of 0 to 1023
    penultimateByteValue = result[-2] << 2
    ultimateByteValue = result[-1] >> 6
    return penultimateByteValue + ultimateByteValue



# 8 is the maximum packet size that will ever come back
# actually, this is not true if some unread (error message)
# bytes have built up in the buffer...
#
# we still need to fully handle error messages...
#
def sendAndWait(intlist, bytesback=8):
    """ This function adds the BOARD_ADDRESS and
        message size to the input (intlist, all in
        decimal), converts to raw-hex bytes and
        sends off along the serial port.

        It then waits for bytesback bytes in a reply, makes the
        reply to a list of decimal values and returns it.

        STILL NEED TO HANDLE ERROR MESSAGES...
    """
    # packetize and send
    #print 'sending intlist', intlist
    message = packet(intlist)
    ser.write(message)
    # get data back, timeout should be set!
    response = ser.read(size=bytesback)
    #print "raw response was", repr(response)
    list = binaryToList(response)
    print "list response was", list
    return list


def binaryToList(s):
    """ creates a list of decimal byte values
        from the raw bytes in s
    """
    return [ord(x) for x in s]


def packet(list):
    """ creates the Acroname BrainStem packet format
        simply by adding the board address and length
        bytes at the start of the list of values given,
        then putting all of it into a raw string of bytes
    """
    # put the board address and length in the front
    list[:0] = [BOARD_ADDRESS, len(list)]
    # create the string
    return reduce(lambda x,y: x+y, [chr(x) for x in list])
        

def asciiToList(s):
    """ creates a list of decimal integers from 
        a space-or-tab-delimited input string
    """
    # strip outer whitespace and expand tabs to 1 space
    s = s.strip().expandtabs(1)
    # split on whitespace
    s = s.split(' ')
    # remove extra whitespace (empty strings)
    # and then convert to integers with chr(int(x))
    return [int(x) for x in s if x != '']
    

# code to open the serial port
def boardInit(PORT):
    """ boardInit sets up the serial port parameters
    """
    ser.port = PORT-1
    ser.baudrate = 9600
    ser.timeout = 0.1    # enough time to get the whole message... ?!
    ser.open()
    if not ser.isOpen():
        print "ERROR: serial port did not open"

def userLoop():
    """ userLoop is really not needed
    """
    while 1:
        print "type a message to send: ",
        reply = raw_input()
        if reply[0].lower() == 'q':   # if they typed 'Q' or 'q'
            break
        l = asciiToList(reply)
        response = sendAndWait(l)
        print "response was", response


def close():
    """ close gets the board ready to shut down -
        it disables the servos and closes the 
        serial connection
    """
    # should we reset the board or anything?
    # let's disable all of the servos...
    disableServos()
    #
    if ser.isOpen():
        ser.close()
    else:
        print 'the serial port was not open!'


if __name__ == '__main__':
    # unit testing for abs.py
    PORT_NUMBER = 1  
    # be sure the PORT_NUMBER is 1 
    # or change the above value
    # or (I've found this works better), change the
    # port number from the Windows control panel...

    # open serial port etc.
    boardInit( PORT_NUMBER )

    # first thing to check - heartbeat
    hb_rate = getHB()
    print 'Heartbeat rate is', hb_rate

    # make sure this can be set
    new_hb_rate = int(input('Input a new hb rate (1-200, an int): '))
    setHB( new_hb_rate )

    # and that the new value is there...
    hb_rate = getHB()
    print 'Now, the heartbeat rate is', hb_rate

    # is there an IR sensor?
    reply = raw_input('Do you have an IR sensor plugged in? ')
    if len(reply) > 0 and reply.lower()[0] == 'y':
        pin_number = input('Which pin number (0-4)? ')

        ir_reading = readIR( pin_number )
        print 'Its ir reading is', ir_reading

    # is there a pan/tilt head?
    reply = raw_input('Do you have an PT head plugged in? ')
    if len(reply) > 0 and reply.lower()[0] == 'y':
        setUpPanTiltServos()

        print 'Moving to the positive limits of position'
        setPTvelocities( 0.1, 0.1 )
        reply = raw_input('Hit enter to continue. ')

        print 'Moving to the negative limits of position'
        setPTvelocities( -0.1, -0.1 )
        reply = raw_input('Hit enter to continue. ')

        print 'Recentering quickly'
        setPT_position_mode()
        ptCenter()


    reply = raw_input('Hit enter to clean up and quit. ')
    close()