artoo 0.1.3 → 0.2.0

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,58 @@
1
+ require 'artoo/drivers/driver'
2
+
3
+ module Artoo
4
+ module Drivers
5
+ # L293 or other H-bridge style motor driver behaviors for Firmata
6
+ class Motor < Driver
7
+ attr_reader :leg1_pin, :leg2_pin, :speed_pin, :current_speed
8
+
9
+ def initialize(params={})
10
+ super
11
+
12
+ raise "Invalid pins, please pass an array in format [leg1, leg2, speed]" unless (pin && pin.is_a?(Array) && pin.size == 3)
13
+ @leg1_pin = pin[0]
14
+ @leg2_pin = pin[1]
15
+ @speed_pin = pin[2]
16
+ @current_speed = 0
17
+ end
18
+
19
+ def start_driver
20
+ every(interval) do
21
+ connection.read_and_process
22
+ end
23
+
24
+ super
25
+ end
26
+
27
+ def forward(s)
28
+ set_legs(Firmata::Board::LOW, Firmata::Board::HIGH)
29
+ speed(s)
30
+ end
31
+
32
+ def backward(s)
33
+ set_legs(Firmata::Board::HIGH, Firmata::Board::LOW)
34
+ speed(s)
35
+ end
36
+
37
+ def stop
38
+ speed(0)
39
+ end
40
+
41
+ def speed(s)
42
+ raise "Motor speed must be an integer between 0-255" unless (s.is_a?(Numeric) && s >= 0 && s <= 255)
43
+ @current_speed = s
44
+ connection.set_pin_mode(speed_pin, Firmata::Board::PWM)
45
+ connection.analog_write(speed_pin, s)
46
+ end
47
+
48
+ private
49
+
50
+ def set_legs(l1, l2)
51
+ connection.set_pin_mode(leg1_pin, Firmata::Board::OUTPUT)
52
+ connection.digital_write(leg1_pin, l1)
53
+ connection.set_pin_mode(leg2_pin, Firmata::Board::OUTPUT)
54
+ connection.digital_write(leg2_pin, l2)
55
+ end
56
+ end
57
+ end
58
+ end
@@ -0,0 +1,155 @@
1
+ require 'artoo/drivers/driver'
2
+
3
+ module Artoo
4
+ module Drivers
5
+ # The Roomba driver behaviors
6
+ class Roomba < Driver
7
+
8
+ STRAIGHT = 32768
9
+ CLOCKWISE = 65535
10
+ COUNTERCLOCKWISE = 1
11
+ MAX = 500
12
+ SLOW = 250
13
+ NEG = (65536 - 250)
14
+ ZERO = 0
15
+
16
+ B = 95
17
+ D = 98
18
+ G = 91
19
+ C = 96
20
+ A = 93
21
+ QUART = 16
22
+ HALF = 57
23
+ WHOLE = 114
24
+ START = 128
25
+
26
+ module Modes
27
+ FULL = 132
28
+ SAFE = 131
29
+ end
30
+
31
+ def start
32
+ send_bytes(START)
33
+ sleep 0.2
34
+ end
35
+
36
+ def safe_mode
37
+ start
38
+ send_bytes(Modes::SAFE)
39
+ sleep 0.1
40
+ end
41
+
42
+ def full_mode
43
+ start
44
+ send_bytes(Modes::FULL)
45
+ sleep 0.1
46
+ end
47
+
48
+ def forward(seconds, velocity = SLOW)
49
+ drive(velocity,STRAIGHT,seconds)
50
+ stop if seconds > 0
51
+ end
52
+
53
+ def stop
54
+ drive(ZERO,STRAIGHT)
55
+ end
56
+
57
+ def fast_forward(seconds)
58
+ drive(MAX,STRAIGHT,seconds)
59
+ stop if seconds > 0
60
+ end
61
+
62
+ def backwards(seconds)
63
+ drive(NEG,STRAIGHT,seconds)
64
+ stop if seconds > 0
65
+ end
66
+
67
+ def nudge_left
68
+ turn_left(0.25)
69
+ end
70
+
71
+ def turn_left(seconds = 1)
72
+ drive(SLOW,COUNTERCLOCKWISE,seconds)
73
+ stop if seconds > 0
74
+ end
75
+
76
+ def turn_right(seconds = 1)
77
+ drive(SLOW,CLOCKWISE,seconds)
78
+ stop if seconds > 0
79
+ end
80
+
81
+ def nudge_right
82
+ turn_right(0.25)
83
+ end
84
+
85
+ def turn_around
86
+ turn_left(1.6)
87
+ end
88
+
89
+ def beep
90
+ notes = [140,0,1,G,WHOLE]
91
+ connection.send_bytes(notes)
92
+ connection.send_bytes([141,0])
93
+ end
94
+
95
+ def sing_jingle_bells
96
+ song0 = [[B,QUART],[B,QUART],[B,HALF],
97
+ [B,QUART],[B,QUART],[B,HALF],
98
+ [B,QUART],[D,QUART],[G,QUART],[A,QUART],
99
+ [B,WHOLE]]
100
+ song1 = [[C,QUART],[C,QUART],[C,QUART],[C,QUART],
101
+ [C,QUART],[B,QUART],[B,HALF],
102
+ [B,QUART],[A,QUART],[A,QUART],[B,QUART],
103
+ [A,HALF],[D,HALF]]
104
+ song2 = [[B,QUART],[B,QUART],[B,HALF],
105
+ [B,QUART],[B,QUART],[B,HALF],
106
+ [B,QUART],[D,QUART],[G,QUART],[A,QUART],
107
+ [B,WHOLE]]
108
+ song3 = [[C,QUART],[C,QUART],[C,QUART],[C,QUART],
109
+ [C,QUART],[B,QUART],[B,QUART],[B,QUART],
110
+ [D,QUART],[D,QUART],[C,QUART],[A,QUART],
111
+ [G,WHOLE]]
112
+
113
+ note_group = song0.flatten.compact
114
+ l = note_group.length / 2
115
+ notes = [140,0,l] + note_group
116
+ connection.send_bytes(notes)
117
+
118
+ note_group = song1.flatten.compact
119
+ l = note_group.length / 2
120
+ notes = [140,1,l] + note_group
121
+ connection.send_bytes(notes)
122
+
123
+ note_group = song2.flatten.compact
124
+ l = note_group.length / 2
125
+ notes = [140,2,l] + note_group
126
+ connection.send_bytes(notes)
127
+
128
+ note_group = song3.flatten.compact
129
+ l = note_group.length / 2
130
+ notes = [140,3,l] + note_group
131
+ connection.send_bytes(notes)
132
+
133
+ connection.send_bytes([141,0])
134
+ sleep(7)
135
+ connection.send_bytes([141,1])
136
+ sleep(7)
137
+ connection.send_bytes([141,2])
138
+ sleep(7)
139
+ connection.send_bytes([141,3])
140
+ end
141
+
142
+ def drive(v,r,s = 0)
143
+ vH,vL = split_bytes(v)
144
+ rH,rL = split_bytes(r)
145
+ connection.send_bytes([137,vH,vL,rH,rL])
146
+ sleep(s) if s > 0
147
+ end
148
+
149
+ def split_bytes(num)
150
+ [num >> 8, num & 255]
151
+ end
152
+
153
+ end
154
+ end
155
+ end
@@ -0,0 +1,49 @@
1
+ require 'artoo/drivers/driver'
2
+
3
+ module Artoo
4
+ module Drivers
5
+ # Servo behaviors for Firmata
6
+ class Servo < Driver
7
+ attr_reader :current_angle
8
+
9
+ def initialize(params={})
10
+ super
11
+
12
+ @current_angle = 0
13
+ end
14
+
15
+ def start_driver
16
+ every(interval) do
17
+ connection.read_and_process
18
+ end
19
+
20
+ super
21
+ end
22
+
23
+ def move(angle)
24
+ raise "Servo angle must be an integer between 0-180" unless (angle.is_a?(Numeric) && angle >= 0 && angle <= 180)
25
+
26
+ @current_angle = angle
27
+ connection.set_pin_mode(pin, Firmata::Board::SERVO)
28
+ connection.analog_write(pin, angle_to_span(angle))
29
+ end
30
+
31
+ def min
32
+ move(0)
33
+ end
34
+
35
+ def center
36
+ move(90)
37
+ end
38
+
39
+ def max
40
+ move(180)
41
+ end
42
+
43
+ # converts an angle to a span between 0-255
44
+ def angle_to_span(angle)
45
+ (angle * 255 / 180).to_i
46
+ end
47
+ end
48
+ end
49
+ end
@@ -4,26 +4,105 @@ module Artoo
4
4
  module Drivers
5
5
  # Wiichuck driver behaviors for Firmata
6
6
  class Wiichuck < Driver
7
+ attr_reader :joystick
8
+
7
9
  def address; 0x52; end
8
10
 
11
+ INITIAL_DEFAULTS = {
12
+ :sy_origin => nil,
13
+ :sx_origin => nil
14
+ }
15
+
16
+ def initialize(params={})
17
+ @joystick = INITIAL_DEFAULTS
18
+ super
19
+ end
20
+
9
21
  def start_driver
22
+ begin
10
23
  listener = ->(value) { update(value) }
11
24
  connection.on("i2c_reply", listener)
12
25
 
13
- connection.i2c_config(10)
14
- connection.i2c_request(address, 0x40, 0x00)
15
-
26
+ connection.i2c_config(0)
16
27
  every(interval) do
17
- connection.i2c_request(address, 0x00)
28
+ connection.i2c_write_request(address, 0x40, 0x00)
29
+ p
30
+ connection.i2c_write_request(address, 0x00, 0x00)
31
+ p
32
+ connection.i2c_read_request(address, 6)
33
+ p
18
34
  connection.read_and_process
19
35
  end
20
-
36
+
21
37
  super
38
+ rescue Exception => e
39
+ p "start driver"
40
+ p e.message
41
+ p e.backtrace.inspect
42
+ end
43
+
22
44
  end
23
45
 
24
46
  def update(value)
25
- publish("#{parent.name}_update", value)
47
+ begin
48
+ if encrypted?(value)
49
+ Logger.error "Encrypted bytes from wiichuck!"
50
+ return
51
+ end
52
+
53
+ data = parse_wiichuck(value)
54
+
55
+ adjust_origins(data)
56
+ update_buttons(data)
57
+ update_joystick(data)
58
+
59
+ rescue Exception => e
60
+ Logger.error "wiichuck update exception!"
61
+ Logger.error e.message
62
+ Logger.error e.backtrace.inspect
63
+ end
64
+ end
65
+
66
+ def adjust_origins(data)
67
+ set_joystick_default_value(:sy_origin, data[:sy])
68
+ set_joystick_default_value(:sx_origin, data[:sx])
69
+ end
70
+
71
+ def set_joystick_default_value(joystick_axis, default_value)
72
+ joystick[joystick_axis] = default_value if joystick[joystick_axis].nil?
73
+ end
74
+
75
+ def update_buttons(data)
76
+ publish(event_topic_name("c_button")) if data[:c] == true
77
+ publish(event_topic_name("z_button")) if data[:z] == true
78
+ end
79
+
80
+ def update_joystick(data)
81
+ publish(event_topic_name("joystick"), {:x => data[:sx] - @joystick[:sx_origin], :y => data[:sy] - @joystick[:sy_origin]})
82
+ end
83
+
84
+ private
85
+
86
+ def encrypted?(value)
87
+ value[:data][0] == value[:data][1] && value[:data][2] == value[:data][3] && value[:data][4] == value[:data][5]
88
+ end
89
+
90
+ def decode( x )
91
+ return ( x ^ 0x17 ) + 0x17
92
+ end
93
+
94
+ def get_value(value, index)
95
+ decode(value[:data][index])
96
+ end
97
+
98
+ def parse_wiichuck(value)
99
+ return {
100
+ :sx => get_value(value, 0),
101
+ :sy => get_value(value, 1),
102
+ :z => (get_value(value, 5) & 0x01 == 0 ? true : false ),
103
+ :c => (get_value(value, 5) & 0x02 == 0 ? true : false )
104
+ }
26
105
  end
27
106
  end
28
107
  end
29
- end
108
+ end
@@ -2,26 +2,28 @@ require 'artoo/drivers/driver'
2
2
 
3
3
  module Artoo
4
4
  module Drivers
5
- # Wiichuck driver behaviors for Firmata
5
+ # Wiiclassic driver behaviors for Firmata
6
6
  class Wiiclassic < Driver
7
+ attr_reader :joystick
8
+
7
9
  def address; 0x52; end
8
10
 
9
11
  INITIAL_DEFAULTS = {
10
- :ry_offset => 8,
11
- :ry_origin => nil,
12
- :ly_offset => 20,
13
- :lx_offset => 20,
14
- :ly_origin => nil,
15
- :lx_origin => nil,
16
- :rt_origin => nil,
17
- :lt_origin => nil,
18
- :rt_offset => 5,
19
- :lt_offset => 5
20
- }
12
+ :ry_origin => nil,
13
+ :rx_origin => nil,
14
+ :ly_origin => nil,
15
+ :lx_origin => nil,
16
+ :rt_origin => nil,
17
+ :lt_origin => nil
18
+ }
19
+
20
+ def initialize(params={})
21
+ @joystick = INITIAL_DEFAULTS
22
+ super
23
+ end
21
24
 
22
25
  def start_driver
23
26
  begin
24
- @joystick = INITIAL_DEFAULTS
25
27
  listener = ->(value) { update(value) }
26
28
  connection.on("i2c_reply", listener)
27
29
 
@@ -47,70 +49,68 @@ module Artoo
47
49
 
48
50
  def update(value)
49
51
  begin
50
- if value[:data][0] == value[:data][1] && value[:data][2] == value[:data][3] && value[:data][4] == value[:data][5]
52
+ if encrypted?(value)
51
53
  Logger.error "Encrypted bytes from wiiclassic!"
52
54
  return
53
55
  end
54
56
 
55
57
  data = parse_wiiclassic(value)
56
58
 
57
- publish(event_topic_name("a_button")) if data[:a] == 0
58
- publish(event_topic_name("b_button")) if data[:b] == 0
59
- publish(event_topic_name("x_button")) if data[:x] == 0
60
- publish(event_topic_name("y_button")) if data[:y] == 0
61
- publish(event_topic_name("home_button")) if data[:h] == 0
62
- publish(event_topic_name("start_button")) if data[:+] == 0
63
- publish(event_topic_name("select_button")) if data[:-] == 0
64
-
65
- @joystick[:ly_origin] = data[:ly] if @joystick[:ly_origin].nil?
66
- @joystick[:lx_origin] = data[:lx] if @joystick[:lx_origin].nil?
67
-
68
- @joystick[:ry_origin] = data[:ry] if @joystick[:ry_origin].nil?
69
-
70
- @joystick[:rt_origin] = data[:rt] if @joystick[:rt_origin].nil?
71
- @joystick[:lt_origin] = data[:lt] if @joystick[:lt_origin].nil?
72
-
73
- update_left_joystick
74
-
75
- if data[:ry] > (@joystick[:ry_origin] + @joystick[:ry_offset])
76
- publish(event_topic_name("ry_up"))
77
- elsif data[:ry] < (@joystick[:ry_origin] - @joystick[:ry_offset])
78
- publish(event_topic_name("ry_down"))
79
- else
80
- publish(event_topic_name("reset_altitude"))
81
- end
82
-
83
- if data[:rt] > (@joystick[:rt_origin] + @joystick[:rt_offset])
84
- publish(event_topic_name("rotate_right"))
85
- elsif data[:lt] > (@joystick[:lt_origin] + @joystick[:lt_offset])
86
- publish(event_topic_name("rotate_left"))
87
- else
88
- publish(event_topic_name("reset_rotate"))
89
- end
59
+ adjust_origins(data)
60
+ update_buttons(data)
61
+ update_left_joystick(data)
62
+ update_right_joystick(data)
63
+ update_triggers(data)
90
64
 
91
65
  rescue Exception => e
92
- p "wiiclassic update exception!"
93
- p e.message
94
- p e.backtrace.inspect
66
+ Logger.error "wiiclassic update exception!"
67
+ Logger.error e.message
68
+ Logger.error e.backtrace.inspect
95
69
  end
96
70
  end
97
71
 
98
- def update_left_joystick
99
- if data[:ly] > (@joystick[:ly_origin] + @joystick[:ly_offset])
100
- publish(event_topic_name("ly_up"))
101
- elsif data[:ly] < (@joystick[:ly_origin] - @joystick[:ly_offset])
102
- publish(event_topic_name("ly_down"))
103
- elsif data[:lx] > (@joystick[:lx_origin] + @joystick[:lx_offset])
104
- publish(event_topic_name("lx_right"))
105
- elsif data[:lx] < (@joystick[:lx_origin] - @joystick[:lx_offset])
106
- publish(event_topic_name("lx_left"))
107
- else
108
- publish(event_topic_name("reset_pitch_roll")) # TODO: rename something not so drone specfic
109
- end
72
+ def adjust_origins(data)
73
+ set_joystick_default_value(:ly_origin, data[:ly])
74
+ set_joystick_default_value(:lx_origin, data[:lx])
75
+ set_joystick_default_value(:ry_origin, data[:ry])
76
+ set_joystick_default_value(:rx_origin, data[:rx])
77
+ set_joystick_default_value(:rt_origin, data[:rt])
78
+ set_joystick_default_value(:lt_origin, data[:lt])
79
+ end
80
+
81
+ def set_joystick_default_value(joystick_axis, default_value)
82
+ joystick[joystick_axis] = default_value if joystick[joystick_axis].nil?
83
+ end
84
+
85
+ def update_buttons(data)
86
+ publish(event_topic_name("a_button")) if data[:a] == true
87
+ publish(event_topic_name("b_button")) if data[:b] == true
88
+ publish(event_topic_name("x_button")) if data[:x] == true
89
+ publish(event_topic_name("y_button")) if data[:y] == true
90
+ publish(event_topic_name("home_button")) if data[:h] == true
91
+ publish(event_topic_name("start_button")) if data[:+] == true
92
+ publish(event_topic_name("select_button")) if data[:-] == true
93
+ end
94
+
95
+ def update_left_joystick(data)
96
+ publish(event_topic_name("left_joystick"), {:x => data[:lx] - @joystick[:lx_origin], :y => data[:ly] - @joystick[:ly_origin]})
97
+ end
98
+
99
+ def update_right_joystick(data)
100
+ publish(event_topic_name("right_joystick"), {:x => data[:rx] - @joystick[:rx_origin], :y => data[:ry] - @joystick[:ry_origin]})
101
+ end
102
+
103
+ def update_triggers(data)
104
+ publish(event_topic_name("right_trigger"), data[:rt] - @joystick[:rt_origin])
105
+ publish(event_topic_name("left_trigger"), data[:lt] - @joystick[:lt_origin])
110
106
  end
111
107
 
112
108
  private
113
109
 
110
+ def encrypted?(value)
111
+ value[:data][0] == value[:data][1] && value[:data][2] == value[:data][3] && value[:data][4] == value[:data][5]
112
+ end
113
+
114
114
  def decode( x )
115
115
  return ( x ^ 0x17 ) + 0x17
116
116
  end
@@ -125,23 +125,27 @@ module Artoo
125
125
  :ly => get_value(value, 1) & 0x3f,
126
126
  :rx => ((get_value(value, 0) & 0xC0) >> 2) | ((get_value(value, 1) & 0xC0) >> 4) | (get_value(value, 2)[7]),
127
127
  :ry => get_value(value, 2) & 0x1f,
128
- :lt => ((get_value(value, 2) & 0x60) >> 3) | ((get_value(value, 3) & 0xE0) >> 6),
128
+ :lt => ((get_value(value, 2) & 0x60) >> 3) | ((get_value(value, 3) & 0xC0) >> 6),
129
129
  :rt => get_value(value, 3) & 0x1f,
130
- :d_up => get_value(value, 5)[0],
131
- :d_down => get_value(value, 4)[6],
132
- :D_left => get_value(value, 5)[1],
133
- :D_right => get_value(value, 4)[7],
134
- :zr => get_value(value, 5)[2],
135
- :zl => get_value(value, 5)[7],
136
- :a => get_value(value, 5)[4],
137
- :b => get_value(value, 5)[6],
138
- :x => get_value(value, 5)[3],
139
- :y => get_value(value, 5)[5],
140
- :+ => get_value(value, 4)[2],
141
- :- => get_value(value, 4)[4],
142
- :h => get_value(value, 4)[3],
130
+ :d_up => generate_bool(get_value(value, 5)[0]),
131
+ :d_down => generate_bool(get_value(value, 4)[6]),
132
+ :d_left => generate_bool(get_value(value, 5)[1]),
133
+ :d_right => generate_bool(get_value(value, 4)[7]),
134
+ :zr => generate_bool(get_value(value, 5)[2]),
135
+ :zl => generate_bool(get_value(value, 5)[7]),
136
+ :a => generate_bool(get_value(value, 5)[4]),
137
+ :b => generate_bool(get_value(value, 5)[6]),
138
+ :x => generate_bool(get_value(value, 5)[3]),
139
+ :y => generate_bool(get_value(value, 5)[5]),
140
+ :+ => generate_bool(get_value(value, 4)[2]),
141
+ :- => generate_bool(get_value(value, 4)[4]),
142
+ :h => generate_bool(get_value(value, 4)[3]),
143
143
  }
144
144
  end
145
+
146
+ def generate_bool(value)
147
+ value == 0 ? true : false
148
+ end
145
149
  end
146
150
  end
147
151
  end