From 67d25d837ac55f28a366c0a3b262e439a6e75fc3 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Sat, 19 Aug 2017 12:15:28 +0200 Subject: Add AmForth --- amforth-6.5/common/lib/hardware/i2c-compass.frt | 49 +++++++++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 amforth-6.5/common/lib/hardware/i2c-compass.frt (limited to 'amforth-6.5/common/lib/hardware/i2c-compass.frt') diff --git a/amforth-6.5/common/lib/hardware/i2c-compass.frt b/amforth-6.5/common/lib/hardware/i2c-compass.frt new file mode 100644 index 0000000..daa6380 --- /dev/null +++ b/amforth-6.5/common/lib/hardware/i2c-compass.frt @@ -0,0 +1,49 @@ +\ +\ compass module mmc2120 (memsic) +\ hwid is always $30 +\ provides: +\ +\ i2c.compass.get ( -- status X Y) +\ X and Y are around 2000 (raw data) +\ status is 0 if no error occured + +\ dechiffer of the raw data: +\ according to http://www.aurob.com/?p=467 +\ interpolate linearly +\ x=map(1900,2188,-180,180) +\ y=map(1910,2193,-180,180) +\ grad=atan2(x,y)*180/pi + +#require i2c.frt +#require ms.frt + +$30 constant i2c.compass + +\ internal commands +: i2c.compass.setcoil + %00000010 0 2 i2c.compass i2c.n! +; +: i2c.compass.resetcoil + %00000100 0 2 i2c.compass i2c.n! +; + +: i2c.compass.measure + %00000001 0 2 i2c.compass i2c.n! +; + +: i2c.compass.fetchdata ( -- status x y ) + 5 0 i2c.compass i2c.n@ + ( -- status msb-x lsb-x msb-y lsb-y) + swap >< or $fff and >r \ Y + swap >< or $fff and r> \ X +; + +\ get the raw data from the module +\ the numbers for X/Y are usually around 2000. +\ status is 0 if everything is ok +: i2c.compass.get ( -- status x y ) + i2c.compass.resetcoil 1ms + i2c.compass.setcoil 5 ms + i2c.compass.measure 5 ms + i2c.compass.fetchdata +; -- cgit v1.2.3