summary | shortlog | log | commit | commitdiff | tree
raw | patch | inline | side by side (parent: f2178de)
raw | patch | inline | side by side (parent: f2178de)
| author | dave russo <d-russo@ti.com> | |
| Fri, 6 Nov 2015 20:26:41 +0000 (12:26 -0800) | ||
| committer | dave russo <d-russo@ti.com> | |
| Fri, 6 Nov 2015 20:26:41 +0000 (12:26 -0800) |
add self-contained ZumoTest sketch
21 files changed:
diff --git a/.gitignore b/.gitignore
index 7da50c9b7c5c5952b40034b28ee246dceb1600d7..1afa61d5873b89bda848d09186c99e82622a22c1 100644 (file)
--- a/.gitignore
+++ b/.gitignore
/.reltree
/.generated_files
/bin/
+/build/
## emacs backup 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
--- /dev/null
+++ b/etc/ino2cpp/ebuild.sh
@@ -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
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
--- /dev/null
+++ b/etc/ino2cpp/ino2cpp.sh
@@ -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
--- /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
--- /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
--- /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
--- /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
--- /dev/null
+++ b/etc/rbload.bat
@@ -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
--- /dev/null
+++ b/etc/rbload.sh
@@ -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
diff --git a/makefile b/makefile
index 818331e37bf3b2b6fffbc8798914950d981d00f9..610c30f6f793174cd49f335c213e96e265ced42e 100644 (file)
--- a/makefile
+++ b/makefile
-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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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);
+ }
+}