brickpi-coffeescript/src/BrickPi.coffee

BrickPi API implementation in CoffeeScript for JS/CS

{SerialPort} = require 'serialport'
Promise = require 'bluebird'

BrickPiError = require './BrickPiError'
SerialPortError = require './SerialPortError'
{replicate, sum, clamp, sign, limit} = require './utils'

PORT_A = 0
PORT_B = 1
PORT_C = 2
PORT_D = 3

PORT_1 = 0
PORT_2 = 1
PORT_3 = 2
PORT_4 = 3

MASK_D0_M = 0x01
MASK_D1_M = 0x02
MASK_9V   = 0x04
MASK_D0_S = 0x08
MASK_D1_S = 0x10

BYTE_MSG_TYPE               = 0 # MSG_TYPE is the first byte.
MSG_TYPE_CHANGE_ADDR        = 1 # Change the UART address.
MSG_TYPE_SENSOR_TYPE        = 2 # Change/set the sensor type.
MSG_TYPE_VALUES             = 3 # Set the motor speed and direction, and return the sesnors and encoders.
MSG_TYPE_E_STOP             = 4 # Float motors immidately
MSG_TYPE_TIMEOUT_SETTINGS   = 5 # Set the timeout
# New UART address (MSG_TYPE_CHANGE_ADDR)
BYTE_NEW_ADDRESS   = 1

# Sensor setup (MSG_TYPE_SENSOR_TYPE)
BYTE_SENSOR_1_TYPE = 1
BYTE_SENSOR_2_TYPE = 2

BYTE_TIMEOUT=1

TYPE_MOTOR_PWM               = 0
TYPE_MOTOR_SPEED             = 1
TYPE_MOTOR_POSITION          = 2

TYPE_SENSOR_RAW              = 0 # - 31
TYPE_SENSOR_LIGHT_OFF        = 0
TYPE_SENSOR_LIGHT_ON         = (MASK_D0_M | MASK_D0_S)
TYPE_SENSOR_TOUCH            = 32
TYPE_SENSOR_ULTRASONIC_CONT  = 33
TYPE_SENSOR_ULTRASONIC_SS    = 34
TYPE_SENSOR_RCX_LIGHT        = 35 # tested minimally
TYPE_SENSOR_COLOR_FULL       = 36
TYPE_SENSOR_COLOR_RED        = 37
TYPE_SENSOR_COLOR_GREEN      = 38
TYPE_SENSOR_COLOR_BLUE       = 39
TYPE_SENSOR_COLOR_NONE       = 40
TYPE_SENSOR_I2C              = 41
TYPE_SENSOR_I2C_9V           = 42

BIT_I2C_MID  = 0x01  # Do one of those funny clock pulses between writing and reading. defined for each device.
BIT_I2C_SAME = 0x02  # The transmit data, and the number of bytes to read and write isn't going to change. defined for each device.

INDEX_RED   = 0
INDEX_GREEN = 1
INDEX_BLUE  = 2
INDEX_BLANK = 3

module.exports = class BrickPi
  constructor: ->
    @address = [1, 2] # Communication addresses
    @timeout = 0      # Communication timeout (how long in ms since the last valid
                      #   communication before floating the motors). 0 disables the timeout.

    # Motors
    @motorSpeed = replicate 0, 4 # Motor speeds, from -255 to 255
    @motorEnable = replicate 0, 4 # Motor enable/disable

    # Encoders
    @encoderOffset = Array 4 # Encoder offsets (possibly not yet implemented, but it should be)
    @encoder       = Array 4 # Encoder values

    # Sensors

    @sensor         = Array 4            # Primary sensor values
    @sensorArray    = Array 4 for [1..4] # For more sensor values for the sensor (e.g. for color sensor FULL mode).
    @sensorType     = replicate 0, 4     # Sensor types
    @sensorSettings = Array 8 for [1..4] # Sensor settings, used for specifying I2C settings.

    # I2C

    @sensorI2CDevices = Array 4 # How many I2C devices are on each bus (1 - 8).
    @sensorI2CSpeed   = Array 4 # The I2C speed.
    @sensorI2CAddr    = Array 8 for [1..4] # The I2C address of each device on each bus.
    @sensorI2CWrite   = Array 8 for [1..4] # How many bytes to write
    @sensorI2CRead    = Array 8 for [1..4] # How many bytes to read
    @sensorI2COut     = Array 16 for [1..8] for [1..4] # The I2C bytes to write
    @sensorI2CIn      = Array 16 for [1..8] for [1..4] # The I2C input buffers

    @zeroFlags() # @flags buffer is used for writing data
    @retried = 0

    @serialPort = Promise.promisifyAll new SerialPort '/dev/ttyAMA0', baudrate: 500000, false
    @receiveBuffer = []

  zeroFlags: ->
    @flags = replicate 0, 256
    @bitOffset = 0

  setFlags: (inputFlags) ->
    @flags[i] = flag for flag, i in inputFlags

  changeAddress: (oldAddress, newAddress) ->
    @flags[BYTE_MSG_TYPE] = MSG_TYPE_CHANGE_ADDR
    @flags[BYTE_NEW_ADDRESS] = newAddress
    @send oldAddress, 2, @flags
    .then =>
      @receive 5
    .then (inputflags) =>
      unless inputflags.length is 1 and @flags[BYTE_MSG_TYPE] is MSG_TYPE_CHANGE_ADDR
        throw new BrickPiError "Incorrect message received while changing address"
      @setFlags inputFlags

# setTimeout: ->
#   for i in [0, 1]
#     @flags[BYTE_MSG_TYPE] = MSG_TYPE_TIMEOUT_SETTINGS
#     @flags[BYTE_TIMEOUT] = @timeout & 0xFF
#     @flags[BYTE_TIMEOUT + 1] = (@timeout / 256     ) & 0xFF
#     @flags[BYTE_TIMEOUT + 2] = (@timeout / 65536   ) & 0xFF
#     @flags[BYTE_TIMEOUT + 3] = (@timeout / 16777216) & 0xFF
#     @send @address[i], 5, @flags
#     [res, BytesReceived, inputFlags] = @receive 0.002500
#     if res #error
#       return -1
#     for j in [0...inputFlags.length]
#       @flags[j] = inputFlags[j]
#     if not (BytesReceived is 1 and @flags[BYTE_MSG_TYPE] is MSG_TYPE_TIMEOUT_SETTINGS)
#       return -1
#     i+=1
#   return 0

# def motorRotateDegree(power, deg, port, sampling_time=.1):
#     """Rotate the selected motors by specified degre

#     Args:
#         power       : an array of the power values at which to rotate the motors (0-255)
#         deg     : an array of the angle's (in degrees) by which to rotate each of the motor
#         port        : an array of the port's on which the motor is connected
#         sampling_time   : (optional) the rate(in seconds) at which to read the data in the encoders

#     Returns:
#         0 on success

#     Usage:
#         Pass the arguments in a list. if a single motor has to be controlled then the arguments should be
#         passed like elements of an array, e.g, motorRotateDegree([255],[360],[PORT_A]) or
#         motorRotateDegree([255, 255],[360, 360],[PORT_A, PORT_B])
#     """

#     num_motor=power.length        #Number of motors being used
#     init_val=[0]*num_motor
#     final_val=[0]*num_motor
#     @updateValues()
#     for i in [0...num_motor]
#         @motorEnable[port[i]] = 1                #Enable the Motors
#         power[i]=abs(power[i])
#         @motorSpeed[port[i]] = power[i] if deg[i]>0 else -power[i]   #For running clockwise and anticlockwise
#         init_val[i]=@encoder[port[i]]                #Initial reading of the encoder
#         final_val[i]=init_val[i]+(deg[i]*2)             #Final value when the motor has to be stopped;One encoder value counts for 0.5 degrees
#     run_stat=[0]*num_motor
#     while True:
#         result = @updateValues()                  #Ask BrickPi to update values for sensors/motors
#             if not result
#             for i in [0...num_motor]              #Do for each of the motors
#                 if run_stat[i]==1
#                     continue
#                 if(deg[i]>0 and final_val[i]>init_val[i]) or (deg[i]<0 and final_val[i]<init_val[i])   #Check if final value reached for each of the motors
#                             init_val[i]=@encoder[port[i]]            #Read the encoder degrees
#                 else:
#                     run_stat[i]=1
#                     @motorSpeed[port[i]]=-power[i] if deg[i]>0 else power[i] #Run the motors in reverse direction to stop instantly
#                     @updateValues()
#                     time.sleep(.04)
#                     @motorEnable[port[i]] = 0
#                     @updateValues()
#         time.sleep(sampling_time)                   #sleep for the sampling time given (default100 ms)
#         if(all(e==1 for e in run_stat))                #If all the motors have already completed their rotation, then stop
#             break
#     return 0

  # Reads bits from @flags, moving the cursor by number of `bits`
  _getBits: (byteOffset, bitOffset, bits) ->
    result = 0
    for i in [bits...0]
      result <<= 1
      offset = bitOffset + @bitOffset + i - 1
      result |= @flags[byteOffset + offset // 8] >> (offset % 8) & 0x01
    @bitOffset += bits
    return result

  # Writes bits to @flags, moving the cursor by number of `bits`
  _addBits: (byteOffset, bitOffset, bits, value) ->
    for i in [0...bits]
      if value & 0x01
        offset = bitOffset + @bitOffset + i
        @flags[byteOffset + offset // 8] |= 0x01 << (offset % 8)
      value //= 2
    @bitOffset += bits

  # How many bits are needed to represent an integer
  # returns 1 to 8a
  _bitsNeeded: (value) ->
    for i in [0...32]
      break if value is 0
      value //= 2
    return i

  _readBits: (bits) ->
    @_getBits 1, 0, bits

  # Setup the sensors
  setupSensors: ->
    for i in [0, 1]
      @zeroFlags()
      @flags[BYTE_MSG_TYPE] = MSG_TYPE_SENSOR_TYPE
      @flags[BYTE_SENSOR_1_TYPE] = @sensorType[PORT_1 + i * 2] # PORT_1, PORT_3
      @flags[BYTE_SENSOR_2_TYPE] = @sensorType[PORT_2 + i * 2] # PORT_2, PORT_4
      for j in [0, 1]
        port = i * 2 + j # 0, 1, 2, 3

        # Setup I2C sensors
        sensorTypeFlag = @flags[BYTE_SENSOR_1_TYPE + j] # 1_TYPE, 2_TYPE, 1_TYPE, 2_TYPE
        if sensorTypeFlag in [TYPE_SENSOR_I2C, TYPE_SENSOR_I2C_9V]
          @_addBits 3, 0, 8, @sensorI2CSpeed[port]

          # normalize @sensorI2CDevices[port]
          clamp @sensorI2CDevices, port, 1, 8

          @_addBits 3, 0, 3, @sensorI2CDevices[port] - 1

          for device in [0...@sensorI2CDevices[port]]
            @_addBits 3, 0, 7, @sensorI2CAddr[port][device] >> 1
            @_addBits 3, 0, 2, @sensorSettings[port][device]
            if @sensorSettings[port][device] & BIT_I2C_SAME
              @_addBits 3, 0, 4, @sensorI2CWrite[port][device]
              @_addBits 3, 0, 4, @sensorI2CRead[port][device]

              for out_byte in [0...@sensorI2CWrite[port][device]]
                @_addBits 3, 0, 8, @sensorI2COut[port][device][out_byte]

      messageLength = (@bitOffset + 7) // 8 + 3 #eq to UART_TX_BYTES
      @send @address[i], messageLength, @flags
      .then =>
        @receive 500
      .then (inputflags) =>
        unless inputflags.length is 1 and @flags[BYTE_MSG_TYPE] is MSG_TYPE_SENSOR_TYPE
          throw new BrickPiError "Incorrect message received while changing address"
        @setFlags inputFlags

  # Computes [direction, speed] vector given possibly negative integer value
  # @direction 0 maps to forward, 1 to backward
  _speedVector: (speed) ->
    [direction, speed] = sign speed
    [direction, limit speed, 255]

  # Send all attributes to the brick
  updateValues: ->
    ret = false

    for i in [0, 1]
      if not ret
        @retried = 0

      #Retry Communication from here, if failed
      @zeroFlags()
      @flags[BYTE_MSG_TYPE] = MSG_TYPE_VALUES

      # Write data for encoders
      for j in [0, 1]
        port = i * 2 + j # 0, 1, 2, 3
        # User reset the encoder
        if @encoderOffset[port]
          # 1 for set encoder, 5 bits for the size of the new offset value in bits
          # then the value and one bit for direction
          @_addBits 1, 0, 1, 1
          [tempEncoderDirection, tempValue] = sign @encoderOffset[port]
          tempBitsNeeded = @_bitsNeeded(tempValue) + 1
          @_addBits 1, 0, 5, tempBitsNeeded
          @_addBits 1, 0, tempBitsNeeded, tempValue << 1 | tempEncoderDirection
        else
          # 0 not setting encoders
          @_addBits 1, 0, 1, 0

      # Write data for motor speeds
      for j in [0, 1]
        port = i * 2 + j # 0, 1, 2, 3
        [direction, speed] = _speedVector @motorSpeed[port]
        # 8 bits for speed, 1 bit for direction, 1 bit for enabling
        @_addBits 1, 0, 10, speed << 2 | direction << 1 | @motorEnable[port]

      # Write data for I2C sensors
      for j in [0, 1]
        port =  i * 2 + j
        if @sensorType[port] in [TYPE_SENSOR_I2C, TYPE_SENSOR_I2C_9V]
          for device in [0...@sensorI2CDevices[port]]
            if not (@sensorSettings[port][device] & BIT_I2C_SAME)
              @_addBits 1, 0, 4, @sensorI2CWrite[port][device]
              @_addBits 1, 0, 4, @sensorI2CRead[port][device]
              for out_byte in [0...@sensorI2CWrite[port][device]]
                @_addBits 1, 0, 8, @sensorI2COut[port][device][out_byte]
            device += 1


      messageLength = (@bitOffset + 7) // 8 + 1 #eq to UART_TX_BYTES
      @send @address[i], messageLength, @flags
      .then =>
        @receive 8
      .then (inputFlags) ->
        @setFlags inputFlags
        @encoderOffset[i * 2 + PORT_A] = 0
        @encoderOffset[i * 2 + PORT_B] = 0

        if @flags[BYTE_MSG_TYPE] isnt MSG_TYPE_VALUES
          throw new SerialPortError "Incorrect returned message type"

      .catch SerialPortError, =>
        if @retried < 2
          ret = yes
          @retried++
          # continue in the outside loop
        else
          throw new BrickPiError "Failed retry"
          # return from updateValues
      .then =>
        ret = false

        # Read from beginning
        @bitOffset = 0

        # Read encoder values
        tempBitsUsed = []
        tempBitsUsed.append @_readBits 5
        tempBitsUsed.append @_readBits 5
        for j in [0, 1]
          tempEncoderVal = @_readBits tempBitsUsed[j]
          direction = if tempEncoderVal & 0x01 then -1 else 1
          @encoder[j + i * 2] = direction * (tempEncoderVal // 2)

        # Read sensors
        for j in [0, 1]
          port = j + i * 2 # 0, 1, 2, 3
          sensorType = @sensorType[port]
          if sensorType is TYPE_SENSOR_TOUCH
            @sensor[port] = @_readBits 1
          else if sensorType in [TYPE_SENSOR_ULTRASONIC_CONT, TYPE_SENSOR_ULTRASONIC_SS]
            @sensor[port] = @_readBits 8
          else if sensorType is TYPE_SENSOR_COLOR_FULL
            @sensor[port] = @_readBits 3
            colorSensorValues = @sensorArray[port]
            colorSensorValues[INDEX_BLANK] = @_readBits 10
            colorSensorValues[INDEX_RED]   = @_readBits 10
            colorSensorValues[INDEX_GREEN] = @_readBits 10
            colorSensorValues[INDEX_BLUE]  = @_readBits 10

          # Read IC2 sensors
          else if sensorType in [TYPE_SENSOR_I2C, TYPE_SENSOR_I2C_9V]
            @sensor[port] = @_readBits @sensorI2CDevices[port]
            for device in [0...@sensorI2CDevices[port]]
              if @sensor[port] & 0x01 << device
                for in_byte in [0...@sensorI2CRead[port][device]]
                  @sensorI2CIn[port][device][in_byte] = @_readBits 8

          #For all the light, color and raw sensors
          else
            @sensor[j + i * 2] = @_readBits 10

  # @returns a promise resolved once the serial port connection has been established
  setup: ->
    @serialPort.on 'error', (error) -> console.log "Serial port error: #{error}"
    @serialPort.on 'close', -> console.log "Serial port closed"
    @serialPort.on 'data', @_handleDataReceived

    @serialPort.openAsync()

  destroy: ->
    @serialPort.closeAsync()

  # Computes a checksum for a message of bytes in `data`, optionally
  # targetting an address `dest`
  # @returns a byte
  checksum: (data, dest = 0) ->
    (dest + data.length + sum data) % 256

  # Sends to `dest` `byteCount` of values from `outputFlags`
  # @returns a promise resolved once the data has been transmitted
  send: (dest, byteCount, outputFlags) ->
    trimmedOutputFlags = outputFlags[...byteCount]
    checksum = @checksum trimmedOutputFlags, dest
    @serialPort.writeAsync [dest, checksum, byteCount].concat trimmedOutputFlags
    .then @serialPort.drainAsync()

  receive: (timeout) ->
    new Promise (resolve, reject) =>
      start = +new Date
      buffer = []
      @serialPort.on 'data', (data) =>
        # time out
        reject new SerialPortError "Timeout" if +new Date - timeout > start

        # copy data
        buffer.push i for i in data

        # message received
        if buffer.length >= buffer[1]
          inputFlags = buffer[2..]
          reject new SerialPortError "Corrupted data" if buffer[0] isnt @checksum inputFlags
          resolve inputFlags

  _handleDataReceived: (data) =>
    @receiveBuffer.push i for i in data


module.exports = {BrickPi, PORT_A, PORT_B}