Partial work on implementing I2C protocol, low speed commands, and ul…

…trasonic sonar sensor.

examples/usb/ultrasonic.rb

@@ -0,0 +1,16 @@
+require 'lego-nxt'
+
+NXTBrick.new(:usb) do |nxt|
+  nxt.add_ultrasonic_input(:one, :sonar)
+
+  nxt.sonar.start
+
+  sleep(2)
+
+  while nxt.sonar.distance < 5
+    sleep(2)
+    puts "Distance: #{nxt.sonar.distance}"
+  end
+
+  puts 'Got it!'
+end

lib/lego-nxt.rb

@@ -3,6 +3,7 @@
 require 'active_support/inflector'
 
 require 'nxt/patches/module'
+require 'nxt/patches/string'
 
 require 'nxt/exceptions'
 
@@ -13,8 +14,11 @@
 require 'nxt/interfaces/usb'
 require 'nxt/interfaces/serial_port'
 
+require 'nxt/protocols/i2c'
+
 require 'nxt/commands/base'
 require 'nxt/commands/input'
+require 'nxt/commands/low_speed'
 require 'nxt/commands/output'
 require 'nxt/commands/program'
 require 'nxt/commands/sound'

lib/nxt/commands/base.rb

@@ -18,14 +18,51 @@ module Base
         three: 0x02,
         four: 0x03,
         all: 0xFF
-      }
+      }.freeze
+
+      ERRORS = {
+        'Pending communication transaction in progress' => 0x20,
+        'Specified mailbox queue is empty' => 0x40,
+        'Request failed (i.e. specified file not found)' => 0xBD,
+        'Unknown command opcode' => 0xBE,
+        'Insane packet' => 0xBF,
+        'Data contains out-of-range values' => 0xC0,
+        'Communication bus error' => 0xDD,
+        'No free memory in communication buffer' => 0xDE,
+        'Specified channel/connection is not valid' => 0xDF,
+        'Specified channel/connection not configured or busy' => 0xE0,
+        'No active program' => 0xEC,
+        'Illegal size specified' => 0xED,
+        'Illegal mailbox queue ID specified' => 0xEE,
+        'Attempted to access invalid field of a structure' => 0xEF,
+        'Bad input or output specified' => 0xF0,
+        'Insufficient memory available' => 0xFB,
+        'Bad arguments' => 0xFF
+      }.invert.freeze
 
       def send_and_receive(command_identifier, payload = [], response_required = true)
-        @interface.send_and_receive([
+        unless response_required
+          command_identifier |= 0x80
+        end
+
+        @interface.send([
           command_type,
           command_identifier,
           port_as_byte(self.port)
-        ] + payload, response_required)
+        ] + payload)
+
+        if response_required
+          response = @interface.receive
+
+          puts response.inspect
+          puts command_identifier.inspect
+
+          raise 'Not a valid response package.' unless response[0] == 0x02
+          raise 'Not a valid response to the command that was sent.' unless response[1] == command_identifier
+          raise ERRORS[response[2]] unless response[2] == 0x00
+
+          response[3..-1]
+        end
       end
 
       def port_as_byte(port)

lib/nxt/commands/input.rb

@@ -15,7 +15,10 @@ module Input
       COMMAND_IDENTIFIER = {
         set_input_mode: 0x05,
         get_input_values: 0x07,
-        reset_input_scaled_value: 0x08
+        reset_input_scaled_value: 0x08,
+        ls_get_status: 0x0E,
+        ls_write: 0x0F,
+        ls_read: 0x10
       }.freeze
 
       # The sensor type enum. This is a list of possible values when setting the
@@ -67,7 +70,6 @@ def command_type
 
       def set_input_mode(response_required = false)
         send_and_receive(COMMAND_IDENTIFIER[:set_input_mode], [
-          self.power,
           SENSOR_TYPE[self.sensor_type],
           SENSOR_MODE[self.sensor_mode]
         ], response_required)
@@ -82,6 +84,18 @@ def reset_input_scaled_value
         # TODO: Parse this response and return hash or something similar.
         send_and_receive(COMMAND_IDENTIFIER[:reset_input_scaled_value])
       end
+
+      def ls_get_status(response_required = false)
+        send_and_receive(COMMAND_IDENTIFIER[:ls_get_status])
+      end
+
+      def ls_write(bytes, response_required = false)
+        send_and_receive(COMMAND_IDENTIFIER[:ls_write], bytes)
+      end
+
+      def ls_read(response_required = false)
+        send_and_receive(COMMAND_IDENTIFIER[:ls_read])
+      end
     end
   end
 end

lib/nxt/commands/low_speed.rb

@@ -0,0 +1,39 @@
+module NXT
+  module Command
+    # An implementation of all the low speed I2C related NXT commands:
+    #
+    # * LSGETSTATUS
+    # * LSWRITE
+    # * LSREAD
+    #
+    # This class can also be used to talk to other third-party accessories
+    # connected in the input ports on the NXT brick that talk using I2C
+    # low speed master/slave communication.
+    module LowSpeed
+      include NXT::Command::Base
+      extend NXT::Utils::Accessors
+
+      COMMAND_IDENTIFIER = {
+        ls_get_status: 0x0E,
+        ls_write: 0x0F,
+        ls_read: 0x10
+      }.freeze
+
+      def command_type
+        COMMAND_TYPES[:direct]
+      end
+
+      def ls_get_status(response_required = false)
+        send_and_receive(COMMAND_IDENTIFIER[:ls_get_status])
+      end
+
+      def ls_write(response_required = false)
+        send_and_receive(COMMAND_IDENTIFIER[:ls_write])
+      end
+
+      def ls_read(response_required = false)
+        send_and_receive(COMMAND_IDENTIFIER[:ls_read])
+      end
+    end
+  end
+end

lib/nxt/connectors/input/ultrasonic.rb

@@ -2,8 +2,66 @@ module NXT
   module Connector
     module Input
       class Ultrasonic
-        def initialize(port)
+        include NXT::Command::Input
+        include NXT::Command::LowSpeed
+        include NXT::Utils::Assertions
+        extend NXT::Utils::Accessors
+
+        # Exception thrown by distance! when the sensor cannot determine the distance.
+        class UnmeasurableDistance < Exception; end
+
+        UNIT = [:centimeters, :inchess].freeze
+
+        attr_accessor :port, :interface
+
+        attr_combined_accessor :unit, :centimeters
+
+        attr_setter :unit, is_key_in: UNIT
+
+        def initialize(port, interface)
           @port = port
+          @interface = interface
+        end
+
+        def distance
+          ls_write(NXT::Protocols::I2C.read_measurement_byte_0)
+
+          binding.pry
+
+          while ls_get_status < 1
+            sleep(0.1)
+            # TODO: implement timeout so we don't get stuck if the expected data never comes
+          end
+
+          distance = ls_read[0]
+
+          if @unit == :centimeters
+            distance.to_i
+          else
+            (distance * 0.3937008).to_i
+          end
+        end
+
+        def distance!
+          d = distance
+          raise UnmeasurableDistance if d == 255
+          return d
+        end
+
+        def start
+          sensor_type(:lowspeed_9v)
+          sensor_mode(:raw)
+
+          set_input_mode
+
+          # clear buffer
+          begin
+            ls_read
+          rescue
+          end
+
+          # set sensor to continuously send pings
+          ls_write(NXT::Protocols::I2C.continuous_measurement_command)
         end
       end
     end

lib/nxt/interfaces/base.rb

@@ -1,19 +1,6 @@
 module NXT
   module Interface
     class Base
-      def send_and_receive(msg, response_required = true)
-        unless response_required
-          msg[0] = msg[0] | 0x80
-        end
-
-        self.send(msg)
-
-        if response_required
-          response = self.receive
-          response
-        end
-      end
-
       def send
         raise InterfaceNotImplemented.new('The #send method must be implemented.')
       end

lib/nxt/interfaces/serial_port.rb

@@ -69,7 +69,7 @@ def receive
         #
         # Reference: Appendix 1, Page 22
         length = @connection.sysread(2)
-        @connection.sysread(length.unpack('v')[0])
+        @connection.sysread(length.unpack('v')[0]).from_hex_str
       end
     end
   end

lib/nxt/interfaces/usb.rb

@@ -44,7 +44,7 @@ def send(msg)
       end
 
       def receive
-        @connection.bulk_transfer(endpoint: IN_ENDPOINT, dataIn: READSIZE, timeout: TIMEOUT)
+        @connection.bulk_transfer(endpoint: IN_ENDPOINT, dataIn: READSIZE, timeout: TIMEOUT).from_hex_str
       end
     end
   end

lib/nxt/patches/string.rb

@@ -17,13 +17,6 @@ def from_hex_str_two
   end
 
   def from_hex_str
-    data = self.split(' ')
-    str = ''
-
-    data.each do |h|
-      str += '%c' % h
-    end
-
-    str
+    self.unpack('C*')
   end
 end

lib/nxt/protocols/i2c.rb

@@ -0,0 +1,87 @@
+module NXT
+  module Protocols
+    module I2C
+      COMMAND_IDENTIFIER = {
+        ls_get_status: 0x0E,
+        ls_write: 0x0F,
+        ls_read: 0x10
+      }.freeze
+
+      I2C_HEADER = 0x02
+
+      # Format is I2C address, followed by the number of bytes expected in the response.
+      I2C_CONSTANT_OPS = {
+        read_version: [0x00, 8],
+        read_product_id: [0x08, 8],
+        read_sensor_type: [0x10, 8],
+        read_factory_zero: [0x11, 1],
+        read_factory_scale_factor: [0x12, 1],
+        read_factory_scale_divisor: [0x13, 1],
+        read_measurement_units: [0x14, 7]
+      }.freeze
+
+      # Format is the I2C address (all variable operations expect a 1 byte reponse).
+      I2C_VARIABLE_OPS = {
+        read_continuous_measurements_interval: 0x40,
+        read_command_state: 0x41,
+        read_measurement_byte_0: 0x42,
+        read_measurement_byte_1: 0x43,
+        read_measurement_byte_2: 0x44,
+        read_measurement_byte_3: 0x45,
+        read_measurement_byte_4: 0x46,
+        read_measurement_byte_5: 0x47,
+        read_measurement_byte_6: 0x48,
+        read_measurement_byte_7: 0x49,
+        read_actual_zero: 0x50,
+        read_actual_scale_factor: 0x51,
+        read_actual_scale_divisor: 0x52
+      }.freeze
+
+      # Format is I2C address, followed by the command. If the array is only
+      # one member large, then the second byte is instead a value passed at 
+      # run-time, eg. the length of the interval to run the continuous
+      # measuring on the ultrasonic sensor for.
+      I2C_COMMANDS = {
+        off_command: [0x41, 0x00],
+        single_shot_command: [0x41, 0x01],
+        continuous_measurement_command: [0x41, 0x02],
+        event_capture_command: [0x41, 0x03],
+        request_warm_reset: [0x41, 0x04],
+        set_continuous_measurement_interval: [0x40],
+        set_actual_zero: [0x50],
+        set_actual_scale_factor: [0x51],
+        set_actual_scale_divisor: [0x52]
+      }.freeze
+
+      def self.method_missing(name, *args)
+        data = []
+
+        if I2C_CONSTANT_OPS.has_key?(name)
+          op = I2C_CONSTANT_OPS[name]
+          addr = op[0]
+          rx_len = op[1]
+        elsif I2C_VARIABLE_OPS.has_key?(name)
+          op = I2C_VARIABLE_OPS[name]
+          addr = op
+          rx_len = 1
+        elsif I2C_COMMANDS.has_key?(name)
+          op = I2C_COMMANDS[name]
+          addr = op[0]
+          rx_len = 0
+
+          if op[1]
+            data = [op[1]]
+          elsif args[0]
+            data = [args[0]]
+          else
+            raise "Missing argument for command #{name}"
+          end
+        else
+          raise "Unknown ultrasonic sensor command: #{name}"
+        end
+
+        [data.size, rx_len, I2C_HEADER, addr] + data
+      end
+    end
+  end
+end