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() )