Dokončil jsem první použitelnou verzi programu (určení azimutu z hodnot magnetometru). Výpočet ATAN je udělán interpolací mezi předpočítanými hodnotami z tabulky. Kód je níže. Třeba se to někomu bude hodit. Přesnost vypadá pro mne přijatelná (lepší než cca 10°, možná i pod 5°). Náklon ale není nijak kompenzován, takže je to použitelné jen pro vodorovné povrchy (např. pro indoor soutěže jako při Robotickém dni v Praze).

#picaxe 08M2
;#no_data

; zpracování dat z I2C magnetometru HMC5883L (modul GY-271)
; C.0 - sertxd (sériový výstup k PC nebo k dalšímu PICAXE)
; C.1 - SCL (k magnetometru)
; C.2 - SDA (k magnetometru)
; C.3 - in (-- nepoužito --)
; C.4 - out (LED, připojena přes odpor cca 800 ohm k zemi nebo k + napájení)
; C.5 - serin (-- nepoužito --)
; napájení 5V (4x AA NiMH nabíjitelné akumulátory) [pro PICAXE i modul současně]
; měřený rozsah v každé ose magnetometru je cca 1200 (+-600), šum cca 30

eeprom 0, (0,6,11,17,22,27,31,35,39,42,45) ; předpočítaná tabulka úhlů (atan)

symbol xm = w0           ; měřená osa X magnetometru
symbol ym = w1           ; měřená osa Y magnetometru
symbol zm = w2           ; měřená osa Z magnetometru
symbol cas = w3          ; pro sledování změn proměnné "time"
symbol cyklu = w4        ; počítadlo cyklů (pro měření rychlosti zpracování dat)
symbol azimut = w5       ; výsledný přepočítaný azimut

symbol uhel0 = w6        ; pomocný úhel (bude to pamět zákl. azimutu po zapnutí)
symbol uhel = b14        ; výsledný úhel po výpočtu arctan
symbol uhel2 = b15       ; pomocný úhel určený z tabulky
symbol tan100 = b16      ; tangens * 100 (X / Y * 100)
symbol index = b17       ; vypočítaný index do tabulky
symbol offset = b18      ; vypočítaný posun mezi body indexu
symbol kvadrant = b19    ; kvadrant azimutu podle hodnot X a Y

symbol x = w10           ; zkalibrovaná a normalizovaná hodnota X (0 - 200)
symbol y = w11           ; zkalibrovaná a normalizovaná hodnota Y (0 - 200)
symbol z = w12           ; zkalibrovaná a normalizovaná hodnota Z (0 - 200)

setfreq m32              ; zvýšení frekvence procesoru (max. 32 MHz [8x normal])

pause 100                ; počkáme na ustálení napájení a vstupů po startu
sertxd ("PICAXE 08M2 kompas s HMC5883L",CR,LF)

hi2csetup i2cmaster, $3C, i2cfast_32, i2cbyte  ; inicializace I2C na adrese 3C
hi2cout 0, ( %01010100 ) ; config A - 4 averaged, 30Hz updates, normal measurement
hi2cout 1, ( %00100000 ) ; config B - 1.3Ga gain
hi2cout 2, ( %00000000 ) ; mode - continuous conversion

sertxd ("setup OK",CR,LF)

do
  inc cyklu
  if time>cas then       ; měříme a vypisujeme po každé změně "time" (2x za sec.)
    cas = time
    hi2cin 3, ( b1,b0 , b3,b2 , b5,b4 ) ; načtení holých dat z magnetometru (x,y,z)
    xm = xm + 800 / 8    ; normalizace X do max. rozsahu 0 - 200 se středem 100
    ym = ym + 800 / 8    ; normalizace Y do max. rozsahu 0 - 200 se středem 100
    zm = zm + 800 / 8    ; normalizace Z do max. rozsahu 0 - 200 se středem 100
    x = xm
    y = 200 - ym min 0 max 200
    z = zm
    gosub atan2           ; výpočet azimutu
    sertxd ("time:",#time,"  azimuth:",#azimut,CR,LF)
    toggle C.4           ; kontrolní blikání LEDkou
    cyklu = 0
  endif
  ;gosub atan2            ; jen pro odhad rychlosti výpočtu
loop

atan2:
  ; výpočet azimutu podle X a Y (arcustangens a kvadrant)
  ; hodnoty X a Y musí být předem normalizovány do rozsahu 0 - 200
  ; nula je na hodnotě 100 (pro X i pro Y)
  if x>=100 then
    x = x - 100
    if y>=100 then
      y = y - 100
      kvadrant = 0
    else
      y = 100 - y
      kvadrant = 3
    endif
  else
    x = 100 - x
    if y>=100 then
      y = y - 100
      kvadrant = 1
    else
      y = 100 - y
      kvadrant = 2
    endif
  endif
  tan100 = 100
  if x>y then
    tan100 = 100 * y / x
  else if y>x then
    tan100 = 100 * x / y
  endif
  ; výpočet atan interpolací mezi dvěma předpočítanými hodnotami z tabulky (0 - 45°)
  index = tan100 / 10
  offset = tan100 % 10
  read index,uhel
  inc index
  read index,uhel2
  azimut = uhel2 - uhel * offset / 10 + uhel
  ; posun azimutu do správného kvadrantu
  if y>=x then
    azimut = 90 - azimut
  endif
  if kvadrant = 1 or kvadrant = 3 then
    azimut = kvadrant * 90 - azimut + 90
  else
    azimut = kvadrant * 90 + azimut
  endif
return

Tak jsem včera zahájil malý podzimní projekt s názvem "PICAXE kompas". Na letošním Robotickém dni jsem měl problém s orientací robotů na hřišti, pokud došlo k neočekávanému posunutí mého robota, například po střetu se soupeřem. Rád bych tedy svoje roboty doplnil o nějaké relativně levné čidlo absolutní polohy, zatím alespoň o elektronický kompas (magnetometr). Proto jsem na Aukru koupil jeden čínský modul GY-271 za cca 200 Kč (obsahuje tříosý magnetometr HMC5883L s rozhraním I2C). Myšlenka je taková, že nechci příliš zatěžovat hlavní procesor robota (v mém případě to je jen PICAXE 20M2 nebo PICAXE 20X2) zpracováním dat z magnetometru, takže zkusím měření průběžně zpracovávat dodatečným PICAXE 08M2 (za cca 50 Kč) a do hlavního procesoru posílat už zpracovaná data (filtrovaná, kalibrovaná a přepočítaná na azimut) pomocí klasického sériového rozhraní jako řetězec znaků (několikrát za sekundu).

Včera jsem tedy koupený modul poprvé propojil s PICAXE 08M2 (na nepájivém poli) a vyzkoušel první testovací program (jen pro výpis surových dat bez jakéhokoliv zpracování). Fungovalo to solidně. Holá data (jen naměřené hodnoty mg. pole v osách x, y, z) jsou ale nekalibrovaná a trochu se mění i bez pohybu čidla (obsahují určitý "šum"). Níže je uveden použitý testovací kód i se stručným popisem propojení.

#picaxe 08M2
#no_data

; zpracovani dat z I2C magnetometru HMC5883L (modul GY-271)
; C.0 - sertxd (seriovy vystup k PC nebo k dalsimu PICAXE)
; C.1 - SCL (k magnetometru)
; C.2 - SDA (k magnetometru)
; C.3 - in (nepouzito)
; C.4 - out (LED, pripojena pres odpor cca 800 ohm k zemi nebo k + napajeni)
; C.5 - serin (nepouzito)
; napajeni 5V (4x AA NiMH nabijitelne akumulatory) [PICAXE i modul]

symbol x = w0
symbol y = w1
symbol z = w2

setfreq m32              ; zvyseni frekvence procesoru (max [8x])
pause 100                ; pockame na ustaleni napajeni a vstupu po startu
sertxd ("PICAXE 08M2 kompas HMC5883L",CR,LF)

hi2csetup i2cmaster, $3C, i2cslow_32, i2cbyte  ; inicializace I2C na adrese 3C
hi2cout 0, ( %00010000 ) ; Config A Default - 15Hz updates, Normal measurement
hi2cout 1, ( %00100000 ) ; Config B Default - +/1Ga gain
hi2cout 2, ( %00000000 ) ; Mode - Continuous conversion

do
  pause 8000             ; pauza 1 sekunda
  hi2cin 3, ( b1,b0 , b3,b2 , b5,b4 ) ; nacteni holych dat z magnetometru (x,y,z)
  sertxd ("t:",#time," x:",#x," y:",#y," z:",#z,CR,LF) ; vypis casu a holych dat
  toggle C.4             ; kontrolni blikani LEDkou
loop

V dalších dnech zkusím data trochu filtrovat (průměrovat), kalibrovat (posunout v jednotivých osách symetricky ke středu) a přepočítat do nějakého rozumného rozsahu (zatím mi asi bude stačit přesnost na 10°, tedy přepočítaný rozsah azimutu např. 0 - 35). Pokračování příště...

Dnes jsem založil tento blog, ve kterém budu popisovat svoje zážitky a zkušenosti z hobby robotiky. Zprávy blogu je možné číst i přes RSS kanál.