artoo 0.1.3 → 0.2.0

data/lib/artoo/drivers/motor.rb

@@ -0,0 +1,58 @@
+require 'artoo/drivers/driver'
+
+module Artoo
+  module Drivers
+    # L293 or other H-bridge style motor driver behaviors for Firmata
+    class Motor < Driver
+      attr_reader :leg1_pin, :leg2_pin, :speed_pin, :current_speed
+
+      def initialize(params={})
+        super
+
+        raise "Invalid pins, please pass an array in format [leg1, leg2, speed]" unless (pin && pin.is_a?(Array) && pin.size == 3)
+        @leg1_pin = pin[0]
+        @leg2_pin = pin[1]
+        @speed_pin = pin[2]
+        @current_speed = 0
+      end
+
+      def start_driver
+        every(interval) do
+          connection.read_and_process
+        end
+
+        super
+      end
+
+      def forward(s)
+        set_legs(Firmata::Board::LOW, Firmata::Board::HIGH)
+        speed(s)
+      end
+
+      def backward(s)
+        set_legs(Firmata::Board::HIGH, Firmata::Board::LOW)
+        speed(s)
+      end
+
+      def stop
+        speed(0)
+      end
+
+      def speed(s)
+        raise "Motor speed must be an integer between 0-255" unless (s.is_a?(Numeric) && s >= 0 && s <= 255)
+        @current_speed = s
+        connection.set_pin_mode(speed_pin, Firmata::Board::PWM)
+        connection.analog_write(speed_pin, s)
+      end
+
+      private
+
+      def set_legs(l1, l2)
+        connection.set_pin_mode(leg1_pin, Firmata::Board::OUTPUT)
+        connection.digital_write(leg1_pin, l1)
+        connection.set_pin_mode(leg2_pin, Firmata::Board::OUTPUT)
+        connection.digital_write(leg2_pin, l2)
+      end
+    end
+  end
+end

data/lib/artoo/drivers/roomba.rb

@@ -0,0 +1,155 @@
+require 'artoo/drivers/driver'
+
+module Artoo
+  module Drivers
+    # The Roomba driver behaviors
+    class Roomba < Driver
+      
+      STRAIGHT = 32768
+      CLOCKWISE = 65535
+      COUNTERCLOCKWISE = 1
+      MAX = 500
+      SLOW = 250
+      NEG = (65536 - 250)
+      ZERO = 0
+  
+      B = 95
+      D = 98
+      G = 91
+      C = 96
+      A = 93
+      QUART = 16
+      HALF = 57
+      WHOLE = 114
+      START = 128
+      
+      module Modes
+        FULL = 132
+        SAFE = 131
+      end
+      
+      def start
+        send_bytes(START)
+        sleep 0.2
+      end
+      
+      def safe_mode
+        start
+        send_bytes(Modes::SAFE)
+        sleep 0.1
+      end
+      
+      def full_mode
+        start
+        send_bytes(Modes::FULL)
+        sleep 0.1
+      end
+      
+      def forward(seconds, velocity = SLOW)
+        drive(velocity,STRAIGHT,seconds)
+        stop if seconds > 0
+      end
+  
+      def stop
+        drive(ZERO,STRAIGHT)
+      end
+  
+      def fast_forward(seconds)
+        drive(MAX,STRAIGHT,seconds)
+        stop if seconds > 0
+      end
+  
+      def backwards(seconds)
+        drive(NEG,STRAIGHT,seconds)
+        stop if seconds > 0
+      end
+  
+      def nudge_left
+        turn_left(0.25)
+      end
+  
+      def turn_left(seconds = 1)
+        drive(SLOW,COUNTERCLOCKWISE,seconds)
+        stop if seconds > 0
+      end
+  
+      def turn_right(seconds = 1)
+        drive(SLOW,CLOCKWISE,seconds)
+        stop if seconds > 0
+      end
+  
+      def nudge_right
+        turn_right(0.25)
+      end
+  
+      def turn_around
+        turn_left(1.6)
+      end
+  
+      def beep
+        notes = [140,0,1,G,WHOLE]
+        connection.send_bytes(notes)
+        connection.send_bytes([141,0])
+      end
+  
+      def sing_jingle_bells
+        song0 = [[B,QUART],[B,QUART],[B,HALF],
+                [B,QUART],[B,QUART],[B,HALF],
+                [B,QUART],[D,QUART],[G,QUART],[A,QUART],
+                [B,WHOLE]]
+        song1 = [[C,QUART],[C,QUART],[C,QUART],[C,QUART],
+                [C,QUART],[B,QUART],[B,HALF],
+                [B,QUART],[A,QUART],[A,QUART],[B,QUART],
+                [A,HALF],[D,HALF]]
+        song2 = [[B,QUART],[B,QUART],[B,HALF],
+                [B,QUART],[B,QUART],[B,HALF],
+                [B,QUART],[D,QUART],[G,QUART],[A,QUART],
+                [B,WHOLE]]
+        song3 = [[C,QUART],[C,QUART],[C,QUART],[C,QUART],
+                [C,QUART],[B,QUART],[B,QUART],[B,QUART],
+                [D,QUART],[D,QUART],[C,QUART],[A,QUART],
+                [G,WHOLE]]
+            
+        note_group = song0.flatten.compact
+        l = note_group.length / 2
+        notes = [140,0,l] + note_group
+        connection.send_bytes(notes)
+    
+        note_group = song1.flatten.compact
+        l = note_group.length / 2
+        notes = [140,1,l] + note_group
+        connection.send_bytes(notes)
+    
+        note_group = song2.flatten.compact
+        l = note_group.length / 2
+        notes = [140,2,l] + note_group
+        connection.send_bytes(notes)
+    
+        note_group = song3.flatten.compact
+        l = note_group.length / 2
+        notes = [140,3,l] + note_group
+        connection.send_bytes(notes)
+    
+        connection.send_bytes([141,0])
+        sleep(7)
+        connection.send_bytes([141,1])
+        sleep(7)
+        connection.send_bytes([141,2])
+        sleep(7)
+        connection.send_bytes([141,3])
+      end
+      
+      def drive(v,r,s = 0)
+        vH,vL = split_bytes(v)
+        rH,rL = split_bytes(r)
+        connection.send_bytes([137,vH,vL,rH,rL])
+        sleep(s) if s > 0
+      end
+      
+      def split_bytes(num)
+        [num >> 8, num & 255]
+      end
+      
+    end
+  end
+end

data/lib/artoo/drivers/servo.rb

@@ -0,0 +1,49 @@
+require 'artoo/drivers/driver'
+
+module Artoo
+  module Drivers
+    # Servo behaviors for Firmata
+    class Servo < Driver
+      attr_reader :current_angle
+
+      def initialize(params={})
+        super
+
+        @current_angle = 0
+      end
+
+      def start_driver
+        every(interval) do
+          connection.read_and_process
+        end
+
+        super
+      end
+
+      def move(angle)
+        raise "Servo angle must be an integer between 0-180" unless (angle.is_a?(Numeric) && angle >= 0 && angle <= 180)
+        
+        @current_angle = angle
+        connection.set_pin_mode(pin, Firmata::Board::SERVO)
+        connection.analog_write(pin, angle_to_span(angle))
+      end
+
+      def min
+        move(0)
+      end
+
+      def center
+        move(90)
+      end
+
+      def max
+        move(180)
+      end
+
+      # converts an angle to a span between 0-255
+      def angle_to_span(angle)
+        (angle * 255 / 180).to_i
+      end
+    end
+  end
+end

data/lib/artoo/drivers/wiichuck.rb

@@ -4,26 +4,105 @@ module Artoo
   module Drivers
     # Wiichuck driver behaviors for Firmata
     class Wiichuck < Driver
+      attr_reader :joystick
+
       def address; 0x52; end
 
+      INITIAL_DEFAULTS = {
+          :sy_origin => nil,
+          :sx_origin => nil
+        }
+
+      def initialize(params={})
+        @joystick = INITIAL_DEFAULTS
+        super
+      end
+
       def start_driver
+        begin
         listener = ->(value) { update(value) }
         connection.on("i2c_reply", listener)
 
-        connection.i2c_config(10)
-        connection.i2c_request(address, 0x40, 0x00)
-
+        connection.i2c_config(0)
         every(interval) do
-          connection.i2c_request(address, 0x00)
+          connection.i2c_write_request(address, 0x40, 0x00)
+          p
+          connection.i2c_write_request(address, 0x00, 0x00)
+          p
+          connection.i2c_read_request(address, 6)
+          p
           connection.read_and_process
         end
-
+        
         super
+        rescue Exception => e
+          p "start driver"
+          p e.message
+          p e.backtrace.inspect
+        end
+
       end
 
       def update(value)
-        publish("#{parent.name}_update", value)
+        begin
+          if encrypted?(value)
+            Logger.error "Encrypted bytes from wiichuck!"
+            return
+          end
+
+          data = parse_wiichuck(value)
+          
+          adjust_origins(data)
+          update_buttons(data)
+          update_joystick(data)
+
+        rescue Exception => e
+          Logger.error "wiichuck update exception!"
+          Logger.error e.message
+          Logger.error e.backtrace.inspect
+        end
+      end
+
+      def adjust_origins(data)
+        set_joystick_default_value(:sy_origin, data[:sy])
+        set_joystick_default_value(:sx_origin, data[:sx])
+      end
+
+      def set_joystick_default_value(joystick_axis, default_value)     
+        joystick[joystick_axis] = default_value if joystick[joystick_axis].nil?
+      end
+
+      def update_buttons(data)
+        publish(event_topic_name("c_button")) if data[:c] == true
+        publish(event_topic_name("z_button")) if data[:z] == true
+      end
+
+      def update_joystick(data)
+        publish(event_topic_name("joystick"), {:x => data[:sx] - @joystick[:sx_origin], :y => data[:sy] - @joystick[:sy_origin]})
+      end
+
+      private 
+
+      def encrypted?(value)
+        value[:data][0] == value[:data][1] && value[:data][2] == value[:data][3] && value[:data][4] == value[:data][5]
+      end
+
+      def decode( x )
+        return ( x ^ 0x17 ) + 0x17
+      end
+
+      def get_value(value, index)
+        decode(value[:data][index])
+      end
+
+      def parse_wiichuck(value)
+        return {
+          :sx => get_value(value, 0),
+          :sy => get_value(value, 1),
+          :z => (get_value(value, 5) & 0x01 == 0 ? true : false ),
+          :c => (get_value(value, 5) & 0x02 == 0 ? true : false )
+        }
       end
     end
   end
-end
+end

data/lib/artoo/drivers/wiiclassic.rb

@@ -2,26 +2,28 @@ require 'artoo/drivers/driver'
 
 module Artoo
   module Drivers
-    # Wiichuck driver behaviors for Firmata
+    # Wiiclassic driver behaviors for Firmata
     class Wiiclassic < Driver
+      attr_reader :joystick
+
       def address; 0x52; end
 
       INITIAL_DEFAULTS = {
-          :ry_offset => 8,
-          :ry_origin => nil,
-          :ly_offset => 20,
-          :lx_offset => 20,
-          :ly_origin => nil,
-          :lx_origin => nil,
-          :rt_origin => nil,
-          :lt_origin => nil,
-          :rt_offset => 5,
-          :lt_offset => 5
-        }
+        :ry_origin => nil,
+        :rx_origin => nil,
+        :ly_origin => nil,
+        :lx_origin => nil,
+        :rt_origin => nil,
+        :lt_origin => nil
+      }  
+
+      def initialize(params={})
+        @joystick = INITIAL_DEFAULTS
+        super
+      end
 
       def start_driver
         begin
-        @joystick = INITIAL_DEFAULTS
         listener = ->(value) { update(value) }
         connection.on("i2c_reply", listener)
 
@@ -47,70 +49,68 @@ module Artoo
 
       def update(value)
         begin
-          if value[:data][0] == value[:data][1] && value[:data][2] == value[:data][3] && value[:data][4] == value[:data][5]
+          if encrypted?(value)
             Logger.error "Encrypted bytes from wiiclassic!"
             return
           end
 
           data = parse_wiiclassic(value)
           
-          publish(event_topic_name("a_button")) if data[:a] == 0
-          publish(event_topic_name("b_button")) if data[:b] == 0
-          publish(event_topic_name("x_button")) if data[:x] == 0
-          publish(event_topic_name("y_button")) if data[:y] == 0
-          publish(event_topic_name("home_button")) if data[:h] == 0
-          publish(event_topic_name("start_button")) if data[:+] == 0
-          publish(event_topic_name("select_button")) if data[:-] == 0
-
-          @joystick[:ly_origin] = data[:ly] if @joystick[:ly_origin].nil?
-          @joystick[:lx_origin] = data[:lx] if @joystick[:lx_origin].nil?
-
-          @joystick[:ry_origin] = data[:ry] if @joystick[:ry_origin].nil?
-
-          @joystick[:rt_origin] = data[:rt] if @joystick[:rt_origin].nil?
-          @joystick[:lt_origin] = data[:lt] if @joystick[:lt_origin].nil?
-
-          update_left_joystick
-
-          if data[:ry] > (@joystick[:ry_origin] + @joystick[:ry_offset])
-            publish(event_topic_name("ry_up"))
-          elsif data[:ry] < (@joystick[:ry_origin] - @joystick[:ry_offset])
-            publish(event_topic_name("ry_down"))
-          else
-            publish(event_topic_name("reset_altitude"))
-          end
-
-          if data[:rt] > (@joystick[:rt_origin] + @joystick[:rt_offset])
-            publish(event_topic_name("rotate_right"))
-          elsif data[:lt] > (@joystick[:lt_origin] + @joystick[:lt_offset])
-            publish(event_topic_name("rotate_left"))
-          else
-            publish(event_topic_name("reset_rotate"))
-          end
+          adjust_origins(data)
+          update_buttons(data)
+          update_left_joystick(data)
+          update_right_joystick(data)
+          update_triggers(data)
 
         rescue Exception => e
-          p "wiiclassic update exception!"
-          p e.message
-          p e.backtrace.inspect
+          Logger.error "wiiclassic update exception!"
+          Logger.error e.message
+          Logger.error e.backtrace.inspect
         end
       end
 
-      def update_left_joystick
-        if data[:ly] > (@joystick[:ly_origin] + @joystick[:ly_offset])
-          publish(event_topic_name("ly_up"))
-        elsif data[:ly] < (@joystick[:ly_origin] - @joystick[:ly_offset])
-          publish(event_topic_name("ly_down"))
-        elsif data[:lx] > (@joystick[:lx_origin] + @joystick[:lx_offset])
-          publish(event_topic_name("lx_right"))
-        elsif data[:lx] < (@joystick[:lx_origin] - @joystick[:lx_offset])
-          publish(event_topic_name("lx_left"))
-        else
-          publish(event_topic_name("reset_pitch_roll")) # TODO: rename something not so drone specfic
-        end
+      def adjust_origins(data)
+        set_joystick_default_value(:ly_origin, data[:ly])
+        set_joystick_default_value(:lx_origin, data[:lx])
+        set_joystick_default_value(:ry_origin, data[:ry])
+        set_joystick_default_value(:rx_origin, data[:rx])
+        set_joystick_default_value(:rt_origin, data[:rt])
+        set_joystick_default_value(:lt_origin, data[:lt])
+      end
+
+      def set_joystick_default_value(joystick_axis, default_value)     
+        joystick[joystick_axis] = default_value if joystick[joystick_axis].nil?
+      end
+
+      def update_buttons(data)
+        publish(event_topic_name("a_button")) if data[:a] == true
+        publish(event_topic_name("b_button")) if data[:b] == true
+        publish(event_topic_name("x_button")) if data[:x] == true
+        publish(event_topic_name("y_button")) if data[:y] == true
+        publish(event_topic_name("home_button")) if data[:h] == true
+        publish(event_topic_name("start_button")) if data[:+] == true
+        publish(event_topic_name("select_button")) if data[:-] == true
+      end
+
+      def update_left_joystick(data)
+        publish(event_topic_name("left_joystick"), {:x => data[:lx] - @joystick[:lx_origin], :y => data[:ly] - @joystick[:ly_origin]}) 
+      end
+
+      def update_right_joystick(data)
+        publish(event_topic_name("right_joystick"), {:x => data[:rx] - @joystick[:rx_origin], :y => data[:ry] - @joystick[:ry_origin]}) 
+      end
+
+      def update_triggers(data)
+        publish(event_topic_name("right_trigger"), data[:rt] - @joystick[:rt_origin])
+        publish(event_topic_name("left_trigger"), data[:lt] - @joystick[:lt_origin])
       end
 
       private 
 
+      def encrypted?(value)
+        value[:data][0] == value[:data][1] && value[:data][2] == value[:data][3] && value[:data][4] == value[:data][5]
+      end
+
       def decode( x )
         return ( x ^ 0x17 ) + 0x17
       end
@@ -125,23 +125,27 @@ module Artoo
           :ly => get_value(value, 1) & 0x3f,
           :rx => ((get_value(value, 0) & 0xC0) >> 2)  | ((get_value(value, 1) & 0xC0) >> 4) | (get_value(value, 2)[7]),
           :ry => get_value(value, 2) & 0x1f,
-          :lt => ((get_value(value, 2) & 0x60) >> 3) | ((get_value(value, 3) & 0xE0) >> 6),
+          :lt => ((get_value(value, 2) & 0x60) >> 3) | ((get_value(value, 3) & 0xC0) >> 6),
           :rt => get_value(value, 3) & 0x1f,
-          :d_up => get_value(value, 5)[0],
-          :d_down => get_value(value, 4)[6],
-          :D_left => get_value(value, 5)[1],
-          :D_right => get_value(value, 4)[7],
-          :zr => get_value(value, 5)[2],
-          :zl => get_value(value, 5)[7],
-          :a => get_value(value, 5)[4],
-          :b => get_value(value, 5)[6],
-          :x => get_value(value, 5)[3],
-          :y => get_value(value, 5)[5],
-          :+ => get_value(value, 4)[2],
-          :- => get_value(value, 4)[4],
-          :h => get_value(value, 4)[3],
+          :d_up => generate_bool(get_value(value, 5)[0]),
+          :d_down => generate_bool(get_value(value, 4)[6]),
+          :d_left => generate_bool(get_value(value, 5)[1]),
+          :d_right => generate_bool(get_value(value, 4)[7]),
+          :zr => generate_bool(get_value(value, 5)[2]),
+          :zl => generate_bool(get_value(value, 5)[7]),
+          :a => generate_bool(get_value(value, 5)[4]),
+          :b => generate_bool(get_value(value, 5)[6]),
+          :x => generate_bool(get_value(value, 5)[3]),
+          :y => generate_bool(get_value(value, 5)[5]),
+          :+ => generate_bool(get_value(value, 4)[2]),
+          :- => generate_bool(get_value(value, 4)[4]),
+          :h => generate_bool(get_value(value, 4)[3]),
         }
       end
+
+      def generate_bool(value)
+        value == 0 ? true : false
+      end
     end
   end
 end