Kompass Sensor HDMM01 am NXT

Einen Kompass kann man sehr preiswert selbst realisieren. Das Kompassmodul HDMM01 gibt es aktuell bei Pollin für knapp 7 Euro. Die Programmierung ist etwas aufwändiger als beim fertigen Lego-Sensor von Hitechnic, aber keineswegs kompliziert. Der Sensor liefert jeweils 12 Bit (auf 2 Bytes verteilt) für die Werte x und y (x-und x-Achse). Zur Berechnung des Winkels benötigt man als Kalibrierungswert die minimalen und maximalen Werte von x und y. Das macht man am Besten mit einem Testprogramm und dreht den Sensor ein paar mal und hat dann die min/max-Werte. Wenn man dies hat, ist die Kompassrichtung ganz einfach:

Richtung = atan2(x – (x_max + x_min) / 2,  y – (y_max + y_min) / 2)

Die Beschaltung ist sehr einfach:

• SDA an NXT-Pin 6 (blaues Kabel am NXT)
• SCL an NXT-Pin 5  (gelbes Kabel am NXT)
• VCC an NXT-Pin4 (grünes Kabel am NXT)
• GND an NXT-Pin 3 (rotes Kabel am NXT)
• jeweils einen Pull-up-Widerstand (82k) zwischen SDA und VCC sowie SCL und VCC

Hier ein Beispielprogramm in pbLua. Das sollte in anderen Sprachen ähnlich leicht sein.

-- test compass sensor HDMM01 on NXT
-- 06.08.2013
-- pblua 2.0RC2

-- waitI2C() - sits in a tight loop until the I2C system goes idle
function waitI2C(port)
  while( 0 ~= nxt.I2CGetStatus(port) ) do
  end
end

function delay(ticks)
  local t = nxt.TimerRead()
    repeat
  until(t+ticks < nxt.TimerRead() )
end

port = 1
inputType = 0x0A -- low speed
i2cAddress = 0x60 -- compass sensor

-- init
nxt.InputSetType(port, inputType)
nxt.InputSetDir(port, 1, 1)  --  digi0 and digi1 output
nxt.InputSetState(port, 1, 1) -- 0 sets the pin of digi0 and digi1 high
nxt.I2CInitPins(port)
waitI2C(port)

nxt.I2CSendData( port, string.char(i2cAddress, 0x00, 0x04), 0 ) -- bit 3 -> reset coil
delay(10)
nxt.I2CSendData( port, string.char(i2cAddress, 0x00, 0x02), 0 ) -- bit 2 -> set coil
delay(10)

-- test: read and show bytes
repeat
  nxt.I2CSendData( port, string.char(i2cAddress, 0x00, 0x01), 0) -- bit 0 ->  TM take measurements
  waitI2C(port)
  nxt.I2CSendData( port, string.char(i2cAddress, 0x00), 5 ) -- read 4 bytes
  waitI2C(port)
  s= nxt.I2CRecvData( port, 5 )
  int_reg, x_msb, x_lsb, y_msb, y_lsb = string.byte(s,1,5)
  print( string.format( "bytes: %3i %3i %3i %3i %3i", int_reg, x_msb, x_lsb, y_msb, y_lsb ) )
  x = 256*x_msb + x_lsb;
  y = 256*y_msb + y_lsb;
  print( string.format( "x: %3i y:%3i", x, y ) )
  delay(300)
until( 8 == nxt.ButtonRead() )

-- calibrate
-- turn sensor around to get max and min values of x and y
-- in my case: x min/max: 1920 / 2234  y min/max :1943 / 2174
x_min = 5000
x_max = 0
y_min = 5000
y_max = 0

repeat
  nxt.I2CSendData( port, string.char(i2cAddress, 0x00, 0x01), 0) -- bit 0 ->  TM take measurements
  waitI2C(port)
  nxt.I2CSendData( port, string.char(i2cAddress, 0x00), 5 ) -- read 4 bytes
  waitI2C(port)
  s= nxt.I2CRecvData( port, 5 )
  int_reg, x_msb, x_lsb, y_msb, y_lsb = string.byte(s,1,5)
  x = 256*x_msb + x_lsb;
  y = 256*y_msb + y_lsb;

  if x > x_max then x_max = x end
  if x < x_min and x > 0 then x_min = x end
  if y > y_max then y_max = y end
  if y < y_min and y > 0 then y_min = y end

  print( string.format( "x min/max: %3i / %3i  y min/max :%3i / %3i",
    x_min, x_max, y_min, y_max ) )
  delay(10)
until( 8 == nxt.ButtonRead() )

-- real compass
repeat
  nxt.I2CSendData( port, string.char(i2cAddress, 0x00, 0x01), 0) -- bit 0 ->  TM take measurements
  waitI2C(port)
  nxt.I2CSendData( port, string.char(i2cAddress, 0x00), 5 ) -- read 4 bytes
  waitI2C(port)
  s= nxt.I2CRecvData( port, 5 )
  int_reg, x_msb, x_lsb, y_msb, y_lsb = string.byte(s,1,5)
  x = 256*x_msb + x_lsb;
  y = 256*y_msb + y_lsb;

  x = x - (x_max + x_min) / 2 -- use offsets from calibration
  y = y - (y_max + y_min) / 2 -- use offsets from calibration

  w = nxt.atan2(x,y)
  if w < 0 then w = 360 + w end

  print(w)
  delay(100)
until( 8 == nxt.ButtonRead() )