add ability to build and upload sketches from the bash command line (via beta ino2cpp...
authordave russo <d-russo@ti.com>
Fri, 6 Nov 2015 20:26:41 +0000 (12:26 -0800)
committerdave russo <d-russo@ti.com>
Fri, 6 Nov 2015 20:26:41 +0000 (12:26 -0800)
add self-contained ZumoTest sketch

21 files changed:
.gitignore
etc/ino2cpp/ebuild.sh [new file with mode: 0644]
etc/ino2cpp/ino2cpp.jar [new file with mode: 0644]
etc/ino2cpp/ino2cpp.sh [new file with mode: 0644]
etc/ino2cpp/templates/Variables.mk.template [new file with mode: 0644]
etc/ino2cpp/templates/cc2600emt/main.template [new file with mode: 0644]
etc/ino2cpp/templates/cc3200emt/main.template [new file with mode: 0644]
etc/ino2cpp/templates/msp432/main.template [new file with mode: 0644]
etc/rbload.bat [new file with mode: 0644]
etc/rbload.sh [new file with mode: 0644]
makefile
src/Energia/ZumoTest/L3G.cpp [new file with mode: 0644]
src/Energia/ZumoTest/L3G.h [new file with mode: 0644]
src/Energia/ZumoTest/LSM303.cpp [new file with mode: 0644]
src/Energia/ZumoTest/LSM303.h [new file with mode: 0644]
src/Energia/ZumoTest/ZumoMotors.cpp [new file with mode: 0644]
src/Energia/ZumoTest/ZumoMotors.h [new file with mode: 0644]
src/Energia/ZumoTest/ZumoTest.ino [new file with mode: 0644]
src/Energia/ZumoTest/apLoop.ino [new file with mode: 0644]
src/Energia/ZumoTest/imuLoop.ino [new file with mode: 0644]
src/Energia/ZumoTest/motorLoop.ino [new file with mode: 0644]

index 7da50c9b7c5c5952b40034b28ee246dceb1600d7..1afa61d5873b89bda848d09186c99e82622a22c1 100644 (file)
@@ -11,6 +11,7 @@
 /.reltree
 /.generated_files
 /bin/
+/build/
 
 ## emacs backup files
 .#*
@@ -31,20 +32,6 @@ doc-files
 .libraries
 .libraries,*
 
-## Legacy build goals
-.src-files
-.include-files
-.etc-files
-.lib-files
-.bin-files,*
-.test-files
-
-## Generated package archives
-*.zip
-*.gz
-*.tar
-*.jar
-
 ## Generated executables, libraries, objects, ...
 *.x86U
 *.x86_64M
diff --git a/etc/ino2cpp/ebuild.sh b/etc/ino2cpp/ebuild.sh
new file mode 100644 (file)
index 0000000..c1545db
--- /dev/null
@@ -0,0 +1,102 @@
+#!/bin/sh
+#
+#  Command-line build of an Energia MT sketch
+#
+#  Usage: ebuild [-e energia_root] [-t target] sketch-dir build-dir [ppopts...]
+#
+#    where,
+#      -e energia_root - installation directory of Energia MT.  If not 
+#                        specified, uses the directory containing the directory
+#                        containing this script; i.e., `dirname $0`/..
+#                        Note: a complete installation of Energia is required
+#                        in order to build an executable.
+#      -t target       - target; e.g., cc3200emt:RedBearLab_CC3200.  If not
+#                        specified, uses cc3200emt:CC3200_LAUNCHXL
+#      sketch_dir      - directory containing the sketch to build
+#      build_dir       - output directory for the build of the sketch
+#      ppopts          - Energia MT preprocessor (ino2cpp) options; e.g., 
+#                          -Dsketchbook.path=...
+#                          -Dbuild.drvlib={true|false}
+#
+#  Note: A complete installation of Energia is REQUIRED in order for the build
+#        to work.  Energia contains numerous makefiles, all of which are 
+#        required to complete the build.
+#
+#  Examples
+#   1. Build a sketch for the cc3200emt:CC3200_LAUNCHXL
+#
+#           ebuild.ksh mysketch mybuild
+#
+#    2. Using example from a contributed library installed in, say, 
+#       $ZUMOCC3200, build in the local directory ./foo
+#
+#           ebuild.ksh -t cc3200emt:RedBearLab_CC3200 \
+#                      $ZUMOCC3200/examples/AssistedDrive ./foo \
+#                     -Dsketchbook.path=../$ZUMOCC3200
+#
+
+pname="$0"
+mydir="`dirname $pname`"
+
+# someday this will be inside Energia
+INO2CPP="$mydir"
+ENERGIA="`cd $mydir/..; /bin/pwd`"
+
+usage="usage: $0 [-e energia_root] [-t target] sketch-dir build-dir [ino2cpp_options ...]"
+
+## parse command line options
+target="cc3200emt:CC3200_LAUNCHXL"
+while getopts e:t: c ; do
+    case $c in
+        e )  ENERGIA="$OPTARG" ;;
+        t )  target="$OPTARG" ;;
+        \?)  echo "$usage"
+                exit 1;;
+    esac
+done
+shift `expr $OPTIND - 1`
+
+## validate command line options
+if [ $# -lt 2 ]; then
+    echo $usage
+    exit 1
+fi
+
+sketch=$1
+build=$2
+shift 2
+
+## locate a java command
+JAVA=java
+for j in "$ENERGIA/java/bin/java" "`which java 2> /dev/null`" "$JAVA_HOME/bin/java"; do
+    if [ -x "$j" -o -x "$j.exe" ]; then
+        JAVA="$j"
+        break
+    fi
+done
+
+## locate a make command
+MAKE=make
+for m in $ENERGIA/tools/common/bin/make "`which make 2> /dev/null`"; do
+    if [ -x "$m" -o -x "$m.exe" ]; then
+        MAKE="$m"
+        break
+    fi
+done
+
+## check that ENERGIA is in fact an Energia installation
+if [ ! -r "$ENERGIA/hardware/common/Makefile" ]; then
+    echo "$pname: error: '$ENERGIA/hardware/common/Makefile' does not exist; '$ENERGIA' doesn't seem to be an installation of Energia." 2>&1
+    exit 1
+fi
+
+## run the Energia Wiring preprocessor
+echo processing $sketch [$target] ...
+$JAVA -jar $INO2CPP/ino2cpp.jar -o $build -T $INO2CPP/templates/Variables.mk.template -r $ENERGIA $* $sketch $target
+if [ "$?" != "0" ]; then
+    echo "$pname: error: Energia ino2cpp preprocessor failed." 2>&1
+    exit 1
+fi
+
+## compile!
+$MAKE -C $build -f "$ENERGIA/hardware/common/Makefile"
diff --git a/etc/ino2cpp/ino2cpp.jar b/etc/ino2cpp/ino2cpp.jar
new file mode 100644 (file)
index 0000000..eae1580
Binary files /dev/null and b/etc/ino2cpp/ino2cpp.jar differ
diff --git a/etc/ino2cpp/ino2cpp.sh b/etc/ino2cpp/ino2cpp.sh
new file mode 100644 (file)
index 0000000..2061f3d
--- /dev/null
@@ -0,0 +1,21 @@
+#!/bin/sh
+#
+# Convert Energia sketch files into buildable C/C++ files
+#
+#  Usage: ino2cpp [-o output] [-Dname=value] [-E] [-T tplt] sketch target
+#
+#  Note: ino2cpp can run without an existing installation of Energia
+#
+root="`dirname $0`"
+
+## locate a java command
+JAVA=java
+for j in "$ENERGIA/java/bin/java" "`which java 2> /dev/null`" "$JAVA_HOME/bin/java"; do
+    if [ -x "$j" -o -x "$j.exe" ]; then
+        JAVA="$j"
+        break
+    fi
+done
+
+## run the ino2cpp preprocessor
+$JAVA -jar "$root"/ino2cpp.jar -r "$root" -T $root/templates/Variables.mk.template "$@" 
diff --git a/etc/ino2cpp/templates/Variables.mk.template b/etc/ino2cpp/templates/Variables.mk.template
new file mode 100644 (file)
index 0000000..8ee2151
--- /dev/null
@@ -0,0 +1,15 @@
+APPLICATION_PATH = `build.app.root`
+MAINSKETCH      ?= `build.mainsketch`
+BOARD           ?= `build.variant`
+PLATFORM        ?= `target`
+BUILD_DRVLIB    ?= `build.drvlib`
+EXTRA_SOURCES    = `build.extra.sources`
+USER_LIB_PATH    = `sketchbook.path`/libraries
+
+CLOSURE ?= $(APPLICATION_PATH)/hardware/common
+CCROOT  ?= $(APPLICATION_PATH)/hardware/tools/lm4f
+
+# if BOARD is not defined, default to some variant (often there is just one)
+ifeq (,$(BOARD))
+    BOARD = $(firstword $(notdir $(wildcard $(APPLICATION_PATH)/hardware/$(PLATFORM)/variants/*)))
+endif
\ No newline at end of file
diff --git a/etc/ino2cpp/templates/cc2600emt/main.template b/etc/ino2cpp/templates/cc2600emt/main.template
new file mode 100644 (file)
index 0000000..ee06929
--- /dev/null
@@ -0,0 +1,90 @@
+/*
+ *  ======== main.cpp ========
+ *  MT wiring Task framework
+ */
+#include <stddef.h>
+//#include <oslib/osi.h>
+
+/* XDC Header files */
+#include <xdc/cfg/global.h>
+
+/* BIOS Header files */
+#include <ti/sysbios/BIOS.h>
+#include <ti/sysbios/knl/Task.h>
+#include <ti/sysbios/family/arm/m3/Hwi.h>
+#include <xdc/runtime/System.h>
+
+/* Board Support Header files (from configuration closure) */
+#include <ti/runtime/wiring/Energia.h>
+
+/* magic insertion point 769d20fcd7a0eedaf64270f591438b01 */
+
+Void the_task(UArg _task_setup, UArg _task_loop);
+
+/* set priority of simple link callbacks
+ * must be >= 0 and < Task_numPriorities
+ * where Task_numPriorities is set by
+ * TI-RTOS config
+ */
+
+/*
+ *  ======== main task ========
+ */
+Void the_task(UArg _task_setup, UArg _task_loop)
+{
+       /* Call setup once */
+       (*(void(*)()) _task_setup)();
+
+       /* Call loop repeatedly */
+       for(;;) {
+               (*(void(*)()) _task_loop)();
+               System_flush();
+               Task_yield();
+       }
+}
+/*
+ *  ======== main ========
+ */
+int main()
+{
+       /* initialize all device/board specific peripherals */
+       Board_init();  /* this function is generated as part of TI-RTOS config */
+
+       /* The SimpleLink Host Driver requires a mechanism to allow functions to
+        * execute in task context.  The SpawnTask is created to handle such
+        * situations.  This task will remain blocked until the host driver
+        * posts a function.  If the SpawnTask priority is higher than other
+        * tasks, it will immediately execute that function and return to a
+        * blocked state.  Otherwise, it will remain ready until it has
+        * the highest priority of any ready function.
+       */
+
+       Task_Params taskParams;
+
+       /* initialize taskParams and set to default */
+       Task_Params_init(&taskParams);
+
+       /* All tasks have the same priority */
+       taskParams.priority = Task_numPriorities - 2;
+       taskParams.stackSize = 0x800;
+
+       uint8_t i = 0;
+       for(i = 0; i < NUM_SKETCHES; i++) {
+               /* Set arg0 to setup() */
+               taskParams.arg0 = (xdc_UArg)func_ptr[i][0];
+               /* Set ar1 to loop */
+               taskParams.arg1 = (xdc_UArg)func_ptr[i][1];
+               /* Set the task name */
+               taskParams.instance->name = (xdc_String) taskNames[i];
+               /* Create the task */
+               Task_create(the_task, &taskParams, NULL);
+       }
+
+       /* does not return */
+       BIOS_start();
+
+       return (0); /* should never get here, but just in case ... */
+}
+
+
+
diff --git a/etc/ino2cpp/templates/cc3200emt/main.template b/etc/ino2cpp/templates/cc3200emt/main.template
new file mode 100644 (file)
index 0000000..1abef6d
--- /dev/null
@@ -0,0 +1,90 @@
+/*
+ *  ======== main.cpp ========
+ *  MT wiring Task framework
+ */
+#include <stddef.h>
+//#include <oslib/osi.h>
+
+/* XDC Header files */
+#include <xdc/cfg/global.h>
+
+/* BIOS Header files */
+#include <ti/sysbios/BIOS.h>
+#include <ti/sysbios/knl/Task.h>
+#include <ti/sysbios/family/arm/m3/Hwi.h>
+#include <xdc/runtime/System.h>
+
+/* Board Support Header files (from configuration closure) */
+#include <ti/runtime/wiring/Energia.h>
+
+/* magic insertion point 769d20fcd7a0eedaf64270f591438b01 */
+
+Void the_task(UArg _task_setup, UArg _task_loop);
+
+/* set priority of simple link callbacks
+ * must be >= 0 and < Task_numPriorities
+ * where Task_numPriorities is set by
+ * TI-RTOS config
+ */
+
+/*
+ *  ======== main task ========
+ */
+Void the_task(UArg _task_setup, UArg _task_loop)
+{
+       /* Call setup once */
+       (*(void(*)()) _task_setup)();
+
+       /* Call loop repeatedly */
+       for(;;) {
+               (*(void(*)()) _task_loop)();
+               System_flush();
+               Task_yield();
+       }
+}
+/*
+ *  ======== main ========
+ */
+int main()
+{
+       /* initialize all device/board specific peripherals */
+       Board_init();  /* this function is generated as part of TI-RTOS config */
+
+       /* The SimpleLink Host Driver requires a mechanism to allow functions to
+        * execute in task context.  The SpawnTask is created to handle such
+        * situations.  This task will remain blocked until the host driver
+        * posts a function.  If the SpawnTask priority is higher than other
+        * tasks, it will immediately execute that function and return to a
+        * blocked state.  Otherwise, it will remain ready until it has
+        * the highest priority of any ready function.
+       */
+
+       Task_Params taskParams;
+
+       /* initialize taskParams and set to default */
+       Task_Params_init(&taskParams);
+
+       /* All tasks have the same priority */
+       taskParams.priority = Task_numPriorities - 1;
+       taskParams.stackSize = 0xc00;
+
+       uint8_t i = 0;
+       for(i = 0; i < NUM_SKETCHES; i++) {
+               /* Set arg0 to setup() */
+               taskParams.arg0 = (xdc_UArg)func_ptr[i][0];
+               /* Set ar1 to loop */
+               taskParams.arg1 = (xdc_UArg)func_ptr[i][1];
+               /* Set the task name */
+               taskParams.instance->name = (xdc_String) taskNames[i];
+               /* Create the task */
+               Task_create(the_task, &taskParams, NULL);
+       }
+
+       /* does not return */
+       BIOS_start();
+
+       return (0); /* should never get here, but just in case ... */
+}
+
+
+
diff --git a/etc/ino2cpp/templates/msp432/main.template b/etc/ino2cpp/templates/msp432/main.template
new file mode 100644 (file)
index 0000000..ee06929
--- /dev/null
@@ -0,0 +1,90 @@
+/*
+ *  ======== main.cpp ========
+ *  MT wiring Task framework
+ */
+#include <stddef.h>
+//#include <oslib/osi.h>
+
+/* XDC Header files */
+#include <xdc/cfg/global.h>
+
+/* BIOS Header files */
+#include <ti/sysbios/BIOS.h>
+#include <ti/sysbios/knl/Task.h>
+#include <ti/sysbios/family/arm/m3/Hwi.h>
+#include <xdc/runtime/System.h>
+
+/* Board Support Header files (from configuration closure) */
+#include <ti/runtime/wiring/Energia.h>
+
+/* magic insertion point 769d20fcd7a0eedaf64270f591438b01 */
+
+Void the_task(UArg _task_setup, UArg _task_loop);
+
+/* set priority of simple link callbacks
+ * must be >= 0 and < Task_numPriorities
+ * where Task_numPriorities is set by
+ * TI-RTOS config
+ */
+
+/*
+ *  ======== main task ========
+ */
+Void the_task(UArg _task_setup, UArg _task_loop)
+{
+       /* Call setup once */
+       (*(void(*)()) _task_setup)();
+
+       /* Call loop repeatedly */
+       for(;;) {
+               (*(void(*)()) _task_loop)();
+               System_flush();
+               Task_yield();
+       }
+}
+/*
+ *  ======== main ========
+ */
+int main()
+{
+       /* initialize all device/board specific peripherals */
+       Board_init();  /* this function is generated as part of TI-RTOS config */
+
+       /* The SimpleLink Host Driver requires a mechanism to allow functions to
+        * execute in task context.  The SpawnTask is created to handle such
+        * situations.  This task will remain blocked until the host driver
+        * posts a function.  If the SpawnTask priority is higher than other
+        * tasks, it will immediately execute that function and return to a
+        * blocked state.  Otherwise, it will remain ready until it has
+        * the highest priority of any ready function.
+       */
+
+       Task_Params taskParams;
+
+       /* initialize taskParams and set to default */
+       Task_Params_init(&taskParams);
+
+       /* All tasks have the same priority */
+       taskParams.priority = Task_numPriorities - 2;
+       taskParams.stackSize = 0x800;
+
+       uint8_t i = 0;
+       for(i = 0; i < NUM_SKETCHES; i++) {
+               /* Set arg0 to setup() */
+               taskParams.arg0 = (xdc_UArg)func_ptr[i][0];
+               /* Set ar1 to loop */
+               taskParams.arg1 = (xdc_UArg)func_ptr[i][1];
+               /* Set the task name */
+               taskParams.instance->name = (xdc_String) taskNames[i];
+               /* Create the task */
+               Task_create(the_task, &taskParams, NULL);
+       }
+
+       /* does not return */
+       BIOS_start();
+
+       return (0); /* should never get here, but just in case ... */
+}
+
+
+
diff --git a/etc/rbload.bat b/etc/rbload.bat
new file mode 100644 (file)
index 0000000..8e1064e
--- /dev/null
@@ -0,0 +1,45 @@
+@echo off
+setlocal
+
+set binpath=%~dp0\..\build
+
+if "%1"=="" (
+    echo usage: %~nx0 binary [[com_port_id] [energia_install_dir]]
+    exit /b 1
+)
+
+set bin=%1
+if not exist %1 (
+    if exist %binpath%\%1 (
+        set bin=%binpath%\%1
+    ) else (
+        echo error: can't find the binary file "%1"
+       exit /b 1
+    )
+)
+
+set comid=%2
+if "%2"=="" (
+    set comid=9
+)
+
+if "%3"=="" (
+    set eidir=C:\ti
+) else (
+    set eidir=%3
+)
+
+for /f "usebackq delims=" %%f in (`dir /b /o:n %eidir%\energia-*`) do (
+    REM cc3200prog must also be run from it's home directory (!)
+    echo chdir %eidir%\%%f\hardware\tools\lm4f\bin
+    chdir %eidir%\%%f\hardware\tools\lm4f\bin
+
+    echo cc3200prog %comid% %bin%
+    cc3200prog %comid% %bin% || exit /b 1
+    echo Upload succeeded.
+    echo Push the reset button on the ZumoCC3200 to start the sketch.
+    exit /b 0
+)
+
+echo error: energia is not installed in %eidir%
+exit /b 1
diff --git a/etc/rbload.sh b/etc/rbload.sh
new file mode 100644 (file)
index 0000000..33319b8
--- /dev/null
@@ -0,0 +1,53 @@
+#!/bin/sh
+#
+# Flash the specified binary on the RedBear CC3200
+#
+# Usage: rbload binary [[com_port_id] [energia_install_dir]]"
+#
+
+if [ $# -lt 1 ]; then
+    echo "usage: `basename $0` binary [[com_port_id] [energia_install_dir]]"
+    exit 1
+fi
+
+# locate the binary file to load
+binpath=`dirname $0`/../build
+bin="$1"
+if [ ! -r "$1" ]; then
+    if [ -r "$binpath/$bin" ]; then
+        bin="$binpath/$bin"
+    else
+        echo "error: can't find the binary file $bin"
+       exit 1
+    fi
+fi
+
+# set serial port id
+comid="$2"
+if [ "$comid" = "" ]; then
+    comid=9
+fi
+
+# set Energia install dir
+eidir="$3"
+if [ "$eidir" = "" ]; then
+    eidir="C:/ti"
+fi
+
+# convert bin to absolute path
+bindir="`dirname $bin`"
+bindir="`cd $bindir; pwd`"
+bin="$bindir/`basename $bin`"
+
+# use the latest version of energia to load $bin
+for f in `ls -dt "$eidir"/energia-* 2> /dev/null` "$eidir"; do
+    # cc3200prog must be run from it's home directory (!)
+    cd "$f/hardware/tools/lm4f/bin"
+    ./cc3200prog "$comid" "$bin" || exit 1
+    echo Upload succeeded.  
+    echo Push the reset button on the ZumoCC3200 to start the sketch.
+    exit 0
+done
+    
+echo "error: energia is not installed in $eidir"
+exit 1
index 818331e37bf3b2b6fffbc8798914950d981d00f9..610c30f6f793174cd49f335c213e96e265ced42e 100644 (file)
--- a/makefile
+++ b/makefile
@@ -1,10 +1,70 @@
-GOALS = README.html
+#
+# ======== ZumoCC3200 Makefile ========
+# This makfile builds README.html from README.md and optionally builds and
+# uploads selected Energia sketches to the ZumoCC3200.
+#
 
-all: $(GOALS)
+all: README.html
 
 %.html:%.md
        rm -f $@
        etc/md2html.ksh $< > $@
-
 clean:
-       rm -f $(GOALS)
+       rm -f README.html
+       rm -rf build
+
+# ======== Sketch Build and Upload Rules ========
+#
+#  To Build Sketches
+#    Windows
+#      Start a git bash shell, cd to the folder containing this makefile, and
+#      enter the following commands
+#        export ENERGIA=c:/ti/energia-0101E0017
+#        $ENERGIA/tools/common/bin/make test
+#
+#    Linux/Mac
+#      Start a  command shell, cd to the folder containing this makefile, and
+#      enter the following commands
+#        export ENERGIA=...
+#        make test
+#
+#  To Build and Upload ZumoTest
+#    Windows
+#        export ENERGIA=c:/ti/energia-0101E0017
+#        export SERPORT=9
+#        $ENERGIA/tools/common/bin/make upload
+#
+#    Linux/Mac
+#        export ENERGIA=...
+#        export SERPORT=...
+#        make upload
+#
+
+# ENERGIA - the installation directory of Energia.  This is only used when
+#           building sketches from the command line
+# SPORT   - the serial port id used by the flash programmer to upload
+#           binaries to the ZumoCC3200
+#
+ENERGIA ?= c:/ti/energia-0101E0017
+SERPORT ?= 9
+
+INO2CPP = $(CURDIR)/etc/ino2cpp
+BUILD   = $(INO2CPP)/ebuild.sh -t cc3200emt:RedBearLab_CC3200 -e $(ENERGIA) 
+FLASH   = $(CURDIR)/etc/rbload.sh
+
+test: ./build/ZumoTest/ZumoTest.cpp.bin ./build/ManualDrive/ManualDrive.cpp.bin
+
+# a self-contained sketch (on need for ZumoCC3200 library)
+./build/ZumoTest/ZumoTest.cpp.bin: $(wildcard src/Energia/ZumoTest/*)
+       @echo build system test ...
+       $(BUILD) ./src/Energia/ZumoTest ./build/ZumoTest
+
+# a simple sketch that requires the ZumoCC3200 library
+./build/ManualDrive/ManualDrive.cpp.bin: $(wildcard ./src/Energia/libraries/ZumoCC3200/examples/ManualDrive/*)
+       @echo build a library example ...
+       $(BUILD) ./src/Energia/libraries/ZumoCC3200/examples/ManualDrive ./build/ManualDrive -Dsketchbook.path=$(CURDIR)/src/Energia
+
+# upload ZumoTest to the ZumoCC3200
+upload: ./build/ZumoTest/ZumoTest.cpp.bin
+       @echo flashing $(<F) via serial port $(SERPORT) ...
+       $(FLASH) $< $(SERPORT) $(ENERGIA)
diff --git a/src/Energia/ZumoTest/L3G.cpp b/src/Energia/ZumoTest/L3G.cpp
new file mode 100644 (file)
index 0000000..d2c483e
--- /dev/null
@@ -0,0 +1,282 @@
+/*
+ *  ======== L3G.cpp ========
+ */
+
+#include <Wire.h>
+#include <math.h>
+
+#include "L3G.h"
+
+// Defines ////////////////////////////////////////////////////////////////
+
+/* The Arduino two-wire interface uses a 7-bit number for the address,
+ * and sets the last bit correctly based on reads and writes
+ */
+#define D20_SA0_HIGH_ADDRESS      0b1101011 // also applies to D20H
+#define D20_SA0_LOW_ADDRESS       0b1101010 // also applies to D20H
+#define L3G4200D_SA0_HIGH_ADDRESS 0b1101001
+#define L3G4200D_SA0_LOW_ADDRESS  0b1101000
+
+#define TEST_REG_ERROR -1
+
+#define D20H_WHO_ID     0xD7
+#define D20_WHO_ID      0xD4
+#define L3G4200D_WHO_ID 0xD3
+
+static int testReg(byte address, L3G::regAddr reg);
+
+/*
+ *  ======== L3G ========
+ *  Constructors for L3G gyro
+ */
+L3G::L3G(void)
+{
+    _device = device_auto;
+
+    io_timeout = 0;  // 0 = no timeout
+    did_timeout = false;
+}
+
+/*
+ *  ======== timeoutOccurred ========
+ *  Did a timeout occur in read() since the last call to timeoutOccurred()?
+ */
+bool L3G::timeoutOccurred()
+{
+    bool tmp = did_timeout;
+    did_timeout = false;
+    return tmp;
+}
+
+/*
+ *  ======== setTimeout ========
+ */
+void L3G::setTimeout(unsigned int timeout)
+{
+    io_timeout = timeout;
+}
+
+/*
+ *  ======== getTimeout ========
+ */
+unsigned int L3G::getTimeout()
+{
+    return io_timeout;
+}
+
+/*
+ *  ======== init ========
+ */
+bool L3G::init(deviceType device, sa0State sa0)
+{
+    int id;
+  
+    /* perform auto-detection unless device type and SA0 state were both
+     * specified
+     */
+    if (device == device_auto || sa0 == sa0_auto) {
+        /* check for L3GD20H, D20 if device is unidentified or was specified
+         * to be one of these types
+         */
+        if (device == device_auto || device == device_D20H
+            || device == device_D20) {
+            // check SA0 high address unless SA0 was specified to be low
+            if (sa0 != sa0_low
+                && (id = testReg(D20_SA0_HIGH_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) {
+                /* device responds to address 1101011; it's a D20H or D20
+                 * with SA0 high
+                 */
+                sa0 = sa0_high;
+                if (device == device_auto) {
+                    // use ID from WHO_AM_I register to determine device type
+                    device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
+                }
+            }
+            // check SA0 low address unless SA0 was specified to be high
+            else if (sa0 != sa0_high
+                     && (id = testReg(D20_SA0_LOW_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) {
+                /* device responds to address 1101010; it's a D20H or D20
+                 * with SA0 low
+                 */
+                sa0 = sa0_low;
+                if (device == device_auto) {
+                    // use ID from WHO_AM_I register to determine device type
+                    device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
+                }
+            }
+        }
+    
+        /* check for L3G4200D if device is still unidentified or was specified
+         * to be this type
+         */
+        if (device == device_auto || device == device_4200D) {
+            if (sa0 != sa0_low
+                && testReg(L3G4200D_SA0_HIGH_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) {
+                /* device responds to address 1101001; it's a 4200D with SA0
+                 * high
+                 */
+                device = device_4200D;
+                sa0 = sa0_high;
+            }
+            else if (sa0 != sa0_high
+                     && testReg(L3G4200D_SA0_LOW_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) {
+                /* device responds to address 1101000; it's a 4200D with SA0
+                 * low
+                 */
+                device = device_4200D;
+                sa0 = sa0_low;
+            }
+        }
+    
+        /* make sure device and SA0 were successfully detected; otherwise,
+         * indicate failure
+         */
+        if (device == device_auto || sa0 == sa0_auto) {
+            return false;
+        }
+    }
+  
+    _device = device;
+
+    // set device address
+    switch (device) {
+        case device_D20H:
+        case device_D20:
+            address = (sa0 == sa0_high) ? D20_SA0_HIGH_ADDRESS : D20_SA0_LOW_ADDRESS;
+            break;
+
+        case device_4200D:
+            address = (sa0 == sa0_high) ? L3G4200D_SA0_HIGH_ADDRESS : L3G4200D_SA0_LOW_ADDRESS;
+            break;
+    }
+  
+    return true;
+}
+
+/*
+ *  ======== enableDefault ========
+ *  Enables the L3G's gyro
+ *
+ *  Also:
+ *      - Sets gyro full scale (gain) to default power-on value of +/- 250 dps
+ *       (specified as +/- 245 dps for L3GD20H).
+ *      - Selects 200 Hz ODR (output data rate). (Exact rate is specified as
+ *        189.4 Hz for L3GD20H and 190 Hz for L3GD20.)
+ *  
+ *  Note that this function will also reset other settings controlled by
+ *  the registers it writes to.
+*/
+void L3G::enableDefault(void)
+{
+    if (_device == device_D20H) {
+        // 0x00 = 0b00000000
+        //     Low_ODR = 0 (low speed ODR disabled)
+        writeReg(LOW_ODR, 0x00);
+    }
+  
+    // 0x00 = 0b00000000
+    //     FS = 00 (+/- 250 dps full scale)
+    writeReg(CTRL_REG4, 0x00);
+  
+    // 0x6F = 0b01101111
+    //     DR = 01 (200 Hz ODR);
+    //     BW = 10 (50 Hz bandwidth);
+    //     PD = 1 (normal mode);
+    //     Zen = Yen = Xen = 1 (all axes enabled)
+    writeReg(CTRL_REG1, 0x6F);
+}
+
+/*
+ *  ======== writeReg ========
+ *  Writes a gyro register
+ */
+void L3G::writeReg(byte reg, byte value)
+{
+    Wire.beginTransmission(address);
+    Wire.write(reg);
+    Wire.write(value);
+    last_status = Wire.endTransmission();
+}
+
+/*
+ *  ======== readReg ========
+ *  Reads a gyro register
+ */
+byte L3G::readReg(byte reg)
+{
+    byte value;
+
+    Wire.beginTransmission(address);
+    Wire.write(reg);
+    last_status = Wire.endTransmission();
+    Wire.requestFrom(address, (byte)1);
+    value = Wire.read();
+    Wire.endTransmission();
+
+    return value;
+}
+
+/*
+ *  ======== read ========
+ *  Reads the 3 gyro channels and stores them in vector g
+ */
+void L3G::read()
+{
+    Wire.beginTransmission(address);
+    // assert the MSB of the address to get the gyro
+    // to do slave-transmit subaddress updating.
+    Wire.write(OUT_X_L | (1 << 7));
+    Wire.endTransmission();
+    Wire.requestFrom(address, (byte)6);
+  
+    unsigned int millis_start = millis();
+    while (Wire.available() < 6) {
+        if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) {
+            did_timeout = true;
+            return;
+        }
+    }
+
+    uint8_t xlg = Wire.read();
+    uint8_t xhg = Wire.read();
+    uint8_t ylg = Wire.read();
+    uint8_t yhg = Wire.read();
+    uint8_t zlg = Wire.read();
+    uint8_t zhg = Wire.read();
+
+    // combine high and low bytes
+    g.x = (int16_t)(xhg << 8 | xlg);
+    g.y = (int16_t)(yhg << 8 | ylg);
+    g.z = (int16_t)(zhg << 8 | zlg);
+}
+
+/*
+ *  ======== vector_normalize ========
+ */
+void L3G::vector_normalize(vector<float> *a)
+{
+    float mag = sqrt(vector_dot(a,a));
+    a->x /= mag;
+    a->y /= mag;
+    a->z /= mag;
+}
+
+/*
+ *  ======== testReg ========
+ */
+static int testReg(byte address, L3G::regAddr reg)
+{
+    Wire.beginTransmission(address);
+    Wire.write((byte)reg);
+    if (Wire.endTransmission() != 0) {
+        return TEST_REG_ERROR;
+    }
+
+    Wire.requestFrom(address, (byte)1);
+    if (Wire.available()) {
+        return Wire.read();
+    }
+    else {
+        return TEST_REG_ERROR;
+    }
+}
diff --git a/src/Energia/ZumoTest/L3G.h b/src/Energia/ZumoTest/L3G.h
new file mode 100644 (file)
index 0000000..5f64ea0
--- /dev/null
@@ -0,0 +1,189 @@
+/*
+ *  ======== L3G ========
+ *  
+ */
+
+#ifndef L3G_h
+#define L3G_h
+
+#include <Arduino.h> // for byte data type
+
+class L3G
+{
+  public:
+
+    /*
+     *  ======== vector ========
+     */
+    template <typename T> struct vector {
+        T x, y, z;
+    };
+
+    enum deviceType { device_4200D, device_D20, device_D20H, device_auto };
+    enum sa0State { sa0_low, sa0_high, sa0_auto };
+
+    // register addresses
+    enum regAddr
+    {
+        WHO_AM_I       = 0x0F,
+
+        CTRL1          = 0x20, // D20H
+        CTRL_REG1      = 0x20, // D20, 4200D
+        CTRL2          = 0x21, // D20H
+        CTRL_REG2      = 0x21, // D20, 4200D
+        CTRL3          = 0x22, // D20H
+        CTRL_REG3      = 0x22, // D20, 4200D
+        CTRL4          = 0x23, // D20H
+        CTRL_REG4      = 0x23, // D20, 4200D
+        CTRL5          = 0x24, // D20H
+        CTRL_REG5      = 0x24, // D20, 4200D
+        REFERENCE      = 0x25,
+        OUT_TEMP       = 0x26,
+        STATUS         = 0x27, // D20H
+        STATUS_REG     = 0x27, // D20, 4200D
+
+        OUT_X_L        = 0x28,
+        OUT_X_H        = 0x29,
+        OUT_Y_L        = 0x2A,
+        OUT_Y_H        = 0x2B,
+        OUT_Z_L        = 0x2C,
+        OUT_Z_H        = 0x2D,
+
+        FIFO_CTRL      = 0x2E, // D20H
+        FIFO_CTRL_REG  = 0x2E, // D20, 4200D
+        FIFO_SRC       = 0x2F, // D20H
+        FIFO_SRC_REG   = 0x2F, // D20, 4200D
+
+        IG_CFG         = 0x30, // D20H
+        INT1_CFG       = 0x30, // D20, 4200D
+        IG_SRC         = 0x31, // D20H
+        INT1_SRC       = 0x31, // D20, 4200D
+        IG_THS_XH      = 0x32, // D20H
+        INT1_THS_XH    = 0x32, // D20, 4200D
+        IG_THS_XL      = 0x33, // D20H
+        INT1_THS_XL    = 0x33, // D20, 4200D
+        IG_THS_YH      = 0x34, // D20H
+        INT1_THS_YH    = 0x34, // D20, 4200D
+        IG_THS_YL      = 0x35, // D20H
+        INT1_THS_YL    = 0x35, // D20, 4200D
+        IG_THS_ZH      = 0x36, // D20H
+        INT1_THS_ZH    = 0x36, // D20, 4200D
+        IG_THS_ZL      = 0x37, // D20H
+        INT1_THS_ZL    = 0x37, // D20, 4200D
+        IG_DURATION    = 0x38, // D20H
+        INT1_DURATION  = 0x38, // D20, 4200D
+
+        LOW_ODR        = 0x39  // D20H
+    };
+
+    /*
+     *  ======== g ========
+     *  gyro angular velocity readings
+     */
+    vector<int16_t> g;
+
+    /*
+     *  ======== last_status ========
+     *  status of last I2C transmission
+     */
+    byte last_status;
+
+    /*
+     *  ======== L3G ========
+     *  Constructor
+     */
+    L3G(void);
+
+    /*
+     *  ======== init ========
+     */
+    bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
+
+    /*
+     *  ======== getDeviceType ========
+     */
+    deviceType getDeviceType(void) { return _device; }
+
+    /*
+     *  ======== enableDefault ========
+     */
+    void enableDefault(void);
+
+    /*
+     *  ======== writeReg ========
+     */
+    void writeReg(byte reg, byte value);
+
+    /*
+     *  ======== readReg ========
+     */
+    byte readReg(byte reg);
+
+    /*
+     *  ======== read ========
+     */
+    void read(void);
+
+    /*
+     *  ======== setTimeout ========
+     */
+    void setTimeout(unsigned int timeout);
+
+    /*
+     *  ======== getTimeout ========
+     */
+    unsigned int getTimeout(void);
+
+    /*
+     *  ======== timeoutOccurred ========
+     */
+    bool timeoutOccurred(void);
+
+    /*
+     *  ======== vector_cross ========
+     */
+    template <typename Ta, typename Tb, typename To>
+        static void vector_cross(
+            const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);
+
+    /*
+     *  ======== vector_dot ========
+     */
+    template <typename Ta, typename Tb>
+        static float vector_dot(const vector<Ta> *a, const vector<Tb> *b);
+
+    /*
+     *  ======== vector_normalize ========
+     */
+    static void vector_normalize(vector<float> *a);
+
+  private:
+    deviceType   _device; // chip type (D20H, D20, or 4200D)
+    byte         address;
+
+    unsigned int io_timeout;
+    bool         did_timeout;
+};
+
+/*
+ *  ======== vector_cross ========
+ */
+template <typename Ta, typename Tb, typename To>
+    void L3G::vector_cross(
+        const vector<Ta> *a, const vector<Tb> *b, vector<To> *out)
+{
+    out->x = (a->y * b->z) - (a->z * b->y);
+    out->y = (a->z * b->x) - (a->x * b->z);
+    out->z = (a->x * b->y) - (a->y * b->x);
+}
+
+/*
+ *  ======== vector_dot ========
+ */
+template <typename Ta, typename Tb>
+    float L3G::vector_dot(const vector<Ta> *a, const vector<Tb> *b)
+{
+    return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
+}
+
+#endif
diff --git a/src/Energia/ZumoTest/LSM303.cpp b/src/Energia/ZumoTest/LSM303.cpp
new file mode 100644 (file)
index 0000000..ec21ec0
--- /dev/null
@@ -0,0 +1,498 @@
+#include <Wire.h>
+#include <math.h>
+
+#include "LSM303.h"
+
+#include <xdc/runtime/System.h>
+
+// Defines ////////////////////////////////////////////////////////////////
+
+// The Arduino two-wire interface uses a 7-bit number for the address,
+// and sets the last bit correctly based on reads and writes
+#define D_SA0_HIGH_ADDRESS                0b0011101
+#define D_SA0_LOW_ADDRESS                 0b0011110
+#define DLHC_DLM_DLH_MAG_ADDRESS          0b0011110
+#define DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS 0b0011001
+#define DLM_DLH_ACC_SA0_LOW_ADDRESS       0b0011000
+
+#define TEST_REG_ERROR -1
+
+#define D_WHO_ID    0x49
+#define DLM_WHO_ID  0x3C
+
+static int testReg(byte address, LSM303::regAddr reg);
+
+// Constructors ///////////////////////////////////////////////////////////////
+
+LSM303::LSM303(void)
+{
+    /*
+     * These values lead to an assumed magnetometer bias of 0.
+     * Use the Calibrate example program to determine appropriate values
+     * for your particular unit. The Heading example demonstrates how to
+     * adjust these values in your own sketch.
+     */
+    m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
+    m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
+
+    _device = device_auto;
+
+    io_timeout = 0;  // 0 = no timeout
+    did_timeout = false;
+}
+
+// Public Methods /////////////////////////////////////////////////////////////
+
+// Did a timeout occur in readAcc(), readMag(), or read() since the last call to timeoutOccurred()?
+bool LSM303::timeoutOccurred()
+{
+    bool tmp = did_timeout;
+    did_timeout = false;
+    return tmp;
+}
+
+void LSM303::setTimeout(unsigned int timeout)
+{
+    io_timeout = timeout;
+}
+
+unsigned int LSM303::getTimeout()
+{
+    return io_timeout;
+}
+
+bool LSM303::init(deviceType device, sa0State sa0)
+{
+    // perform auto-detection unless device type and SA0 state were both specified
+    if (device == device_auto || sa0 == sa0_auto)
+    {
+        // check for LSM303D if device is unidentified or was specified to be this type
+        if (device == device_auto || device == device_D)
+        {
+            // check SA0 high address unless SA0 was specified to be low
+            if (sa0 != sa0_low && testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID)
+            {
+                // device responds to address 0011101 with D ID; it's a D with SA0 high
+                device = device_D;
+                sa0 = sa0_high;
+            }
+            // check SA0 low address unless SA0 was specified to be high
+            else if (sa0 != sa0_high && testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID)
+            {
+                // device responds to address 0011110 with D ID; it's a D with SA0 low
+                device = device_D;
+                sa0 = sa0_low;
+            }
+        }
+    
+        // check for LSM303DLHC, DLM, DLH if device is still unidentified or was specified to be one of these types
+        if (device == device_auto || device == device_DLHC || device == device_DLM || device == device_DLH)
+        {
+            // check SA0 high address unless SA0 was specified to be low
+            if (sa0 != sa0_low && testReg(DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
+            {
+                // device responds to address 0011001; it's a DLHC, DLM with SA0 high, or DLH with SA0 high
+                sa0 = sa0_high;
+                if (device == device_auto)
+                { 
+                    // use magnetometer WHO_AM_I register to determine device type
+                    //
+                    // DLHC seems to respond to WHO_AM_I request the same way as DLM, even though this
+                    // register isn't documented in its datasheet. Since the DLHC accelerometer address is the
+                    // same as the DLM with SA0 high, but Pololu DLM boards pull SA0 low by default, we'll
+                    // guess that a device whose accelerometer responds to the SA0 high address and whose
+                    // magnetometer gives the DLM ID is actually a DLHC.
+                    device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLHC : device_DLH;
+                }
+            }
+            // check SA0 low address unless SA0 was specified to be high
+            else if (sa0 != sa0_high && testReg(DLM_DLH_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
+            {
+                // device responds to address 0011000; it's a DLM with SA0 low or DLH with SA0 low
+                sa0 = sa0_low;
+                if (device == device_auto)
+                {
+                    // use magnetometer WHO_AM_I register to determine device type
+                    device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLM : device_DLH;
+                }
+            }
+        }
+    
+        // make sure device and SA0 were successfully detected; otherwise, indicate failure
+        if (device == device_auto || sa0 == sa0_auto)
+        {
+            return false;
+        }
+    }
+  
+    _device = device;
+  
+    // set device addresses and translated register addresses
+    switch (device) {
+        case device_D:
+            acc_address = mag_address = (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS;
+            translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;
+            translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;
+            translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;
+            translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;
+            translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;
+            translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;
+            break;
+
+        case device_DLHC:
+            acc_address = DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have configurable SA0 but uses same acc address as DLM/DLH with SA0 high
+            mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
+            translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M;
+            translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M;
+            translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M;
+            translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M;
+            translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M;
+            translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M;
+            break;
+
+        case device_DLM:
+            acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
+            mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
+            translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M;
+            translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M;
+            translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M;
+            translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M;
+            translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M;
+            translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M;
+            break;
+
+        case device_DLH:
+            acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
+            mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
+            translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M;
+            translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M;
+            translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M;
+            translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M;
+            translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M;
+            translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M;
+            break;
+    }
+  
+    return true;
+}
+
+/*
+Enables the LSM303's accelerometer and magnetometer. Also:
+- Sets sensor full scales (gain) to default power-on values, which are
+  +/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer
+  (+/- 4 gauss on LSM303D).
+- Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz
+  ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR
+  settings for which the electrical characteristics are specified in
+  the datasheets.)
+- Enables high resolution modes (if available).
+Note that this function will also reset other settings controlled by
+the registers it writes to.
+*/
+void LSM303::enableDefault(void)
+{
+    if (_device == device_D)
+    {
+        // Accelerometer
+
+        // 0x00 = 0b00000000
+        // AFS = 0 (+/- 2 g full scale)
+        writeReg(CTRL2, 0x00);
+
+        // 0x57 = 0b01010111
+        // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
+        writeReg(CTRL1, 0x57);
+
+        // Magnetometer
+
+        // 0x64 = 0b01100100
+        // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
+        writeReg(CTRL5, 0x64);
+
+        // 0x20 = 0b00100000
+        // MFS = 01 (+/- 4 gauss full scale)
+        writeReg(CTRL6, 0x20);
+
+        // 0x00 = 0b00000000
+        // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
+        writeReg(CTRL7, 0x00);
+    }
+    else
+    {
+        // Accelerometer
+    
+        if (_device == device_DLHC)
+        {
+            // 0x08 = 0b00001000
+            // FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable)
+            writeAccReg(CTRL_REG4_A, 0x08);
+
+            // 0x47 = 0b01000111
+            // ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
+            writeAccReg(CTRL_REG1_A, 0x47);
+        }
+        else // DLM, DLH
+        {
+            // 0x00 = 0b00000000
+            // FS = 00 (+/- 2 g full scale)
+            writeAccReg(CTRL_REG4_A, 0x00);
+
+            // 0x27 = 0b00100111
+            // PM = 001 (normal mode); DR = 00 (50 Hz ODR); Zen = Yen = Xen = 1 (all axes enabled)
+            writeAccReg(CTRL_REG1_A, 0x27);
+        }
+
+        // Magnetometer
+
+        // 0x0C = 0b00001100
+        // DO = 011 (7.5 Hz ODR)
+        writeMagReg(CRA_REG_M, 0x0C);
+
+        // 0x20 = 0b00100000
+        // GN = 001 (+/- 1.3 gauss full scale)
+        writeMagReg(CRB_REG_M, 0x20);
+
+        // 0x00 = 0b00000000
+        // MD = 00 (continuous-conversion mode)
+        writeMagReg(MR_REG_M, 0x00);
+    }
+}
+
+// Writes an accelerometer register
+void LSM303::writeAccReg(byte reg, byte value)
+{
+    Wire.beginTransmission(acc_address);
+    Wire.write(reg);
+    Wire.write(value);
+    last_status = Wire.endTransmission();
+}
+
+// Reads an accelerometer register
+byte LSM303::readAccReg(byte reg)
+{
+    byte value;
+
+    Wire.beginTransmission(acc_address);
+    Wire.write(reg);
+    last_status = Wire.endTransmission();
+    Wire.requestFrom(acc_address, (byte)1);
+    value = Wire.read();
+    Wire.endTransmission();
+
+    return value;
+}
+
+// Writes a magnetometer register
+void LSM303::writeMagReg(byte reg, byte value)
+{
+    Wire.beginTransmission(mag_address);
+    Wire.write(reg);
+    Wire.write(value);
+    last_status = Wire.endTransmission();
+}
+
+// Reads a magnetometer register
+byte LSM303::readMagReg(int reg)
+{
+    byte value;
+
+    // if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type)
+    if (reg < 0)
+    {
+        reg = translated_regs[-reg];
+    }
+
+    Wire.beginTransmission(mag_address);
+    Wire.write(reg);
+    last_status = Wire.endTransmission();
+    Wire.requestFrom(mag_address, (byte)1);
+    value = Wire.read();
+    Wire.endTransmission();
+
+    return value;
+}
+
+void LSM303::writeReg(byte reg, byte value)
+{
+    // mag address == acc_address for LSM303D, so it doesn't really matter which one we use.
+    if (_device == device_D || reg < CTRL_REG1_A)
+    {
+        writeMagReg(reg, value);
+    }
+    else
+    {
+        writeAccReg(reg, value);
+    }
+}
+
+// Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC.
+// To read those two registers, use readMagReg() instead.
+byte LSM303::readReg(int reg)
+{
+    // mag address == acc_address for LSM303D, so it doesn't really matter which one we use.
+    // Use readMagReg so it can translate OUT_[XYZ]_[HL]_M
+    if (_device == device_D || reg < CTRL_REG1_A)
+    {
+        return readMagReg(reg);
+    }
+    else
+    {
+        return readAccReg(reg);
+    }
+}
+
+// Reads the 3 accelerometer channels and stores them in vector a
+void LSM303::readAcc(void)
+{
+    Wire.beginTransmission(acc_address);
+    // assert the MSB of the address to get the accelerometer
+    // to do slave-transmit subaddress updating.
+    Wire.write(OUT_X_L_A | (1 << 7));
+    last_status = Wire.endTransmission();
+    Wire.requestFrom(acc_address, (byte)6);
+
+    unsigned int millis_start = millis();
+    while (Wire.available() < 6) {
+        if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)
+        {
+            did_timeout = true;
+            return;
+        }
+    }
+
+    byte xla = Wire.read();
+    byte xha = Wire.read();
+    byte yla = Wire.read();
+    byte yha = Wire.read();
+    byte zla = Wire.read();
+    byte zha = Wire.read();
+
+    // combine high and low bytes
+    // This no longer drops the lowest 4 bits of the readings from the DLH/DLM/DLHC, which are always 0
+    // (12-bit resolution, left-aligned). The D has 16-bit resolution
+    a.x = (int16_t)(xha << 8 | xla);
+    a.y = (int16_t)(yha << 8 | yla);
+    a.z = (int16_t)(zha << 8 | zla);
+}
+
+// Reads the 3 magnetometer channels and stores them in vector m
+void LSM303::readMag(void)
+{
+    Wire.beginTransmission(mag_address);
+    // If LSM303D, assert MSB to enable subaddress updating
+    // OUT_X_L_M comes first on D, OUT_X_H_M on others
+    Wire.write((_device == device_D) ? translated_regs[-OUT_X_L_M] | (1 << 7) : translated_regs[-OUT_X_H_M]);
+    last_status = Wire.endTransmission();
+    Wire.requestFrom(mag_address, (byte)6);
+
+    unsigned int millis_start = millis();
+    while (Wire.available() < 6) {
+        if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)
+        {
+            did_timeout = true;
+            return;
+        }
+    }
+
+    byte xlm, xhm, ylm, yhm, zlm, zhm;
+
+    if (_device == device_D)
+    {
+        // D: X_L, X_H, Y_L, Y_H, Z_L, Z_H
+        xlm = Wire.read();
+        xhm = Wire.read();
+        ylm = Wire.read();
+        yhm = Wire.read();
+        zlm = Wire.read();
+        zhm = Wire.read();
+    }
+    else
+    {
+        // DLHC, DLM, DLH: X_H, X_L...
+        xhm = Wire.read();
+        xlm = Wire.read();
+
+        if (_device == device_DLH)
+        {
+            // DLH: ...Y_H, Y_L, Z_H, Z_L
+            yhm = Wire.read();
+            ylm = Wire.read();
+            zhm = Wire.read();
+            zlm = Wire.read();
+        }
+        else
+        {
+            // DLM, DLHC: ...Z_H, Z_L, Y_H, Y_L
+            zhm = Wire.read();
+            zlm = Wire.read();
+            yhm = Wire.read();
+            ylm = Wire.read();
+        }
+    }
+
+    // combine high and low bytes
+    m.x = (int16_t)(xhm << 8 | xlm);
+    m.y = (int16_t)(yhm << 8 | ylm);
+    m.z = (int16_t)(zhm << 8 | zlm);
+}
+
+// Reads all 6 channels of the LSM303 and stores them in the object variables
+void LSM303::read(void)
+{
+    readAcc();
+    readMag();
+}
+
+/*
+  Returns the angular difference in the horizontal plane between a
+  default vector and north, in degrees.
+
+  The default vector here is chosen to point along the surface of the
+  PCB, in the direction of the top of the text on the silkscreen.
+  This is the +X axis on the Pololu LSM303D carrier and the -Y axis on
+  the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH carriers.
+*/
+float LSM303::heading(void)
+{
+    if (_device == device_D)
+    {
+        return heading((vector<int>){1, 0, 0});
+    }
+    else
+    {
+        return heading((vector<int>){0, -1, 0});
+    }
+}
+
+void LSM303::vector_normalize(vector<float> *a)
+{
+    float mag = sqrt(vector_dot(a, a));
+    a->x /= mag;
+    a->y /= mag;
+    a->z /= mag;
+}
+
+// Private Methods ////////////////////////////////////////////////////////////
+
+static int testReg(byte address, LSM303::regAddr reg)
+{
+    System_printf("testReg: 0x%x : 0x%x ...", address, reg);
+    Wire.beginTransmission(address);
+    System_printf(" begin ...");
+    Wire.write((byte)reg);
+    if (Wire.endTransmission() != 0) {
+        System_printf("testReg: error\n");
+        return TEST_REG_ERROR;
+    }
+
+    System_printf(" request from 0x%x...", address);
+    Wire.requestFrom(address, (byte)1);
+    System_printf(" read ...");
+    if (Wire.available()) {
+        return Wire.read();
+    }
+    else {
+        System_printf("testReg: error\n");
+        return TEST_REG_ERROR;
+    }
+}
diff --git a/src/Energia/ZumoTest/LSM303.h b/src/Energia/ZumoTest/LSM303.h
new file mode 100644 (file)
index 0000000..31e3382
--- /dev/null
@@ -0,0 +1,349 @@
+#ifndef LSM303_h
+#define LSM303_h
+
+#include <Arduino.h> // for byte data type
+
+class LSM303
+{
+  public:
+    template <typename T> struct vector {
+        T x, y, z;
+    };
+
+    enum deviceType { device_DLH, device_DLM, device_DLHC, device_D, device_auto };
+    enum sa0State { sa0_low, sa0_high, sa0_auto };
+
+    // register addresses
+    enum regAddr {
+        TEMP_OUT_L        = 0x05, // D
+        TEMP_OUT_H        = 0x06, // D
+
+        STATUS_M          = 0x07, // D
+
+        INT_CTRL_M        = 0x12, // D
+        INT_SRC_M         = 0x13, // D
+        INT_THS_L_M       = 0x14, // D
+        INT_THS_H_M       = 0x15, // D
+
+        OFFSET_X_L_M      = 0x16, // D
+        OFFSET_X_H_M      = 0x17, // D
+        OFFSET_Y_L_M      = 0x18, // D
+        OFFSET_Y_H_M      = 0x19, // D
+        OFFSET_Z_L_M      = 0x1A, // D
+        OFFSET_Z_H_M      = 0x1B, // D
+        REFERENCE_X       = 0x1C, // D
+        REFERENCE_Y       = 0x1D, // D
+        REFERENCE_Z       = 0x1E, // D
+
+        CTRL0             = 0x1F, // D
+        CTRL1             = 0x20, // D
+        CTRL_REG1_A       = 0x20, // DLH, DLM, DLHC
+        CTRL2             = 0x21, // D
+        CTRL_REG2_A       = 0x21, // DLH, DLM, DLHC
+        CTRL3             = 0x22, // D
+        CTRL_REG3_A       = 0x22, // DLH, DLM, DLHC
+        CTRL4             = 0x23, // D
+        CTRL_REG4_A       = 0x23, // DLH, DLM, DLHC
+        CTRL5             = 0x24, // D
+        CTRL_REG5_A       = 0x24, // DLH, DLM, DLHC
+        CTRL6             = 0x25, // D
+        CTRL_REG6_A       = 0x25, // DLHC
+        HP_FILTER_RESET_A = 0x25, // DLH, DLM
+        CTRL7             = 0x26, // D
+        REFERENCE_A       = 0x26, // DLH, DLM, DLHC
+        STATUS_A          = 0x27, // D
+        STATUS_REG_A      = 0x27, // DLH, DLM, DLHC
+
+        OUT_X_L_A         = 0x28,
+        OUT_X_H_A         = 0x29,
+        OUT_Y_L_A         = 0x2A,
+        OUT_Y_H_A         = 0x2B,
+        OUT_Z_L_A         = 0x2C,
+        OUT_Z_H_A         = 0x2D,
+
+        FIFO_CTRL         = 0x2E, // D
+        FIFO_CTRL_REG_A   = 0x2E, // DLHC
+        FIFO_SRC          = 0x2F, // D
+        FIFO_SRC_REG_A    = 0x2F, // DLHC
+
+        IG_CFG1           = 0x30, // D
+        INT1_CFG_A        = 0x30, // DLH, DLM, DLHC
+        IG_SRC1           = 0x31, // D
+        INT1_SRC_A        = 0x31, // DLH, DLM, DLHC
+        IG_THS1           = 0x32, // D
+        INT1_THS_A        = 0x32, // DLH, DLM, DLHC
+        IG_DUR1           = 0x33, // D
+        INT1_DURATION_A   = 0x33, // DLH, DLM, DLHC
+        IG_CFG2           = 0x34, // D
+        INT2_CFG_A        = 0x34, // DLH, DLM, DLHC
+        IG_SRC2           = 0x35, // D
+        INT2_SRC_A        = 0x35, // DLH, DLM, DLHC
+        IG_THS2           = 0x36, // D
+        INT2_THS_A        = 0x36, // DLH, DLM, DLHC
+        IG_DUR2           = 0x37, // D
+        INT2_DURATION_A   = 0x37, // DLH, DLM, DLHC
+
+        CLICK_CFG         = 0x38, // D
+        CLICK_CFG_A       = 0x38, // DLHC
+        CLICK_SRC         = 0x39, // D
+        CLICK_SRC_A       = 0x39, // DLHC
+        CLICK_THS         = 0x3A, // D
+        CLICK_THS_A       = 0x3A, // DLHC
+        TIME_LIMIT        = 0x3B, // D
+        TIME_LIMIT_A      = 0x3B, // DLHC
+        TIME_LATENCY      = 0x3C, // D
+        TIME_LATENCY_A    = 0x3C, // DLHC
+        TIME_WINDOW       = 0x3D, // D
+        TIME_WINDOW_A     = 0x3D, // DLHC
+
+        Act_THS           = 0x3E, // D
+        Act_DUR           = 0x3F, // D
+
+        CRA_REG_M         = 0x00, // DLH, DLM, DLHC
+        CRB_REG_M         = 0x01, // DLH, DLM, DLHC
+        MR_REG_M          = 0x02, // DLH, DLM, DLHC
+
+        SR_REG_M          = 0x09, // DLH, DLM, DLHC
+        IRA_REG_M         = 0x0A, // DLH, DLM, DLHC
+        IRB_REG_M         = 0x0B, // DLH, DLM, DLHC
+        IRC_REG_M         = 0x0C, // DLH, DLM, DLHC
+
+        WHO_AM_I          = 0x0F, // D
+        WHO_AM_I_M        = 0x0F, // DLM
+
+        TEMP_OUT_H_M      = 0x31, // DLHC
+        TEMP_OUT_L_M      = 0x32, // DLHC
+
+        // dummy addresses for registers in different locations on different
+        // devices; the library translates these based on device type value
+        // with sign flipped is used as index into translated_regs array
+
+        OUT_X_H_M         = -1,
+        OUT_X_L_M         = -2,
+        OUT_Y_H_M         = -3,
+        OUT_Y_L_M         = -4,
+        OUT_Z_H_M         = -5,
+        OUT_Z_L_M         = -6,
+        // update dummy_reg_count if registers are added here!
+
+        // device-specific register addresses
+        DLH_OUT_X_H_M     = 0x03,
+        DLH_OUT_X_L_M     = 0x04,
+        DLH_OUT_Y_H_M     = 0x05,
+        DLH_OUT_Y_L_M     = 0x06,
+        DLH_OUT_Z_H_M     = 0x07,
+        DLH_OUT_Z_L_M     = 0x08,
+
+        DLM_OUT_X_H_M     = 0x03,
+        DLM_OUT_X_L_M     = 0x04,
+        DLM_OUT_Z_H_M     = 0x05,
+        DLM_OUT_Z_L_M     = 0x06,
+        DLM_OUT_Y_H_M     = 0x07,
+        DLM_OUT_Y_L_M     = 0x08,
+
+        DLHC_OUT_X_H_M    = 0x03,
+        DLHC_OUT_X_L_M    = 0x04,
+        DLHC_OUT_Z_H_M    = 0x05,
+        DLHC_OUT_Z_L_M    = 0x06,
+        DLHC_OUT_Y_H_M    = 0x07,
+        DLHC_OUT_Y_L_M    = 0x08,
+
+        D_OUT_X_L_M       = 0x08,
+        D_OUT_X_H_M       = 0x09,
+        D_OUT_Y_L_M       = 0x0A,
+        D_OUT_Y_H_M       = 0x0B,
+        D_OUT_Z_L_M       = 0x0C,
+        D_OUT_Z_H_M       = 0x0D
+    };
+
+    vector<int16_t> a;     // accelerometer readings
+    vector<int16_t> m;     // magnetometer readings
+    vector<int16_t> m_max; // maximum magnetometer values, used for calibration
+    vector<int16_t> m_min; // minimum magnetometer values, used for calibration
+
+    /*
+     *  ======== last_status ========
+     */
+    byte last_status;      // status of last I2C transmission
+
+    /*
+     *  ======== LSM303 ========
+     */
+    LSM303(void);
+
+    /*
+     *  ======== init ========
+     */
+    bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
+
+    /*
+     *  ======== getDeviceType ========
+     */
+    deviceType getDeviceType(void) { return _device; }
+
+    /*
+     *  ======== enableDefault ========
+     */
+    void enableDefault(void);
+
+    /*
+     *  ======== writeAccReg ========
+     */
+    void writeAccReg(byte reg, byte value);
+
+    /*
+     *  ======== readAccReg ========
+     */
+    byte readAccReg(byte reg);
+
+    /*
+     *  ======== writeMagReg ========
+     */
+    void writeMagReg(byte reg, byte value);
+
+    /*
+     *  ======== readMagReg ========
+     */
+    byte readMagReg(int reg);
+
+    /*
+     *  ======== writeReg ========
+     */
+    void writeReg(byte reg, byte value);
+
+    /*
+     *  ======== readReg ========
+     */
+    byte readReg(int reg);
+
+    /*
+     *  ======== readAcc ========
+     */
+    void readAcc(void);
+
+    /*
+     *  ======== readMag ========
+     */
+    void readMag(void);
+
+    /*
+     *  ======== read ========
+     */
+    void read(void);
+
+    /*
+     *  ======== setTimeout ========
+     */
+    void setTimeout(unsigned int timeout);
+
+    /*
+     *  ======== getTimeout ========
+     */
+    unsigned int getTimeout(void);
+
+    /*
+     *  ======== timeoutOccurred ========
+     */
+    bool timeoutOccurred(void);
+
+    /*
+     *  ======== heading ========
+     */
+    float heading(void);
+
+    template <typename T> float heading(vector<T> from);
+
+    /*
+     *  ======== vector_cross ========
+     */
+    template <typename Ta, typename Tb, typename To>
+        static void vector_cross(
+            const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);
+
+    /*
+     *  ======== vector_dot ========
+     */
+    template <typename Ta, typename Tb>
+        static float vector_dot(const vector<Ta> *a, const vector<Tb> *b);
+
+    /*
+     *  ======== vector_normalize ========
+     */
+    static void vector_normalize(vector<float> *a);
+
+  private:
+    deviceType _device; // chip type (D, DLHC, DLM, or DLH)
+    byte acc_address;
+    byte mag_address;
+
+    static const int dummy_reg_count = 6;
+    regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used
+
+    unsigned int io_timeout;
+    bool did_timeout;
+};
+
+/*
+ *  ======== heading ========
+ *  Returns the angular difference in the horizontal plane between the
+ *  "from" vector and north, in degrees.
+ *
+ *  Description of heading algorithm:
+ *    Shift and scale the magnetic reading based on calibration data to find
+ *    the North vector. Use the acceleration readings to determine the Up
+ *    vector (gravity is measured as an upward acceleration). The cross
+ *    product of North and Up vectors is East. The vectors East and North
+ *    form a basis for the horizontal plane. The From vector is projected
+ *    into the horizontal plane and the angle between the projected vector
+ *    and horizontal north is returned.
+ */
+template <typename T> float LSM303::heading(vector<T> from)
+{
+    vector<int32_t> temp_m = {m.x, m.y, m.z};
+
+    /* subtract offset (average of min and max) from magnetometer readings */
+    temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
+    temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
+    temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
+
+    /* compute E and N */
+    vector<float> E;
+    vector<float> N;
+
+    vector_cross(&temp_m, &a, &E);
+    vector_normalize(&E);
+    vector_cross(&a, &E, &N);
+    vector_normalize(&N);
+
+    /* compute heading */
+    float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;
+    if (heading < 0) {
+        heading += 360;
+    }
+    return heading;
+}
+
+/*
+ *  ======== vector_cross ========
+ *  To = Ta x Tb
+ */
+template <typename Ta, typename Tb, typename To>
+    void LSM303::vector_cross(
+        const vector<Ta> *a, const vector<Tb> *b, vector<To> *out)
+{
+    out->x = (a->y * b->z) - (a->z * b->y);
+    out->y = (a->z * b->x) - (a->x * b->z);
+    out->z = (a->x * b->y) - (a->y * b->x);
+}
+
+/*
+ *  ======== vector_dot ========
+ *  Returns Ta * Tb
+ */
+template <typename Ta, typename Tb>
+    float LSM303::vector_dot(const vector<Ta> *a, const vector<Tb> *b)
+{
+    return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
+}
+
+#endif
diff --git a/src/Energia/ZumoTest/ZumoMotors.cpp b/src/Energia/ZumoTest/ZumoMotors.cpp
new file mode 100644 (file)
index 0000000..d720e48
--- /dev/null
@@ -0,0 +1,131 @@
+#include "ZumoMotors.h"
+
+#define REDBEAR 1
+
+#ifdef REDBEAR
+// redbear cc3200 (PWM only available on pins 5 and 6)
+#define PWM_L 6
+#define PWM_R 5 
+#else
+#define PWM_L 10
+#define PWM_R 9
+#endif
+
+#define DIR_L 8
+#define DIR_R 7
+
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) || defined (__AVR_ATmega32U4__)
+  #define USE_20KHZ_PWM
+#endif
+#undef USE_20KHZ_PWM // redbear cc3200 (need portable, non-AVR, code)
+
+static boolean flipLeft = false;
+static boolean flipRight = false;
+
+// constructor (doesn't do anything)
+ZumoMotors::ZumoMotors()
+{
+}
+
+// initialize timer1 to generate the proper PWM outputs to the motor drivers
+void ZumoMotors::init2()
+{
+  pinMode(PWM_L,  OUTPUT);
+  pinMode(PWM_R,  OUTPUT);
+  pinMode(DIR_L, OUTPUT);
+  pinMode(DIR_R, OUTPUT);
+#ifdef REDBEAR
+  /* redbear pins 5 & 6 are connected to pins 9 & 10, so we these to be input
+   * so they don't inadvertently drive
+   */
+  pinMode(10, INPUT);    
+  pinMode(9, INPUT);
+#endif
+
+#ifdef USE_20KHZ_PWM
+  // Timer 1 configuration
+  // prescaler: clockI/O / 1
+  // outputs enabled
+  // phase-correct PWM
+  // top of 400
+  //
+  // PWM frequency calculation
+  // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz
+  TCCR1A = 0b10100000;
+  TCCR1B = 0b00010001;
+  ICR1 = 400;
+#endif
+}
+
+// enable/disable flipping of left motor
+void ZumoMotors::flipLeftMotor(boolean flip)
+{
+  flipLeft = flip;
+}
+
+// enable/disable flipping of right motor
+void ZumoMotors::flipRightMotor(boolean flip)
+{
+  flipRight = flip;
+}
+
+// set speed for left motor; speed is a number between -400 and 400
+void ZumoMotors::setLeftSpeed(int speed)
+{
+  init(); // initialize if necessary
+    
+  boolean reverse = 0;
+  
+  if (speed < 0)
+  {
+    speed = -speed; // make speed a positive quantity
+    reverse = 1;    // preserve the direction
+  }
+  if (speed > 400)  // Max 
+    speed = 400;
+    
+#ifdef USE_20KHZ_PWM
+  OCR1B = speed;
+#else
+  analogWrite(PWM_L, speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
+#endif 
+
+  if (reverse ^ flipLeft) // flip if speed was negative or flipLeft setting is active, but not both
+    digitalWrite(DIR_L, HIGH);
+  else
+    digitalWrite(DIR_L, LOW);
+}
+
+// set speed for right motor; speed is a number between -400 and 400
+void ZumoMotors::setRightSpeed(int speed)
+{
+  init(); // initialize if necessary
+    
+  boolean reverse = 0;
+  
+  if (speed < 0)
+  {
+    speed = -speed;  // Make speed a positive quantity
+    reverse = 1;  // Preserve the direction
+  }
+  if (speed > 400)  // Max PWM dutycycle
+    speed = 400;
+    
+#ifdef USE_20KHZ_PWM
+  OCR1A = speed;
+#else
+  analogWrite(PWM_R, speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
+#endif
+
+  if (reverse ^ flipRight) // flip if speed was negative or flipRight setting is active, but not both
+    digitalWrite(DIR_R, HIGH);
+  else
+    digitalWrite(DIR_R, LOW);
+}
+
+// set speed for both motors
+void ZumoMotors::setSpeeds(int leftSpeed, int rightSpeed)
+{
+  setLeftSpeed(leftSpeed);
+  setRightSpeed(rightSpeed);
+}
diff --git a/src/Energia/ZumoTest/ZumoMotors.h b/src/Energia/ZumoTest/ZumoMotors.h
new file mode 100644 (file)
index 0000000..9606aed
--- /dev/null
@@ -0,0 +1,48 @@
+/*! \file ZumoMotors.h
+ *
+ * See the ZumoMotors class reference for more information about this library.
+ *
+ * \class ZumoMotors ZumoMotors.h
+ * \brief Control motor speed and direction
+ * 
+ */
+
+#ifndef ZumoMotors_h
+#define ZumoMotors_h
+
+#include <Energia.h>
+
+class ZumoMotors
+{
+  public:  
+  
+    // constructor (doesn't do anything)
+    ZumoMotors();
+    
+    // enable/disable flipping of motors
+    static void flipLeftMotor(boolean flip);
+    static void flipRightMotor(boolean flip);
+    
+    // set speed for left, right, or both motors
+    static void setLeftSpeed(int speed);
+    static void setRightSpeed(int speed);
+    static void setSpeeds(int leftSpeed, int rightSpeed);
+    
+  private:
+
+    static inline void init()
+    {
+      static boolean initialized = false;
+
+      if (!initialized)
+      {
+        initialized = true;
+        init2();
+      }
+    }
+    
+    // initializes timer1 for proper PWM generation
+    static void init2();
+};
+
+#endif
diff --git a/src/Energia/ZumoTest/ZumoTest.ino b/src/Energia/ZumoTest/ZumoTest.ino
new file mode 100644 (file)
index 0000000..040f9e9
--- /dev/null
@@ -0,0 +1,29 @@
+/*  ======== ZumoTest.ino ========
+ *  Completely self-contained test of the Zumo CC3200
+ *
+ *  This example creates an access point named zumo-test with password
+ *  "password" and a simple command/telemetry server on port 8080.
+ *
+ *  The command server allows any WiFi client to drive the Zumo using WASD
+ *  keystrokes and acquire IMU telemetry data for real-time display by a
+ *  client program running on a laptop.
+ *
+ *  Two examples of such a client are provided in the ../Processing/ManualDrive
+ *  sub-directory: zgraph and zecho.  Both are Processing applications (which
+ *  uses the development IDE that inspired the Energia/Wiring development
+ *  environment; see https://processing.org).
+ */
+#include "L3G.h"
+#include "LSM303.h"
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G    imuGyro;    /* gyro data */
+
+/* motor sketch external declarations */
+extern char motorCmd;
+extern int  motorLeft;
+extern int  motorRight;
+
+/* main external declarations */
+#define MAIN_LED_PIN RED_LED
diff --git a/src/Energia/ZumoTest/apLoop.ino b/src/Energia/ZumoTest/apLoop.ino
new file mode 100644 (file)
index 0000000..2b1b547
--- /dev/null
@@ -0,0 +1,191 @@
+/*
+ *  ======== apLoop ========
+ *  This sketch starts a network and listens on port PORTNUM for
+ *  command that can control the zumo motors.
+ *
+ *  The name and password of the network and the port number of the server
+ *  (always at IP address 192.168.1.1) can be changed below.
+ */
+
+#include <WiFi.h>
+#include <ti/sysbios/knl/Task.h>
+
+#include <string.h>
+
+/* name of the network and its password */
+static const char ssid[] = "zumo-test";
+static const char wifipw[] = "password";
+
+/* port number of the server listening for commands at 192.168.1.1 */
+#define PORTNUM 8080
+
+/* max and min PWM values to be sent to the motorLoop */
+#define PWM_MAX 400
+#define PWM_MIN 100
+
+/* create data server on port PORTNUM */
+static WiFiServer server(PORTNUM);
+
+static void doCMD(char *buffer, WiFiClient client);
+
+/*
+ *  ======== apSetup ========
+ */
+void apSetup()
+{
+    Serial.begin(9600);
+
+    /* set priority of this task to be lower than other tasks */
+    Task_setPri(Task_self(), 1);
+
+    /* startup a new network and get the first IP address: 192.168.1.1 */
+    Serial.print("Starting a new network: "); Serial.println(ssid);
+    WiFi.beginNetwork((char *)ssid, (char *)wifipw);
+    while (WiFi.localIP() == INADDR_NONE) {
+        Serial.print(".");
+        delay(300);
+    }
+
+    /* startup the command server on port PORTNUM */
+    server.begin();
+    Serial.print("dataserver started on port "); Serial.println(PORTNUM);
+}
+
+/*
+ *  ======== apLoop ========
+ */
+void apLoop()
+{
+    /* Did a client connect/disconnect since the last time we checked? */
+    if (WiFi.getTotalDevices() > 0) {
+
+        /* listen for incoming clients */
+        WiFiClient client = server.available();
+        if (client) {
+
+            /* if there's a client, read and process commands */
+            static char buffer[64] = {0};
+            int bufLen = 0;
+
+            /* while connected to the client, read commands and send results */
+            while (client.connected()) {
+                /* if there's a byte to read from the client .. */
+                if (client.available()) {
+                    /* copy it to the command buffer, byte at a time */
+                    char c = client.read();
+
+                    /* ignore bogus characters */
+                    if (c == '\0' || c == '\r') continue;
+                    
+                    /* never overrun the command buffer */
+                    if (bufLen >= (int)(sizeof (buffer))) { 
+                        bufLen = sizeof (buffer) - 1;
+                    }
+                    buffer[bufLen++] = c;
+
+                    /* if there's a new line, we have a complete command */
+                    if (c == '\n') {
+                        doCMD(buffer, client);
+                        
+                        /* reset command buffer index to get next command */
+                        bufLen = 0;
+                    }
+                }
+            }
+
+            /* client disconnected or timed out, close the connection */
+            client.flush();
+            client.stop();
+
+            /* disconnect => implicitly stop the motor */
+            motorCmd = ' ';
+        }
+    }
+
+    /* check for new connections 2 times per second */
+    delay(500);
+}
+
+/*
+ *  ======== getVelocityToPWM ========
+ *  Convert raw motor velocity values to PWM values
+ *  
+ *  Raw motor velocity values range from +/-100; 0 being off
+ *
+ *  A conversion can be done by
+ *  pwmOutput = factor * (logical velocity) + pwm_min
+ *  factor = (pwm_max / pwm_min)
+ */
+static int getVelocityToPWM(int velocity)
+{
+   velocity *= (PWM_MAX / PWM_MIN);
+   
+   if (velocity > 0) {
+       velocity = (velocity < PWM_MIN) ? PWM_MIN : velocity;
+   }
+   else if (velocity < 0) {
+       velocity = (velocity > -PWM_MIN) ? -PWM_MIN : velocity;
+   }
+
+   return (velocity);
+}
+
+/*
+ *  ======== doCMD ========
+ */
+static void doCMD(char *buffer, WiFiClient client)
+{
+    static char report[80];
+    int left = (~0);
+    int right = (~0);
+    char newCMD = ' ';
+    UInt savePri;
+    
+    /* parse the command in buffer[] */
+    switch (buffer[0]) {
+        /* handle raw motor control commands: "l<left>, r<right>" */
+        case 'l':
+            left = strtol(buffer + 1, &buffer, 10);
+            while (buffer[0] != '\n') {
+                if (buffer[0] == 'r') {
+                    right = strtol(buffer + 1, &buffer, 10);
+                    break;
+                }
+                buffer++;
+            }
+
+            newCMD = 'r';
+            break;
+            
+        case 'w':
+        case 'a':
+        case 's':
+        case 'd':
+            newCMD = buffer[0];
+            break;
+                
+        default:
+            break;
+    }
+    
+    /* atomically update motor control variables */
+    savePri = Task_setPri(Task_self(), Task_numPriorities - 1);
+    if (right != (~0)) {
+        motorRight = getVelocityToPWM(right);
+    }
+    if (left != (~0)) {
+        motorLeft = getVelocityToPWM(left);
+    }
+    motorCmd = newCMD;
+    Task_setPri(Task_self(), savePri);
+    
+    /* send current IMU data */
+    System_snprintf(report, sizeof(report),
+        "A: %6d %6d %6d G: %6d %6d %6d M: %6d %6d %6d",
+                    imuCompass.a.x, imuCompass.a.y, imuCompass.a.z,
+                    imuGyro.g.x,    imuGyro.g.y,    imuGyro.g.z, 
+                    imuCompass.m.x, imuCompass.m.y, imuCompass.m.z);
+    if (client.write((unsigned char *)report, 72) != 72) {
+        Serial.println("Error: reply failed, status != 72");
+    }
+}
diff --git a/src/Energia/ZumoTest/imuLoop.ino b/src/Energia/ZumoTest/imuLoop.ino
new file mode 100644 (file)
index 0000000..af8dd8c
--- /dev/null
@@ -0,0 +1,47 @@
+/*
+ *  ======== imuLoop ========
+ *  This sketch simply reads the Zumo IMU sensors at 20Hz and prints
+ *  the current value once per second to the UART.
+ */
+
+#include <Wire.h>
+
+#include "L3G.h"
+#include "LSM303.h"
+
+/* Pololu IMU data instance objects */
+LSM303 imuCompass; /* acceleration and magnetometer */
+L3G    imuGyro;    /* gyro data */
+
+/*
+ *  ======== imuSetup ========
+ */
+void imuSetup()
+{
+    Serial.begin(9600);
+    Serial.println("imuSetup ...");
+    Wire.begin();
+
+    /* initialize Zumo accelerometer and magnetometer */
+    imuCompass.init();
+    imuCompass.enableDefault();
+
+    /* initialize Zumo gyro */
+    if (!imuGyro.init()) {
+        Serial.print("Failed to autodetect gyro type!");
+        delay(1000);
+    }
+    imuGyro.enableDefault();
+    Serial.println("imuSetup done.");
+}
+
+/*
+ *  ======== imuLoop ========
+ */
+void imuLoop()
+{
+    /* update IMU data every 50 ms (200 Hz) */
+    imuGyro.read();
+    imuCompass.read();
+    delay(50);
+}
diff --git a/src/Energia/ZumoTest/motorLoop.ino b/src/Energia/ZumoTest/motorLoop.ino
new file mode 100644 (file)
index 0000000..75feeab
--- /dev/null
@@ -0,0 +1,176 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorCmd, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w' - drive forward
+ *      's' - drive backward
+ *      'a' - turn left
+ *      'd' - turn right
+ *      'r' - raw reads from motorLeft and motorRight
+ *      ' ' - stop
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorCmd.
+ */
+#include <Energia.h>
+
+#include "ZumoMotors.h"
+
+#define PERIOD  1           /* period of motor control updates */
+
+char motorCmd = ' ';       /* current motor drive command */
+int motorLeft = 0;
+int motorRight = 0;
+
+static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
+
+static int clip(int speed);
+static void drive(char wasdr, int goal, unsigned int duration);
+
+/*
+ *  ======== motorSetup ========
+ */
+void motorSetup(void)
+{
+    Serial.println("motorSetup ...");
+    /* initialize the Pololu driver motor library */
+    motors.setRightSpeed(0);
+    motors.setLeftSpeed(0);
+
+    /* setup an LED to indcate forward/backward movement or turning */
+    pinMode(MAIN_LED_PIN, OUTPUT);
+    Serial.println("motorSetup done.");
+}
+
+/*
+ *  ======== motorLoop ========
+ */
+void motorLoop(void)
+{
+    /* state used to blink LED */
+    static unsigned count = 0;
+    static char state = 0;
+    
+    switch (motorCmd) {
+        case 's':
+        case 'w': {
+            /* illuminate LED while driving */
+            digitalWrite(MAIN_LED_PIN, HIGH);
+            drive(motorCmd, 200, PERIOD);
+            break;
+        }
+
+        case 'd':
+        case 'a': {
+            /* blink LED while turning */
+            if (count == ((count / 100) * 100)) {
+                state ^= 1;
+                digitalWrite(MAIN_LED_PIN, state);
+            }   
+            drive(motorCmd, 100, PERIOD);
+            break;
+        }
+        
+        case 'r': {
+            /* blink LED while turning */
+            if (count == ((count / 50) * 50)) {
+                state ^= 1;
+                digitalWrite(MAIN_LED_PIN, state);
+            }  
+            drive('r', 0, PERIOD);
+            break;
+        }
+
+        default: {
+            /* turn off LED while stopped */
+            motorCmd = ' ';
+            digitalWrite(MAIN_LED_PIN, LOW);
+            drive(' ', 0, 10);
+            break;
+        }
+    }
+
+    count++;
+}
+
+/*
+ *  ======== clip ========
+ */
+static int clip(int speed)
+{
+    if (speed < -400) {
+        speed = -400;
+    }
+    else if (speed > 400) {
+        speed = 400;
+    }
+    return (speed);
+}
+
+/*
+ *  ======== drive ========
+ */
+#define P 0.9375f
+static void drive(char wasdr, int goal, unsigned int duration)
+{
+    static int leftSpeed = 0;
+    static int rightSpeed = 0;
+    
+    while (duration > 0) {
+        duration--;
+
+        /* gradually adjust curent speeds to goal */
+        switch (wasdr) {
+            case ' ': { /* stop */
+                leftSpeed = (int)(P * leftSpeed);
+                rightSpeed = (int)(P * rightSpeed);
+                break;
+            }
+            case 'w': { /* forward */
+                leftSpeed = (leftSpeed - goal) * P + goal;
+                rightSpeed = (rightSpeed - goal) * P + goal;
+                break;
+            }
+
+            case 's': { /* backward */
+                leftSpeed = (leftSpeed + goal) * P - goal;
+                rightSpeed = (rightSpeed + goal) * P - goal;
+                break;
+            }
+
+            case 'd': { /* turn right */
+                leftSpeed = (leftSpeed - goal) * P + goal;
+                rightSpeed = (rightSpeed + goal) * P - goal;
+                break;
+            }
+
+            case 'a': { /* turn left */
+                leftSpeed = (leftSpeed + goal) * P - goal;
+                rightSpeed = (rightSpeed - goal) * P + goal;
+                break;
+            }
+            
+            case 'r': {
+               leftSpeed = (leftSpeed - motorLeft) * P + motorLeft;
+               rightSpeed = (rightSpeed - motorRight) * P + motorRight;
+               break; 
+            }
+
+            default: {
+                break;
+            }
+        }
+
+        /* clip speeds to allowable range */
+        leftSpeed = clip(leftSpeed);
+        rightSpeed = clip(rightSpeed);
+    
+        /* set motor speeds */
+        motors.setLeftSpeed(leftSpeed);
+        motors.setRightSpeed(rightSpeed);
+
+        /* sleep for 1 ms (so duration is in milliseconds) */
+        delay(1);
+    }
+}