dwm1000: correct spelling errors
[ros_wild_thumper.git] / scripts / ledstripe.rb
1 #!/usr/bin/env ruby
2
3 require "spi"
4 require "ros"
5 require 'wild_thumper/LedStripe'
6
7 class LPD8806
8         def initialize(device, num_leds)
9                 @spi = SPI.new(device: device)
10                 @spi.driver.mode=[0b00]
11                 @spi.speed=2e6
12                 @num_leds = num_leds
13                 self.latch()
14                 @l = [[0, 0, 0]] * num_leds
15                 self.update()
16         end
17         
18         def set(i, red=0, green=0, blue=0)
19                 if red > 127 or green > 127 or blue > 127 or red < 0 or green < 0 or blue < 0
20                         raise "Bad RGB Value"
21                 end
22                 @l[i] = [red, green, blue]
23         end
24
25         def latch
26                 @spi.xfer(txdata: (0...(@num_leds+31)/32).map { |i| 0 })
27         end
28         
29         def update
30                 l = []
31                 (0...@num_leds).each do |i|
32                         red, green, blue = @l[i]
33                         l.push(0x80 | green)
34                         l.push(0x80 | red)
35                         l.push(0x80 | blue)
36                 end
37                 @spi.xfer(txdata: l)
38                 self.latch()
39         end
40 end
41
42 if __FILE__ == $0
43         def test
44                 puts "Starting.."
45                 (0...16).each do |i|
46                         stripe.set(i, 10, 0, 0)
47                         stripe.update()
48                         sleep(0.1)
49                 end
50                 (0...16).each do |i|
51                         stripe.set(i, 0, 10, 0)
52                         stripe.update()
53                         sleep(0.1)
54                 end
55                 (0...16).each do |i|
56                         stripe.set(i, 0, 0, 10)
57                         stripe.update()
58                         sleep(0.1)
59                 end
60                 (0...16).each do |i|
61                         stripe.set(i, 0, 0, 0)
62                 end
63                 stripe.update()
64         end
65
66         stripe = LPD8806.new("/dev/spidev1.0", 16)
67         node = ROS::Node.new('wild_thumper/led_stripe')
68         node.subscribe('led_stripe', Wild_thumper::LedStripe) do |msg|
69                 msg.leds.each do |led|
70                         stripe.set(led.num, red=led.red, green=led.green, blue=led.blue)
71                         stripe.update()
72                 end
73         end
74
75         while node.ok?
76                 node.spin
77         end
78 end