#################################################
# RaSCL : Robotics & Sensorial Control Language #
#                                               #
#  http://www.slyware.com/projects_rascl.shtml  #
#      Simon Chudley (simon@slyware.com)        #
#                                               #
#################################################
#
# File    : libraries/lib_devices.rascl
# Summary : Common code for manipulating devices.
#
# Contains common functions for manipulating devices/resources.



##########################
## RESOURCE CONTROLLERS ##
##########################
#
# Within this section, we define all the resource controllers that can be used by RaSCL.
# Each controller is a resource, and a resource container. This means it can hold a
# number of resources (such as a servo controller holding a number of servos).
#
# When a servo sends a command, it will ask the controller to encode the ID, speed and
# position into a byte stream (which is specific for each controller), and then ask the
# controller to send it. It also uses the controller to work out how long it will take
# to move the servo.
#
# You MUST following naming conventions when defining servo controllers. You MUST implement
# a 'encode_servo_command' method, taking a RaSCL list of (ID Position Speed)
# and return a RaSCL list of raw bytes. You should also implement a 'servo_move_time'
# method, taking a RaSCL list of (Distance Speed), and returning how long it will take to
# move a servo that distance at the given speed (in miliseconds).
#
# For example, the Milford Instruments SSBD12 servo controller accepts:
#
#    <syncbyte> <databyte1> <databyte2>
#
# Where <syncbyte> is 255, the first four bits of <databyte1> are the
# servo speed, the second four bits the servo ID, and the final <databyte2>
# is the required position:
#
#           1   4   8
#    <sync> <-byte1-> <byte2>
#     255   |spd| ID|   pos
#
# Assuming the ID, position and speed have been split into lID, lPos and lSpeed, the
# following creates a RaSCL list containing the three raw byte command:
#
#    (list 255 (bit| (bit<< (bit& lSpeed 15) 4) (bit& lID 15)) lPos)
#


    ###################################
    ##  Milford Instruments SSBD12   ##
    ## Rs232 Serial Servo Controller ##
    ##   RS232Controller<MFSSBD12>   ##
    ###################################

    ##########
    # Need dummy constructor
    #
    (::complex_function RS232Controller<MFSSBD12> create)
    (::endfunc)


    ##########
    # Encode bytes (<sync> <byte1> <byte2>)
    # Where <sync>  = 255
    #       <byte1> = Top four bits servo id, lower four bits speed
    #       <byte2> = Position
    #
    (::complex_function RS232Controller<MFSSBD12> encode_servo_command)

        # Split out the arguments
        (::= .lID (icar func_args 0))
        (::= .lPos (icar func_args 1))
        (::= .lSpeed (icar func_args 2))

        # Build and return the command
        (::return (list 255 (bit| (bit<< (bit& lSpeed 15) 4) (bit& lID 15)) lPos))

    (::endfunc)


    ##########
    # Calculate servo distance move time
    #
    #        distance * MFSSBD12_RESOLUTION * MFSSBD12_FRAME_TIME
    # time = ----------------------------------------------------
    #               speed * MFSSBD12_FRAME_PULSE_TIME
    #
    # Where:
    #       MFSSBD12_RESOLUTION       = 8   (Resoultion of drive)
    #       MFSSBD12_FRAME_TIME       = 20  (1 frame equals 20msecs)
    #       MFSSBD12_FRAME_PULSE_TIME = 4   (Speed in units of 4usecs/change in pulse width)
    #
    (::complex_function RS232Controller<MFSSBD12> servo_move_time)

        # Split out the arguments
        (::= .lDistance (icar func_args 0))
        (::= .lSpeed (icar func_args 1))

        # Calculate result
        (::return (/ (* (* lDistance 8) 20) (* lSpeed 4)))

    (::endfunc)