Blog
PICAXE kompas (2)
21.09.2013 21:04Dokonč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
PICAXE kompas (1)
17.09.2013 20:33Tak 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ě...
Hobby robotika
12.08.2013 21:58Dnes 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.