From 07e4a3c849807c664568fed826a73b40045253e9 Mon Sep 17 00:00:00 2001 From: dave russo Date: Sun, 27 Sep 2015 20:46:12 -0700 Subject: [PATCH] initial commit of content from Adam Dai and Tony Oliverio --- .gitignore | 71 + README.html | 76 + etc/Markdown.pl | 1450 +++++++++++++++++ etc/build.svg | 34 + etc/github-markdown.css | 652 ++++++++ etc/md2html.ksh | 32 + license.html | 473 ++++++ makefile | 10 + src/Energia/libraries/ZumoCC3200/ZumoCC3200.h | 37 + .../examples/AssistedDrive/AssistedDrive.ino | 36 + .../AssistedDrive/MotorControlLoop.ino | 331 ++++ .../examples/AssistedDrive/SensorLoop.ino | 99 ++ .../examples/AssistedDrive/WifiLoop.ino | 219 +++ .../AttitudeDisplay/AttitudeDisplay.ino | 26 + .../examples/AttitudeDisplay/apLoop.ino | 169 ++ .../examples/AttitudeDisplay/imuLoop.ino | 133 ++ .../examples/AttitudeDisplay/motorLoop.ino | 175 ++ .../examples/Balancing/Balancing.ino | 23 + .../ZumoCC3200/examples/Balancing/apLoop.ino | 160 ++ .../ZumoCC3200/examples/Balancing/imuLoop.ino | 58 + .../examples/Balancing/motorLoop.ino | 175 ++ .../CommandExample/CommandExample.ino | 28 + .../CommandExample/MotorControlLoop.ino | 91 ++ .../examples/CommandExample/SensorLoop.ino | 70 + .../examples/CommandExample/WifiLoop.ino | 199 +++ .../examples/ManualDrive/ManualDrive.ino | 30 + .../examples/ManualDrive/apLoop.ino | 123 ++ .../examples/ManualDrive/imuLoop.ino | 47 + .../examples/ManualDrive/motorLoop.ino | 170 ++ .../examples/MasterSlave/MasterSlave.ino | 30 + .../examples/MasterSlave/apLoop.ino | 216 +++ .../examples/MasterSlave/imuLoop.ino | 47 + .../examples/MasterSlave/motorLoop.ino | 169 ++ .../ZumoCC3200/examples/Multicast/MCP.cpp | 25 + .../ZumoCC3200/examples/Multicast/MCP.h | 8 + .../examples/Multicast/Multicast.ino | 22 + .../ZumoCC3200/examples/Multicast/apLoop.ino | 104 ++ .../ZumoCC3200/examples/Multicast/imuLoop.ino | 47 + .../examples/Multicast/motorLoop.ino | 170 ++ .../examples/SplineDrive/MotionPlanner.cpp | 301 ++++ .../examples/SplineDrive/MotionPlanner.h | 49 + .../examples/SplineDrive/SplineDrive.ino | 31 + .../examples/SplineDrive/apLoop.ino | 159 ++ .../examples/SplineDrive/imuLoop.ino | 68 + .../examples/SplineDrive/motorLoop.ino | 70 + .../examples/UDPBroadcast/UDPBroadcast.ino | 28 + .../examples/UDPBroadcast/apLoop.ino | 137 ++ .../examples/UDPBroadcast/imuLoop.ino | 47 + .../examples/UDPBroadcast/motorLoop.ino | 173 ++ .../examples/UDPExample/UDPExample.ino | 24 + .../ZumoCC3200/examples/UDPExample/apLoop.ino | 99 ++ .../examples/UDPExample/imuLoop.ino | 47 + .../examples/UDPExample/motorLoop.ino | 170 ++ .../libraries/ZumoCC3200/utility/Balancer.cpp | 150 ++ .../libraries/ZumoCC3200/utility/Balancer.h | 68 + .../libraries/ZumoCC3200/utility/Command.h | 53 + .../ZumoCC3200/utility/CommandManager.cpp | 79 + .../ZumoCC3200/utility/CommandManager.h | 19 + .../libraries/ZumoCC3200/utility/DCM.cpp | 660 ++++++++ .../libraries/ZumoCC3200/utility/DCM.h | 89 + .../ZumoCC3200/utility/DriveLineCommand.cpp | 116 ++ .../ZumoCC3200/utility/DriveLineCommand.h | 61 + .../ZumoCC3200/utility/IMUManager.cpp | 455 ++++++ .../libraries/ZumoCC3200/utility/IMUManager.h | 149 ++ .../libraries/ZumoCC3200/utility/L3G.cpp | 288 ++++ .../libraries/ZumoCC3200/utility/L3G.h | 195 +++ .../libraries/ZumoCC3200/utility/LSM303.cpp | 479 ++++++ .../libraries/ZumoCC3200/utility/LSM303.h | 356 ++++ .../ZumoCC3200/utility/PIDController.cpp | 92 ++ .../ZumoCC3200/utility/PIDController.h | 70 + .../ZumoCC3200/utility/Pushbutton.cpp | 173 ++ .../libraries/ZumoCC3200/utility/Pushbutton.h | 70 + .../ZumoCC3200/utility/TurnAngleCommand.cpp | 125 ++ .../ZumoCC3200/utility/TurnAngleCommand.h | 63 + .../ZumoCC3200/utility/Utilities.cpp | 148 ++ .../libraries/ZumoCC3200/utility/Utilities.h | 66 + .../ZumoCC3200/utility/WaitCommand.cpp | 73 + .../ZumoCC3200/utility/WaitCommand.h | 54 + .../ZumoCC3200/utility/ZumoMotors.cpp | 138 ++ .../libraries/ZumoCC3200/utility/ZumoMotors.h | 43 + .../libraries/ZumoCC3200/utility/spline.h | 404 +++++ .../AssistedDrive/AssistedDrive.pde | 704 ++++++++ src/Processing/AssistedDrive/GraphClass.pde | 371 +++++ .../AttitudeDisplay/AttitudeDisplay.pde | 208 +++ src/Processing/Balancing/Balancing.pde | 373 +++++ src/Processing/Balancing/GraphClass.pde | 371 +++++ src/Processing/ManualDrive/zecho/zecho.pde | 110 ++ .../ManualDrive/zgraph/GraphClass.pde | 371 +++++ src/Processing/ManualDrive/zgraph/zgraph.pde | 274 ++++ src/Processing/MasterSlave/GraphClass.pde | 371 +++++ src/Processing/MasterSlave/MasterSlave.pde | 280 ++++ src/Processing/SplineDrive/SplineDrive.pde | 174 ++ src/Processing/UDPBroadcast/GraphClass.pde | 371 +++++ src/Processing/UDPBroadcast/UDPBroadcast.pde | 570 +++++++ src/Processing/UDPExample/GraphClass.pde | 371 +++++ src/Processing/UDPExample/UDPExample.pde | 277 ++++ 96 files changed, 17370 insertions(+) create mode 100644 .gitignore create mode 100644 README.html create mode 100644 etc/Markdown.pl create mode 100644 etc/build.svg create mode 100644 etc/github-markdown.css create mode 100644 etc/md2html.ksh create mode 100644 license.html create mode 100644 makefile create mode 100644 src/Energia/libraries/ZumoCC3200/ZumoCC3200.h create mode 100644 src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/AssistedDrive.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/MotorControlLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/SensorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/WifiLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/AttitudeDisplay.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/apLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/imuLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/motorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Balancing/Balancing.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Balancing/apLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Balancing/imuLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Balancing/motorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/CommandExample/CommandExample.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/CommandExample/MotorControlLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/CommandExample/SensorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/CommandExample/WifiLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/ManualDrive/ManualDrive.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/ManualDrive/apLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/ManualDrive/imuLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/ManualDrive/motorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/MasterSlave/MasterSlave.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/MasterSlave/apLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/MasterSlave/imuLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/MasterSlave/motorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Multicast/MCP.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Multicast/MCP.h create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Multicast/Multicast.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Multicast/apLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Multicast/imuLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/Multicast/motorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/SplineDrive/MotionPlanner.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/examples/SplineDrive/MotionPlanner.h create mode 100644 src/Energia/libraries/ZumoCC3200/examples/SplineDrive/SplineDrive.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/SplineDrive/apLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/SplineDrive/imuLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/SplineDrive/motorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/UDPBroadcast.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/apLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/imuLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/motorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/UDPExample/UDPExample.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/UDPExample/apLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/UDPExample/imuLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/examples/UDPExample/motorLoop.ino create mode 100644 src/Energia/libraries/ZumoCC3200/utility/Balancer.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/Balancer.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/Command.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/CommandManager.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/CommandManager.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/DCM.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/DCM.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/DriveLineCommand.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/DriveLineCommand.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/IMUManager.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/IMUManager.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/L3G.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/L3G.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/LSM303.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/LSM303.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/PIDController.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/PIDController.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/Pushbutton.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/Pushbutton.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/TurnAngleCommand.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/TurnAngleCommand.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/Utilities.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/Utilities.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/WaitCommand.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/WaitCommand.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/ZumoMotors.cpp create mode 100644 src/Energia/libraries/ZumoCC3200/utility/ZumoMotors.h create mode 100644 src/Energia/libraries/ZumoCC3200/utility/spline.h create mode 100644 src/Processing/AssistedDrive/AssistedDrive.pde create mode 100644 src/Processing/AssistedDrive/GraphClass.pde create mode 100644 src/Processing/AttitudeDisplay/AttitudeDisplay.pde create mode 100644 src/Processing/Balancing/Balancing.pde create mode 100644 src/Processing/Balancing/GraphClass.pde create mode 100644 src/Processing/ManualDrive/zecho/zecho.pde create mode 100644 src/Processing/ManualDrive/zgraph/GraphClass.pde create mode 100644 src/Processing/ManualDrive/zgraph/zgraph.pde create mode 100644 src/Processing/MasterSlave/GraphClass.pde create mode 100644 src/Processing/MasterSlave/MasterSlave.pde create mode 100644 src/Processing/SplineDrive/SplineDrive.pde create mode 100644 src/Processing/UDPBroadcast/GraphClass.pde create mode 100644 src/Processing/UDPBroadcast/UDPBroadcast.pde create mode 100644 src/Processing/UDPExample/GraphClass.pde create mode 100644 src/Processing/UDPExample/UDPExample.pde diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7da50c9 --- /dev/null +++ b/.gitignore @@ -0,0 +1,71 @@ +## Build artifacts in the tree root directory +/Make.log +/makelog +/exports/ +/imports/ +/.tools-* +/.gnu-* +/.exports +/.imports +/.imports.xml +/.reltree +/.generated_files +/bin/ + +## emacs backup files +.#* +.,* +*~ + +## RTSC package generated files & build goals +package.mak +package/ +java/ +.xdcenv.mak +.javadoc +doc-files +.dlls +.docs +.executables +.interfaces +.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 +*.xv7A +*.vcproj +*.xm4fg +*.xem4f +*.xm4g +*.xem4 +*.xem3 +*.xm3g + +*.exe +*.bin +*.out +*.obj +*.lib + +## CCS/Eclipse/RTSC generated project files +.config/ +.settings/ +.xdchelp +.launches/ +Debug/ diff --git a/README.html b/README.html new file mode 100644 index 0000000..8d9e8fc --- /dev/null +++ b/README.html @@ -0,0 +1,76 @@ + + + + Zumo CC3200 README.md + + + +
+

download

+ +

Zumo CC3200

+ +

The Zumo CC3200 project was created to enable makers to easily create +applications that leverage the power of a TI CC3200 connected to a +motorized hardware platform equipped with Inertial Measurement (IMU) +sensors.

+ +

This repository provides an Energia library, ZumoCC3200, that provides core +functions that make it possible to create applications with very little code +and without requiring in-depth knowledge of the CC3200 or the specific IMU +sensors used to enable closed-loop motor control. As with most libraries, +ZumoCC3200 includes numerous examples that serve as starting points for +new projects.

+ +

Each of the ZumoCC3200 examples consists of two programs: an Energia program +that runs on the Zumo CC3200 robot and a Processing +sketch that runs on your laptop. These two programs communicate with one +another via WiFi, enabling you to view telemetry data and control the bot +from your laptop.

+ +

The ZumoCC3200 Library

+ +

To use the ZumoCC3200 library,

+ + + +

Like most Energia/Arduino libraries, the ZumoCC3200 library includes serveral +examples that serve as simple starting points for your project.

+ +

Processing Sketches

+ +

The Processing sketches that communicate with the ZumoCC3200 bot are also +included in this repo. Note that some of these sketches +rely on freely available Contributed libraries that must be installed.

+ +

Prerequisite libraries and installation instructions for these sketches is +here.

+ +

License

+ +

All of the files in this repo are Open Source and most are licensed under +either a BSD 3-clause or MIT license. However, one file does come with a +GPL-2.0 license.

+ +

Complete license details are provided here.

+ +

Contributing

+ +

Zumo CC3200 is a collaborative project originally created as a part of the TI Santa Barbara 2015 Summer Intern program by Adam Dai and Tony Oliverio, and you are invited to help.

+ +

In-depth details about contributing code, bug fixes, and documentation are +forthcoming.

+
+ diff --git a/etc/Markdown.pl b/etc/Markdown.pl new file mode 100644 index 0000000..e4c8469 --- /dev/null +++ b/etc/Markdown.pl @@ -0,0 +1,1450 @@ +#!/usr/bin/perl + +# +# Markdown -- A text-to-HTML conversion tool for web writers +# +# Copyright (c) 2004 John Gruber +# +# + + +package Markdown; +require 5.006_000; +use strict; +use warnings; + +use Digest::MD5 qw(md5_hex); +use vars qw($VERSION); +$VERSION = '1.0.1'; +# Tue 14 Dec 2004 + +## Disabled; causes problems under Perl 5.6.1: +# use utf8; +# binmode( STDOUT, ":utf8" ); # c.f.: http://acis.openlib.org/dev/perl-unicode-struggle.html + + +# +# Global default settings: +# +my $g_empty_element_suffix = " />"; # Change to ">" for HTML output +my $g_tab_width = 4; + + +# +# Globals: +# + +# Regex to match balanced [brackets]. See Friedl's +# "Mastering Regular Expressions", 2nd Ed., pp. 328-331. +my $g_nested_brackets; +$g_nested_brackets = qr{ + (?> # Atomic matching + [^\[\]]+ # Anything other than brackets + | + \[ + (??{ $g_nested_brackets }) # Recursive set of nested brackets + \] + )* +}x; + + +# Table of hash values for escaped characters: +my %g_escape_table; +foreach my $char (split //, '\\`*_{}[]()>#+-.!') { + $g_escape_table{$char} = md5_hex($char); +} + + +# Global hashes, used by various utility routines +my %g_urls; +my %g_titles; +my %g_html_blocks; + +# Used to track when we're inside an ordered or unordered list +# (see _ProcessListItems() for details): +my $g_list_level = 0; + + +#### Blosxom plug-in interface ########################################## + +# Set $g_blosxom_use_meta to 1 to use Blosxom's meta plug-in to determine +# which posts Markdown should process, using a "meta-markup: markdown" +# header. If it's set to 0 (the default), Markdown will process all +# entries. +my $g_blosxom_use_meta = 0; + +sub start { 1; } +sub story { + my($pkg, $path, $filename, $story_ref, $title_ref, $body_ref) = @_; + + if ( (! $g_blosxom_use_meta) or + (defined($meta::markup) and ($meta::markup =~ /^\s*markdown\s*$/i)) + ){ + $$body_ref = Markdown($$body_ref); + } + 1; +} + + +#### Movable Type plug-in interface ##################################### +eval {require MT}; # Test to see if we're running in MT. +unless ($@) { + require MT; + import MT; + require MT::Template::Context; + import MT::Template::Context; + + eval {require MT::Plugin}; # Test to see if we're running >= MT 3.0. + unless ($@) { + require MT::Plugin; + import MT::Plugin; + my $plugin = new MT::Plugin({ + name => "Markdown", + description => "A plain-text-to-HTML formatting plugin. (Version: $VERSION)", + doc_link => 'http://daringfireball.net/projects/markdown/' + }); + MT->add_plugin( $plugin ); + } + + MT::Template::Context->add_container_tag(MarkdownOptions => sub { + my $ctx = shift; + my $args = shift; + my $builder = $ctx->stash('builder'); + my $tokens = $ctx->stash('tokens'); + + if (defined ($args->{'output'}) ) { + $ctx->stash('markdown_output', lc $args->{'output'}); + } + + defined (my $str = $builder->build($ctx, $tokens) ) + or return $ctx->error($builder->errstr); + $str; # return value + }); + + MT->add_text_filter('markdown' => { + label => 'Markdown', + docs => 'http://daringfireball.net/projects/markdown/', + on_format => sub { + my $text = shift; + my $ctx = shift; + my $raw = 0; + if (defined $ctx) { + my $output = $ctx->stash('markdown_output'); + if (defined $output && $output =~ m/^html/i) { + $g_empty_element_suffix = ">"; + $ctx->stash('markdown_output', ''); + } + elsif (defined $output && $output eq 'raw') { + $raw = 1; + $ctx->stash('markdown_output', ''); + } + else { + $raw = 0; + $g_empty_element_suffix = " />"; + } + } + $text = $raw ? $text : Markdown($text); + $text; + }, + }); + + # If SmartyPants is loaded, add a combo Markdown/SmartyPants text filter: + my $smartypants; + + { + no warnings "once"; + $smartypants = $MT::Template::Context::Global_filters{'smarty_pants'}; + } + + if ($smartypants) { + MT->add_text_filter('markdown_with_smartypants' => { + label => 'Markdown With SmartyPants', + docs => 'http://daringfireball.net/projects/markdown/', + on_format => sub { + my $text = shift; + my $ctx = shift; + if (defined $ctx) { + my $output = $ctx->stash('markdown_output'); + if (defined $output && $output eq 'html') { + $g_empty_element_suffix = ">"; + } + else { + $g_empty_element_suffix = " />"; + } + } + $text = Markdown($text); + $text = $smartypants->($text, '1'); + }, + }); + } +} +else { +#### BBEdit/command-line text filter interface ########################## +# Needs to be hidden from MT (and Blosxom when running in static mode). + + # We're only using $blosxom::version once; tell Perl not to warn us: + no warnings 'once'; + unless ( defined($blosxom::version) ) { + use warnings; + + #### Check for command-line switches: ################# + my %cli_opts; + use Getopt::Long; + Getopt::Long::Configure('pass_through'); + GetOptions(\%cli_opts, + 'version', + 'shortversion', + 'html4tags', + ); + if ($cli_opts{'version'}) { # Version info + print "\nThis is Markdown, version $VERSION.\n"; + print "Copyright 2004 John Gruber\n"; + print "http://daringfireball.net/projects/markdown/\n\n"; + exit 0; + } + if ($cli_opts{'shortversion'}) { # Just the version number string. + print $VERSION; + exit 0; + } + if ($cli_opts{'html4tags'}) { # Use HTML tag style instead of XHTML + $g_empty_element_suffix = ">"; + } + + + #### Process incoming text: ########################### + my $text; + { + local $/; # Slurp the whole file + $text = <>; + } + print Markdown($text); + } +} + + + +sub Markdown { +# +# Main function. The order in which other subs are called here is +# essential. Link and image substitutions need to happen before +# _EscapeSpecialChars(), so that any *'s or _'s in the +# and tags get encoded. +# + my $text = shift; + + # Clear the global hashes. If we don't clear these, you get conflicts + # from other articles when generating a page which contains more than + # one article (e.g. an index page that shows the N most recent + # articles): + %g_urls = (); + %g_titles = (); + %g_html_blocks = (); + + + # Standardize line endings: + $text =~ s{\r\n}{\n}g; # DOS to Unix + $text =~ s{\r}{\n}g; # Mac to Unix + + # Make sure $text ends with a couple of newlines: + $text .= "\n\n"; + + # Convert all tabs to spaces. + $text = _Detab($text); + + # Strip any lines consisting only of spaces and tabs. + # This makes subsequent regexen easier to write, because we can + # match consecutive blank lines with /\n+/ instead of something + # contorted like /[ \t]*\n+/ . + $text =~ s/^[ \t]+$//mg; + + # Turn block-level HTML blocks into hash entries + $text = _HashHTMLBlocks($text); + + # Strip link definitions, store in hashes. + $text = _StripLinkDefinitions($text); + + $text = _RunBlockGamut($text); + + $text = _UnescapeSpecialChars($text); + + return $text . "\n"; +} + + +sub _StripLinkDefinitions { +# +# Strips link definitions from text, stores the URLs and titles in +# hash references. +# + my $text = shift; + my $less_than_tab = $g_tab_width - 1; + + # Link defs are in the form: ^[id]: url "optional title" + while ($text =~ s{ + ^[ ]{0,$less_than_tab}\[(.+)\]: # id = $1 + [ \t]* + \n? # maybe *one* newline + [ \t]* + ? # url = $2 + [ \t]* + \n? # maybe one newline + [ \t]* + (?: + (?<=\s) # lookbehind for whitespace + ["(] + (.+?) # title = $3 + [")] + [ \t]* + )? # title is optional + (?:\n+|\Z) + } + {}mx) { + $g_urls{lc $1} = _EncodeAmpsAndAngles( $2 ); # Link IDs are case-insensitive + if ($3) { + $g_titles{lc $1} = $3; + $g_titles{lc $1} =~ s/"/"/g; + } + } + + return $text; +} + + +sub _HashHTMLBlocks { + my $text = shift; + my $less_than_tab = $g_tab_width - 1; + + # Hashify HTML blocks: + # We only want to do this for block-level HTML tags, such as headers, + # lists, and tables. That's because we still want to wrap

s around + # "paragraphs" that are wrapped in non-block-level tags, such as anchors, + # phrase emphasis, and spans. The list of tags we're looking for is + # hard-coded: + my $block_tags_a = qr/p|div|h[1-6]|blockquote|pre|table|dl|ol|ul|script|noscript|form|fieldset|iframe|math|ins|del/; + my $block_tags_b = qr/p|div|h[1-6]|blockquote|pre|table|dl|ol|ul|script|noscript|form|fieldset|iframe|math/; + + # First, look for nested blocks, e.g.: + #

+ #
+ # tags for inner block must be indented. + #
+ #
+ # + # The outermost tags must start at the left margin for this to match, and + # the inner nested divs must be indented. + # We need to do this before the next, more liberal match, because the next + # match will start at the first `
` and stop at the first `
`. + $text =~ s{ + ( # save in $1 + ^ # start of line (with /m) + <($block_tags_a) # start tag = $2 + \b # word break + (.*\n)*? # any number of lines, minimally matching + # the matching end tag + [ \t]* # trailing spaces/tabs + (?=\n+|\Z) # followed by a newline or end of document + ) + }{ + my $key = md5_hex($1); + $g_html_blocks{$key} = $1; + "\n\n" . $key . "\n\n"; + }egmx; + + + # + # Now match more liberally, simply from `\n` to `\n` + # + $text =~ s{ + ( # save in $1 + ^ # start of line (with /m) + <($block_tags_b) # start tag = $2 + \b # word break + (.*\n)*? # any number of lines, minimally matching + .* # the matching end tag + [ \t]* # trailing spaces/tabs + (?=\n+|\Z) # followed by a newline or end of document + ) + }{ + my $key = md5_hex($1); + $g_html_blocks{$key} = $1; + "\n\n" . $key . "\n\n"; + }egmx; + # Special case just for
. It was easier to make a special case than + # to make the other regex more complicated. + $text =~ s{ + (?: + (?<=\n\n) # Starting after a blank line + | # or + \A\n? # the beginning of the doc + ) + ( # save in $1 + [ ]{0,$less_than_tab} + <(hr) # start tag = $2 + \b # word break + ([^<>])*? # + /?> # the matching end tag + [ \t]* + (?=\n{2,}|\Z) # followed by a blank line or end of document + ) + }{ + my $key = md5_hex($1); + $g_html_blocks{$key} = $1; + "\n\n" . $key . "\n\n"; + }egx; + + # Special case for standalone HTML comments: + $text =~ s{ + (?: + (?<=\n\n) # Starting after a blank line + | # or + \A\n? # the beginning of the doc + ) + ( # save in $1 + [ ]{0,$less_than_tab} + (?s: + + ) + [ \t]* + (?=\n{2,}|\Z) # followed by a blank line or end of document + ) + }{ + my $key = md5_hex($1); + $g_html_blocks{$key} = $1; + "\n\n" . $key . "\n\n"; + }egx; + + + return $text; +} + + +sub _RunBlockGamut { +# +# These are all the transformations that form block-level +# tags like paragraphs, headers, and list items. +# + my $text = shift; + + $text = _DoHeaders($text); + + # Do Horizontal Rules: + $text =~ s{^[ ]{0,2}([ ]?\*[ ]?){3,}[ \t]*$}{\n tags around block-level tags. + $text = _HashHTMLBlocks($text); + + $text = _FormParagraphs($text); + + return $text; +} + + +sub _RunSpanGamut { +# +# These are all the transformations that occur *within* block-level +# tags like paragraphs, headers, and list items. +# + my $text = shift; + + $text = _DoCodeSpans($text); + + $text = _EscapeSpecialChars($text); + + # Process anchor and image tags. Images must come first, + # because ![foo][f] looks like an anchor. + $text = _DoImages($text); + $text = _DoAnchors($text); + + # Make links out of things like `` + # Must come after _DoAnchors(), because you can use < and > + # delimiters in inline links like [this](). + $text = _DoAutoLinks($text); + + $text = _EncodeAmpsAndAngles($text); + + $text = _DoItalicsAndBold($text); + + # Do hard breaks: + $text =~ s/ {2,}\n/ or tags. +# my $tags_to_skip = qr!<(/?)(?:pre|code|kbd|script|math)[\s>]!; + + foreach my $cur_token (@$tokens) { + if ($cur_token->[0] eq "tag") { + # Within tags, encode * and _ so they don't conflict + # with their use in Markdown for italics and strong. + # We're replacing each such character with its + # corresponding MD5 checksum value; this is likely + # overkill, but it should prevent us from colliding + # with the escape values by accident. + $cur_token->[1] =~ s! \* !$g_escape_table{'*'}!gx; + $cur_token->[1] =~ s! _ !$g_escape_table{'_'}!gx; + $text .= $cur_token->[1]; + } else { + my $t = $cur_token->[1]; + $t = _EncodeBackslashEscapes($t); + $text .= $t; + } + } + return $text; +} + + +sub _DoAnchors { +# +# Turn Markdown link shortcuts into XHTML tags. +# + my $text = shift; + + # + # First, handle reference-style links: [link text] [id] + # + $text =~ s{ + ( # wrap whole match in $1 + \[ + ($g_nested_brackets) # link text = $2 + \] + + [ ]? # one optional space + (?:\n[ ]*)? # one optional newline followed by spaces + + \[ + (.*?) # id = $3 + \] + ) + }{ + my $result; + my $whole_match = $1; + my $link_text = $2; + my $link_id = lc $3; + + if ($link_id eq "") { + $link_id = lc $link_text; # for shortcut links like [this][]. + } + + if (defined $g_urls{$link_id}) { + my $url = $g_urls{$link_id}; + $url =~ s! \* !$g_escape_table{'*'}!gx; # We've got to encode these to avoid + $url =~ s! _ !$g_escape_table{'_'}!gx; # conflicting with italics/bold. + $result = "? # href = $3 + [ \t]* + ( # $4 + (['"]) # quote char = $5 + (.*?) # Title = $6 + \5 # matching quote + )? # title is optional + \) + ) + }{ + my $result; + my $whole_match = $1; + my $link_text = $2; + my $url = $3; + my $title = $6; + + $url =~ s! \* !$g_escape_table{'*'}!gx; # We've got to encode these to avoid + $url =~ s! _ !$g_escape_table{'_'}!gx; # conflicting with italics/bold. + $result = " tags. +# + my $text = shift; + + # + # First, handle reference-style labeled images: ![alt text][id] + # + $text =~ s{ + ( # wrap whole match in $1 + !\[ + (.*?) # alt text = $2 + \] + + [ ]? # one optional space + (?:\n[ ]*)? # one optional newline followed by spaces + + \[ + (.*?) # id = $3 + \] + + ) + }{ + my $result; + my $whole_match = $1; + my $alt_text = $2; + my $link_id = lc $3; + + if ($link_id eq "") { + $link_id = lc $alt_text; # for shortcut links like ![this][]. + } + + $alt_text =~ s/"/"/g; + if (defined $g_urls{$link_id}) { + my $url = $g_urls{$link_id}; + $url =~ s! \* !$g_escape_table{'*'}!gx; # We've got to encode these to avoid + $url =~ s! _ !$g_escape_table{'_'}!gx; # conflicting with italics/bold. + $result = "\"$alt_text\"";? # src url = $3 + [ \t]* + ( # $4 + (['"]) # quote char = $5 + (.*?) # title = $6 + \5 # matching quote + [ \t]* + )? # title is optional + \) + ) + }{ + my $result; + my $whole_match = $1; + my $alt_text = $2; + my $url = $3; + my $title = ''; + if (defined($6)) { + $title = $6; + } + + $alt_text =~ s/"/"/g; + $title =~ s/"/"/g; + $url =~ s! \* !$g_escape_table{'*'}!gx; # We've got to encode these to avoid + $url =~ s! _ !$g_escape_table{'_'}!gx; # conflicting with italics/bold. + $result = "\"$alt_text\"";" . _RunSpanGamut($1) . "\n\n"; + }egmx; + + $text =~ s{ ^(.+)[ \t]*\n-+[ \t]*\n+ }{ + "

" . _RunSpanGamut($1) . "

\n\n"; + }egmx; + + + # atx-style headers: + # # Header 1 + # ## Header 2 + # ## Header 2 with closing hashes ## + # ... + # ###### Header 6 + # + $text =~ s{ + ^(\#{1,6}) # $1 = string of #'s + [ \t]* + (.+?) # $2 = Header text + [ \t]* + \#* # optional closing #'s (not counted) + \n+ + }{ + my $h_level = length($1); + "" . _RunSpanGamut($2) . "\n\n"; + }egmx; + + return $text; +} + + +sub _DoLists { +# +# Form HTML ordered (numbered) and unordered (bulleted) lists. +# + my $text = shift; + my $less_than_tab = $g_tab_width - 1; + + # Re-usable patterns to match list item bullets and number markers: + my $marker_ul = qr/[*+-]/; + my $marker_ol = qr/\d+[.]/; + my $marker_any = qr/(?:$marker_ul|$marker_ol)/; + + # Re-usable pattern to match any entirel ul or ol list: + my $whole_list = qr{ + ( # $1 = whole list + ( # $2 + [ ]{0,$less_than_tab} + (${marker_any}) # $3 = first list item marker + [ \t]+ + ) + (?s:.+?) + ( # $4 + \z + | + \n{2,} + (?=\S) + (?! # Negative lookahead for another list item marker + [ \t]* + ${marker_any}[ \t]+ + ) + ) + ) + }mx; + + # We use a different prefix before nested lists than top-level lists. + # See extended comment in _ProcessListItems(). + # + # Note: There's a bit of duplication here. My original implementation + # created a scalar regex pattern as the conditional result of the test on + # $g_list_level, and then only ran the $text =~ s{...}{...}egmx + # substitution once, using the scalar as the pattern. This worked, + # everywhere except when running under MT on my hosting account at Pair + # Networks. There, this caused all rebuilds to be killed by the reaper (or + # perhaps they crashed, but that seems incredibly unlikely given that the + # same script on the same server ran fine *except* under MT. I've spent + # more time trying to figure out why this is happening than I'd like to + # admit. My only guess, backed up by the fact that this workaround works, + # is that Perl optimizes the substition when it can figure out that the + # pattern will never change, and when this optimization isn't on, we run + # afoul of the reaper. Thus, the slightly redundant code to that uses two + # static s/// patterns rather than one conditional pattern. + + if ($g_list_level) { + $text =~ s{ + ^ + $whole_list + }{ + my $list = $1; + my $list_type = ($3 =~ m/$marker_ul/) ? "ul" : "ol"; + # Turn double returns into triple returns, so that we can make a + # paragraph for the last item in a list, if necessary: + $list =~ s/\n{2,}/\n\n\n/g; + my $result = _ProcessListItems($list, $marker_any); + $result = "<$list_type>\n" . $result . "\n"; + $result; + }egmx; + } + else { + $text =~ s{ + (?:(?<=\n\n)|\A\n?) + $whole_list + }{ + my $list = $1; + my $list_type = ($3 =~ m/$marker_ul/) ? "ul" : "ol"; + # Turn double returns into triple returns, so that we can make a + # paragraph for the last item in a list, if necessary: + $list =~ s/\n{2,}/\n\n\n/g; + my $result = _ProcessListItems($list, $marker_any); + $result = "<$list_type>\n" . $result . "\n"; + $result; + }egmx; + } + + + return $text; +} + + +sub _ProcessListItems { +# +# Process the contents of a single ordered or unordered list, splitting it +# into individual list items. +# + + my $list_str = shift; + my $marker_any = shift; + + + # The $g_list_level global keeps track of when we're inside a list. + # Each time we enter a list, we increment it; when we leave a list, + # we decrement. If it's zero, we're not in a list anymore. + # + # We do this because when we're not inside a list, we want to treat + # something like this: + # + # I recommend upgrading to version + # 8. Oops, now this line is treated + # as a sub-list. + # + # As a single paragraph, despite the fact that the second line starts + # with a digit-period-space sequence. + # + # Whereas when we're inside a list (or sub-list), that line will be + # treated as the start of a sub-list. What a kludge, huh? This is + # an aspect of Markdown's syntax that's hard to parse perfectly + # without resorting to mind-reading. Perhaps the solution is to + # change the syntax rules such that sub-lists must start with a + # starting cardinal number; e.g. "1." or "a.". + + $g_list_level++; + + # trim trailing blank lines: + $list_str =~ s/\n{2,}\z/\n/; + + + $list_str =~ s{ + (\n)? # leading line = $1 + (^[ \t]*) # leading whitespace = $2 + ($marker_any) [ \t]+ # list marker = $3 + ((?s:.+?) # list item text = $4 + (\n{1,2})) + (?= \n* (\z | \2 ($marker_any) [ \t]+)) + }{ + my $item = $4; + my $leading_line = $1; + my $leading_space = $2; + + if ($leading_line or ($item =~ m/\n{2,}/)) { + $item = _RunBlockGamut(_Outdent($item)); + } + else { + # Recursion for sub-lists: + $item = _DoLists(_Outdent($item)); + chomp $item; + $item = _RunSpanGamut($item); + } + + "
  • " . $item . "
    • \n"; + }egmx; + + $g_list_level--; + return $list_str; +} + + + +sub _DoCodeBlocks { +# +# Process Markdown `
    ` blocks.
+#	
+
+	my $text = shift;
+
+	$text =~ s{
+			(?:\n\n|\A)
+			(	            # $1 = the code block -- one or more lines, starting with a space/tab
+			  (?:
+			    (?:[ ]{$g_tab_width} | \t)  # Lines must start with a tab or a tab-width of spaces
+			    .*\n+
+			  )+
+			)
+			((?=^[ ]{0,$g_tab_width}\S)|\Z)	# Lookahead for non-space at line-start, or end of doc
+		}{
+			my $codeblock = $1;
+			my $result; # return value
+
+			$codeblock = _EncodeCode(_Outdent($codeblock));
+			$codeblock = _Detab($codeblock);
+			$codeblock =~ s/\A\n+//; # trim leading newlines
+			$codeblock =~ s/\s+\z//; # trim trailing whitespace
+
+			$result = "\n\n
    " . $codeblock . "\n
    \n\n";
+
+			$result;
+		}egmx;
+
+	return $text;
+}
+
+
+sub _DoCodeSpans {
+#
+# 	*	Backtick quotes are used for  spans.
+# 
+# 	*	You can use multiple backticks as the delimiters if you want to
+# 		include literal backticks in the code span. So, this input:
+#     
+#         Just type ``foo `bar` baz`` at the prompt.
+#     
+#     	Will translate to:
+#     
+#         
    Just type foo `bar` baz at the prompt.
    
+#     
+#		There's no arbitrary limit to the number of backticks you
+#		can use as delimters. If you need three consecutive backticks
+#		in your code, use four for delimiters, etc.
+#
+#	*	You can use spaces to get literal backticks at the edges:
+#     
+#         ... type `` `bar` `` ...
+#     
+#     	Turns to:
+#     
+#         ... type `bar` ...
+#
+
+	my $text = shift;
+
+	$text =~ s@
+			(`+)		# $1 = Opening run of `
+			(.+?)		# $2 = The code block
+			(?$c    ";
+		@egsx;
+
+	return $text;
+}
+
+
+sub _EncodeCode {
+#
+# Encode/escape certain characters inside Markdown code runs.
+# The point is that in code, these characters are literals,
+# and lose their special Markdown meanings.
+#
+    local $_ = shift;
+
+	# Encode all ampersands; HTML entities are not
+	# entities within a Markdown code span.
+	s/&/&/g;
+
+	# Encode $'s, but only if we're running under Blosxom.
+	# (Blosxom interpolates Perl variables in article bodies.)
+	{
+		no warnings 'once';
+    	if (defined($blosxom::version)) {
+    		s/\$/$/g;	
+    	}
+    }
+
+
+	# Do the angle bracket song and dance:
+	s! <  !<!gx;
+	s! >  !>!gx;
+
+	# Now, escape characters that are magic in Markdown:
+	s! \* !$g_escape_table{'*'}!gx;
+	s! _  !$g_escape_table{'_'}!gx;
+	s! {  !$g_escape_table{'{'}!gx;
+	s! }  !$g_escape_table{'}'}!gx;
+	s! \[ !$g_escape_table{'['}!gx;
+	s! \] !$g_escape_table{']'}!gx;
+	s! \\ !$g_escape_table{'\\'}!gx;
+
+	return $_;
+}
+
+
+sub _DoItalicsAndBold {
+	my $text = shift;
+
+	#  must go first:
+	$text =~ s{ (\*\*|__) (?=\S) (.+?[*_]*) (?<=\S) \1 }
+		{$2}gsx;
+
+	$text =~ s{ (\*|_) (?=\S) (.+?) (?<=\S) \1 }
+		{$2}gsx;
+
+	return $text;
+}
+
+
+sub _DoBlockQuotes {
+	my $text = shift;
+
+	$text =~ s{
+		  (								# Wrap whole match in $1
+			(
+			  ^[ \t]*>[ \t]?			# '>' at the start of a line
+			    .+\n					# rest of the first line
+			  (.+\n)*					# subsequent consecutive lines
+			  \n*						# blanks
+			)+
+		  )
+		}{
+			my $bq = $1;
+			$bq =~ s/^[ \t]*>[ \t]?//gm;	# trim one level of quoting
+			$bq =~ s/^[ \t]+$//mg;			# trim whitespace-only lines
+			$bq = _RunBlockGamut($bq);		# recurse
+
+			$bq =~ s/^/  /g;
+			# These leading spaces screw with 
     content, so we need to fix that:
+			$bq =~ s{
+					(\s*
    .+?
    )
+				}{
+					my $pre = $1;
+					$pre =~ s/^  //mg;
+					$pre;
+				}egsx;
+
+			"
    \n$bq\n
    \n\n";
+		}egmx;
+
+
+	return $text;
+}
+
+
+sub _FormParagraphs {
+#
+#	Params:
+#		$text - string to process with html 
     tags
+#
+	my $text = shift;
+
+	# Strip leading and trailing lines:
+	$text =~ s/\A\n+//;
+	$text =~ s/\n+\z//;
+
+	my @grafs = split(/\n{2,}/, $text);
+
+	#
+	# Wrap 
     tags.
+	#
+	foreach (@grafs) {
+		unless (defined( $g_html_blocks{$_} )) {
+			$_ = _RunSpanGamut($_);
+			s/^([ \t]*)/
    /;
+			$_ .= "
    ";
+		}
+	}
+
+	#
+	# Unhashify HTML blocks
+	#
+	foreach (@grafs) {
+		if (defined( $g_html_blocks{$_} )) {
+			$_ = $g_html_blocks{$_};
+		}
+	}
+
+	return join "\n\n", @grafs;
+}
+
+
+sub _EncodeAmpsAndAngles {
+# Smart processing for ampersands and angle brackets that need to be encoded.
+
+	my $text = shift;
+
+	# Ampersand-encoding based entirely on Nat Irons's Amputator MT plugin:
+	#   http://bumppo.net/projects/amputator/
+ 	$text =~ s/&(?!#?[xX]?(?:[0-9a-fA-F]+|\w+);)/&/g;
+
+	# Encode naked <'s
+ 	$text =~ s{<(?![a-z/?\$!])}{<}gi;
+
+	return $text;
+}
+
+
+sub _EncodeBackslashEscapes {
+#
+#   Parameter:  String.
+#   Returns:    The string, with after processing the following backslash
+#               escape sequences.
+#
+    local $_ = shift;
+
+    s! \\\\  !$g_escape_table{'\\'}!gx;		# Must process escaped backslashes first.
+    s! \\`   !$g_escape_table{'`'}!gx;
+    s! \\\*  !$g_escape_table{'*'}!gx;
+    s! \\_   !$g_escape_table{'_'}!gx;
+    s! \\\{  !$g_escape_table{'{'}!gx;
+    s! \\\}  !$g_escape_table{'}'}!gx;
+    s! \\\[  !$g_escape_table{'['}!gx;
+    s! \\\]  !$g_escape_table{']'}!gx;
+    s! \\\(  !$g_escape_table{'('}!gx;
+    s! \\\)  !$g_escape_table{')'}!gx;
+    s! \\>   !$g_escape_table{'>'}!gx;
+    s! \\\#  !$g_escape_table{'#'}!gx;
+    s! \\\+  !$g_escape_table{'+'}!gx;
+    s! \\\-  !$g_escape_table{'-'}!gx;
+    s! \\\.  !$g_escape_table{'.'}!gx;
+    s{ \\!  }{$g_escape_table{'!'}}gx;
+
+    return $_;
+}
+
+
+sub _DoAutoLinks {
+	my $text = shift;
+
+	$text =~ s{<((https?|ftp):[^'">\s]+)>}{    $1}gi;
+
+	# Email addresses: 
+	$text =~ s{
+		<
+        (?:mailto:)?
+		(
+			[-.\w]+
+			\@
+			[-a-z0-9]+(\.[-a-z0-9]+)*\.[a-z]+
+		)
+		>
+	}{
+		_EncodeEmailAddress( _UnescapeSpecialChars($1) );
+	}egix;
+
+	return $text;
+}
+
+
+sub _EncodeEmailAddress {
+#
+#	Input: an email address, e.g. "foo@example.com"
+#
+#	Output: the email address as a mailto link, with each character
+#		of the address encoded as either a decimal or hex entity, in
+#		the hopes of foiling most address harvesting spam bots. E.g.:
+#
+#	  foo
+#       @example.com
+#
+#	Based on a filter by Matthew Wickline, posted to the BBEdit-Talk
+#	mailing list: 
+#
+
+	my $addr = shift;
+
+	srand;
+	my @encode = (
+		sub { '&#' .                 ord(shift)   . ';' },
+		sub { '&#x' . sprintf( "%X", ord(shift) ) . ';' },
+		sub {                            shift          },
+	);
+
+	$addr = "mailto:" . $addr;
+
+	$addr =~ s{(.)}{
+		my $char = $1;
+		if ( $char eq '@' ) {
+			# this *must* be encoded. I insist.
+			$char = $encode[int rand 1]->($char);
+		} elsif ( $char ne ':' ) {
+			# leave ':' alone (to spot mailto: later)
+			my $r = rand;
+			# roughly 10% raw, 45% hex, 45% dec
+			$char = (
+				$r > .9   ?  $encode[2]->($char)  :
+				$r < .45  ?  $encode[1]->($char)  :
+							 $encode[0]->($char)
+			);
+		}
+		$char;
+	}gex;
+
+	$addr = qq{$addr};
+	$addr =~ s{">.+?:}{">}; # strip the mailto: from the visible part
+
+	return $addr;
+}
+
+
+sub _UnescapeSpecialChars {
+#
+# Swap back in all the special characters we've hidden.
+#
+	my $text = shift;
+
+	while( my($char, $hash) = each(%g_escape_table) ) {
+		$text =~ s/$hash/$char/g;
+	}
+    return $text;
+}
+
+
+sub _TokenizeHTML {
+#
+#   Parameter:  String containing HTML markup.
+#   Returns:    Reference to an array of the tokens comprising the input
+#               string. Each token is either a tag (possibly with nested,
+#               tags contained therein, such as , or a
+#               run of text between tags. Each element of the array is a
+#               two-element array; the first is either 'tag' or 'text';
+#               the second is the actual value.
+#
+#
+#   Derived from the _tokenize() subroutine from Brad Choate's MTRegex plugin.
+#       
+#
+
+    my $str = shift;
+    my $pos = 0;
+    my $len = length $str;
+    my @tokens;
+
+    my $depth = 6;
+    my $nested_tags = join('|', ('(?:<[a-z/!$](?:[^<>]') x $depth) . (')*>)' x  $depth);
+    my $match = qr/(?s:  ) |  # comment
+                   (?s: <\? .*? \?> ) |              # processing instruction
+                   $nested_tags/ix;                   # nested tags
+
+    while ($str =~ m/($match)/g) {
+        my $whole_tag = $1;
+        my $sec_start = pos $str;
+        my $tag_start = $sec_start - length $whole_tag;
+        if ($pos < $tag_start) {
+            push @tokens, ['text', substr($str, $pos, $tag_start - $pos)];
+        }
+        push @tokens, ['tag', $whole_tag];
+        $pos = pos $str;
+    }
+    push @tokens, ['text', substr($str, $pos, $len - $pos)] if $pos < $len;
+    \@tokens;
+}
+
+
+sub _Outdent {
+#
+# Remove one level of line-leading tabs or spaces
+#
+	my $text = shift;
+
+	$text =~ s/^(\t|[ ]{1,$g_tab_width})//gm;
+	return $text;
+}
+
+
+sub _Detab {
+#
+# Cribbed from a post by Bart Lateur:
+# 
+#
+	my $text = shift;
+
+	$text =~ s{(.*?)\t}{$1.(' ' x ($g_tab_width - length($1) % $g_tab_width))}ge;
+	return $text;
+}
+
+
+1;
+
+__END__
+
+
+=pod
+
+=head1 NAME
+
+B
+
+
+=head1 SYNOPSIS
+
+B [ B<--html4tags> ] [ B<--version> ] [ B<-shortversion> ]
+    [ I ... ]
+
+
+=head1 DESCRIPTION
+
+Markdown is a text-to-HTML filter; it translates an easy-to-read /
+easy-to-write structured text format into HTML. Markdown's text format
+is most similar to that of plain text email, and supports features such
+as headers, *emphasis*, code blocks, blockquotes, and links.
+
+Markdown's syntax is designed not as a generic markup language, but
+specifically to serve as a front-end to (X)HTML. You can  use span-level
+HTML tags anywhere in a Markdown document, and you can use block level
+HTML tags (like 
     and  as well).
+
+For more information about Markdown's syntax, see:
+
+    http://daringfireball.net/projects/markdown/
+
+
+=head1 OPTIONS
+
+Use "--" to end switch parsing. For example, to open a file named "-z", use:
+
+	Markdown.pl -- -z
+
+=over 4
+
+
+=item B<--html4tags>
+
+Use HTML 4 style for empty element tags, e.g.:
+
+    
    
+
+instead of Markdown's default XHTML style tags, e.g.:
+
+    
    
+
+
+=item B<-v>, B<--version>
+
+Display Markdown's version number and copyright information.
+
+
+=item B<-s>, B<--shortversion>
+
+Display the short-form version number.
+
+
+=back
+
+
+
+=head1 BUGS
+
+To file bug reports or feature requests (other than topics listed in the
+Caveats section above) please send email to:
+
+    support@daringfireball.net
+
+Please include with your report: (1) the example input; (2) the output
+you expected; (3) the output Markdown actually produced.
+
+
+=head1 VERSION HISTORY
+
+See the readme file for detailed release notes for this version.
+
+1.0.1 - 14 Dec 2004
+
+1.0 - 28 Aug 2004
+
+
+=head1 AUTHOR
+
+    John Gruber
+    http://daringfireball.net
+
+    PHP port and other contributions by Michel Fortin
+    http://michelf.com
+
+
+=head1 COPYRIGHT AND LICENSE
+
+Copyright (c) 2003-2004 John Gruber   
+   
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+* Redistributions of source code must retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name "Markdown" nor the names of its contributors may
+  be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+This software is provided by the copyright holders and contributors "as
+is" and any express or implied warranties, including, but not limited
+to, the implied warranties of merchantability and fitness for a
+particular purpose are disclaimed. In no event shall the copyright owner
+or contributors be liable for any direct, indirect, incidental, special,
+exemplary, or consequential damages (including, but not limited to,
+procurement of substitute goods or services; loss of use, data, or
+profits; or business interruption) however caused and on any theory of
+liability, whether in contract, strict liability, or tort (including
+negligence or otherwise) arising in any way out of the use of this
+software, even if advised of the possibility of such damage.
+
+=cut
diff --git a/etc/build.svg b/etc/build.svg
new file mode 100644
index 0000000..5df26ce
--- /dev/null
+++ b/etc/build.svg
@@ -0,0 +1,34 @@
+
+
+
+    
+        
+        
+    
+
+    
+    
+    
+    
+
+    
+        
+            Download
+        
+        
+            Download
+        
+        
+            1.0.0.0
+        
+        
+            1.0.0.0
+        
+    
+
diff --git a/etc/github-markdown.css b/etc/github-markdown.css
new file mode 100644
index 0000000..1bd00d8
--- /dev/null
+++ b/etc/github-markdown.css
@@ -0,0 +1,652 @@
+@font-face {
+  font-family: octicons-anchor;
+  src: url(data:font/woff;charset=utf-8;base64,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) format('woff');
+}
+
+.markdown-body {
+  -webkit-text-size-adjust: 100%;
+  text-size-adjust: 100%;
+  color: #333;
+  overflow: hidden;
+  font-family: "Helvetica Neue", Helvetica, "Segoe UI", Arial, freesans, sans-serif;
+  font-size: 16px;
+  line-height: 1.6;
+  word-wrap: break-word;
+}
+
+.markdown-body a {
+  background-color: transparent;
+}
+
+.markdown-body a:active,
+.markdown-body a:hover {
+  outline: 0;
+}
+
+.markdown-body strong {
+  font-weight: bold;
+}
+
+.markdown-body h1 {
+  font-size: 2em;
+  margin: 0.67em 0;
+}
+
+.markdown-body img {
+  border: 0;
+}
+
+.markdown-body hr {
+  box-sizing: content-box;
+  height: 0;
+}
+
+.markdown-body pre {
+  overflow: auto;
+}
+
+.markdown-body code,
+.markdown-body kbd,
+.markdown-body pre {
+  font-family: monospace, monospace;
+  font-size: 1em;
+}
+
+.markdown-body input {
+  color: inherit;
+  font: inherit;
+  margin: 0;
+}
+
+.markdown-body html input[disabled] {
+  cursor: default;
+}
+
+.markdown-body input {
+  line-height: normal;
+}
+
+.markdown-body input[type="checkbox"] {
+  box-sizing: border-box;
+  padding: 0;
+}
+
+.markdown-body table {
+  border-collapse: collapse;
+  border-spacing: 0;
+}
+
+.markdown-body td,
+.markdown-body th {
+  padding: 0;
+}
+
+.markdown-body * {
+  box-sizing: border-box;
+}
+
+.markdown-body input {
+  font: 13px/1.4 Helvetica, arial, nimbussansl, liberationsans, freesans, clean, sans-serif, "Segoe UI Emoji", "Segoe UI Symbol";
+}
+
+.markdown-body a {
+  color: #4078c0;
+  text-decoration: none;
+}
+
+.markdown-body a:hover,
+.markdown-body a:active {
+  text-decoration: underline;
+}
+
+.markdown-body hr {
+  height: 0;
+  margin: 15px 0;
+  overflow: hidden;
+  background: transparent;
+  border: 0;
+  border-bottom: 1px solid #ddd;
+}
+
+.markdown-body hr:before {
+  display: table;
+  content: "";
+}
+
+.markdown-body hr:after {
+  display: table;
+  clear: both;
+  content: "";
+}
+
+.markdown-body h1,
+.markdown-body h2,
+.markdown-body h3,
+.markdown-body h4,
+.markdown-body h5,
+.markdown-body h6 {
+  margin-top: 15px;
+  margin-bottom: 15px;
+  line-height: 1.1;
+}
+
+.markdown-body h1 {
+  font-size: 30px;
+}
+
+.markdown-body h2 {
+  font-size: 21px;
+}
+
+.markdown-body h3 {
+  font-size: 16px;
+}
+
+.markdown-body h4 {
+  font-size: 14px;
+}
+
+.markdown-body h5 {
+  font-size: 12px;
+}
+
+.markdown-body h6 {
+  font-size: 11px;
+}
+
+.markdown-body blockquote {
+  margin: 0;
+}
+
+.markdown-body ul,
+.markdown-body ol {
+  padding: 0;
+  margin-top: 0;
+  margin-bottom: 0;
+}
+
+.markdown-body ol ol,
+.markdown-body ul ol {
+  list-style-type: lower-roman;
+}
+
+.markdown-body ul ul ol,
+.markdown-body ul ol ol,
+.markdown-body ol ul ol,
+.markdown-body ol ol ol {
+  list-style-type: lower-alpha;
+}
+
+.markdown-body dd {
+  margin-left: 0;
+}
+
+.markdown-body code {
+  font-family: Consolas, "Liberation Mono", Menlo, Courier, monospace;
+  font-size: 12px;
+}
+
+.markdown-body pre {
+  margin-top: 0;
+  margin-bottom: 0;
+  font: 12px Consolas, "Liberation Mono", Menlo, Courier, monospace;
+}
+
+.markdown-body .select::-ms-expand {
+  opacity: 0;
+}
+
+.markdown-body .octicon {
+  font: normal normal normal 16px/1 octicons-anchor;
+  display: inline-block;
+  text-decoration: none;
+  text-rendering: auto;
+  -webkit-font-smoothing: antialiased;
+  -moz-osx-font-smoothing: grayscale;
+  -webkit-user-select: none;
+  -moz-user-select: none;
+  -ms-user-select: none;
+  user-select: none;
+}
+
+.markdown-body .octicon-link:before {
+  content: '\f05c';
+}
+
+.markdown-body>*:first-child {
+  margin-top: 0 !important;
+}
+
+.markdown-body>*:last-child {
+  margin-bottom: 0 !important;
+}
+
+.markdown-body a:not([href]) {
+  color: inherit;
+  text-decoration: none;
+}
+
+.markdown-body .anchor {
+  position: absolute;
+  top: 0;
+  left: 0;
+  display: block;
+  padding-right: 6px;
+  padding-left: 30px;
+  margin-left: -30px;
+}
+
+.markdown-body .anchor:focus {
+  outline: none;
+}
+
+.markdown-body h1,
+.markdown-body h2,
+.markdown-body h3,
+.markdown-body h4,
+.markdown-body h5,
+.markdown-body h6 {
+  position: relative;
+  margin-top: 1em;
+  margin-bottom: 16px;
+  font-weight: bold;
+  line-height: 1.4;
+}
+
+.markdown-body h1 .octicon-link,
+.markdown-body h2 .octicon-link,
+.markdown-body h3 .octicon-link,
+.markdown-body h4 .octicon-link,
+.markdown-body h5 .octicon-link,
+.markdown-body h6 .octicon-link {
+  display: none;
+  color: #000;
+  vertical-align: middle;
+}
+
+.markdown-body h1:hover .anchor,
+.markdown-body h2:hover .anchor,
+.markdown-body h3:hover .anchor,
+.markdown-body h4:hover .anchor,
+.markdown-body h5:hover .anchor,
+.markdown-body h6:hover .anchor {
+  padding-left: 8px;
+  margin-left: -30px;
+  text-decoration: none;
+}
+
+.markdown-body h1:hover .anchor .octicon-link,
+.markdown-body h2:hover .anchor .octicon-link,
+.markdown-body h3:hover .anchor .octicon-link,
+.markdown-body h4:hover .anchor .octicon-link,
+.markdown-body h5:hover .anchor .octicon-link,
+.markdown-body h6:hover .anchor .octicon-link {
+  display: inline-block;
+}
+
+.markdown-body h1 {
+  padding-bottom: 0.3em;
+  font-size: 2.25em;
+  line-height: 1.2;
+  border-bottom: 1px solid #eee;
+}
+
+.markdown-body h1 .anchor {
+  line-height: 1;
+}
+
+.markdown-body h2 {
+  padding-bottom: 0.3em;
+  font-size: 1.75em;
+  line-height: 1.225;
+  border-bottom: 1px solid #eee;
+}
+
+.markdown-body h2 .anchor {
+  line-height: 1;
+}
+
+.markdown-body h3 {
+  font-size: 1.5em;
+  line-height: 1.43;
+}
+
+.markdown-body h3 .anchor {
+  line-height: 1.2;
+}
+
+.markdown-body h4 {
+  font-size: 1.25em;
+}
+
+.markdown-body h4 .anchor {
+  line-height: 1.2;
+}
+
+.markdown-body h5 {
+  font-size: 1em;
+}
+
+.markdown-body h5 .anchor {
+  line-height: 1.1;
+}
+
+.markdown-body h6 {
+  font-size: 1em;
+  color: #777;
+}
+
+.markdown-body h6 .anchor {
+  line-height: 1.1;
+}
+
+.markdown-body p,
+.markdown-body blockquote,
+.markdown-body ul,
+.markdown-body ol,
+.markdown-body dl,
+.markdown-body table,
+.markdown-body pre {
+  margin-top: 0;
+  margin-bottom: 16px;
+}
+
+.markdown-body hr {
+  height: 4px;
+  padding: 0;
+  margin: 16px 0;
+  background-color: #e7e7e7;
+  border: 0 none;
+}
+
+.markdown-body ul,
+.markdown-body ol {
+  padding-left: 2em;
+}
+
+.markdown-body ul ul,
+.markdown-body ul ol,
+.markdown-body ol ol,
+.markdown-body ol ul {
+  margin-top: 0;
+  margin-bottom: 0;
+}
+
+.markdown-body li>p {
+  margin-top: 16px;
+}
+
+.markdown-body dl {
+  padding: 0;
+}
+
+.markdown-body dl dt {
+  padding: 0;
+  margin-top: 16px;
+  font-size: 1em;
+  font-style: italic;
+  font-weight: bold;
+}
+
+.markdown-body dl dd {
+  padding: 0 16px;
+  margin-bottom: 16px;
+}
+
+.markdown-body blockquote {
+  padding: 0 15px;
+  color: #777;
+  border-left: 4px solid #ddd;
+}
+
+.markdown-body blockquote>:first-child {
+  margin-top: 0;
+}
+
+.markdown-body blockquote>:last-child {
+  margin-bottom: 0;
+}
+
+.markdown-body table {
+  display: block;
+  width: 100%;
+  overflow: auto;
+  word-break: normal;
+  word-break: keep-all;
+}
+
+.markdown-body table th {
+  font-weight: bold;
+}
+
+.markdown-body table th,
+.markdown-body table td {
+  padding: 6px 13px;
+  border: 1px solid #ddd;
+}
+
+.markdown-body table tr {
+  background-color: #fff;
+  border-top: 1px solid #ccc;
+}
+
+.markdown-body table tr:nth-child(2n) {
+  background-color: #f8f8f8;
+}
+
+.markdown-body img {
+  max-width: 100%;
+  box-sizing: border-box;
+}
+
+.markdown-body code {
+  padding: 0;
+  padding-top: 0.2em;
+  padding-bottom: 0.2em;
+  margin: 0;
+  font-size: 85%;
+  background-color: rgba(0,0,0,0.04);
+  border-radius: 3px;
+}
+
+.markdown-body code:before,
+.markdown-body code:after {
+  letter-spacing: -0.2em;
+  content: "\00a0";
+}
+
+.markdown-body pre>code {
+  padding: 0;
+  margin: 0;
+  font-size: 100%;
+  word-break: normal;
+  white-space: pre;
+  background: transparent;
+  border: 0;
+}
+
+.markdown-body .highlight {
+  margin-bottom: 16px;
+}
+
+.markdown-body .highlight pre,
+.markdown-body pre {
+  padding: 16px;
+  overflow: auto;
+  font-size: 85%;
+  line-height: 1.45;
+  background-color: #f7f7f7;
+  border-radius: 3px;
+}
+
+.markdown-body .highlight pre {
+  margin-bottom: 0;
+  word-break: normal;
+}
+
+.markdown-body pre {
+  word-wrap: normal;
+}
+
+.markdown-body pre code {
+  display: inline;
+  max-width: initial;
+  padding: 0;
+  margin: 0;
+  overflow: initial;
+  line-height: inherit;
+  word-wrap: normal;
+  background-color: transparent;
+  border: 0;
+}
+
+.markdown-body pre code:before,
+.markdown-body pre code:after {
+  content: normal;
+}
+
+.markdown-body kbd {
+  display: inline-block;
+  padding: 3px 5px;
+  font-size: 11px;
+  line-height: 10px;
+  color: #555;
+  vertical-align: middle;
+  background-color: #fcfcfc;
+  border: solid 1px #ccc;
+  border-bottom-color: #bbb;
+  border-radius: 3px;
+  box-shadow: inset 0 -1px 0 #bbb;
+}
+
+.markdown-body .pl-c {
+  color: #969896;
+}
+
+.markdown-body .pl-c1,
+.markdown-body .pl-s .pl-v {
+  color: #0086b3;
+}
+
+.markdown-body .pl-e,
+.markdown-body .pl-en {
+  color: #795da3;
+}
+
+.markdown-body .pl-s .pl-s1,
+.markdown-body .pl-smi {
+  color: #333;
+}
+
+.markdown-body .pl-ent {
+  color: #63a35c;
+}
+
+.markdown-body .pl-k {
+  color: #a71d5d;
+}
+
+.markdown-body .pl-pds,
+.markdown-body .pl-s,
+.markdown-body .pl-s .pl-pse .pl-s1,
+.markdown-body .pl-sr,
+.markdown-body .pl-sr .pl-cce,
+.markdown-body .pl-sr .pl-sra,
+.markdown-body .pl-sr .pl-sre {
+  color: #183691;
+}
+
+.markdown-body .pl-v {
+  color: #ed6a43;
+}
+
+.markdown-body .pl-id {
+  color: #b52a1d;
+}
+
+.markdown-body .pl-ii {
+  background-color: #b52a1d;
+  color: #f8f8f8;
+}
+
+.markdown-body .pl-sr .pl-cce {
+  color: #63a35c;
+  font-weight: bold;
+}
+
+.markdown-body .pl-ml {
+  color: #693a17;
+}
+
+.markdown-body .pl-mh,
+.markdown-body .pl-mh .pl-en,
+.markdown-body .pl-ms {
+  color: #1d3e81;
+  font-weight: bold;
+}
+
+.markdown-body .pl-mq {
+  color: #008080;
+}
+
+.markdown-body .pl-mi {
+  color: #333;
+  font-style: italic;
+}
+
+.markdown-body .pl-mb {
+  color: #333;
+  font-weight: bold;
+}
+
+.markdown-body .pl-md {
+  background-color: #ffecec;
+  color: #bd2c00;
+}
+
+.markdown-body .pl-mi1 {
+  background-color: #eaffea;
+  color: #55a532;
+}
+
+.markdown-body .pl-mdr {
+  color: #795da3;
+  font-weight: bold;
+}
+
+.markdown-body .pl-mo {
+  color: #1d3e81;
+}
+
+.markdown-body kbd {
+  display: inline-block;
+  padding: 3px 5px;
+  font: 11px Consolas, "Liberation Mono", Menlo, Courier, monospace;
+  line-height: 10px;
+  color: #555;
+  vertical-align: middle;
+  background-color: #fcfcfc;
+  border: solid 1px #ccc;
+  border-bottom-color: #bbb;
+  border-radius: 3px;
+  box-shadow: inset 0 -1px 0 #bbb;
+}
+
+.markdown-body .task-list-item {
+  list-style-type: none;
+}
+
+.markdown-body .task-list-item+.task-list-item {
+  margin-top: 3px;
+}
+
+.markdown-body .task-list-item input {
+  margin: 0 0.35em 0.25em -1.6em;
+  vertical-align: middle;
+}
+
+.markdown-body :checked+.radio-label {
+  z-index: 1;
+  position: relative;
+  border-color: #4078c0;
+}
diff --git a/etc/md2html.ksh b/etc/md2html.ksh
new file mode 100644
index 0000000..35f4c07
--- /dev/null
+++ b/etc/md2html.ksh
@@ -0,0 +1,32 @@
+#!/bin/ksh
+#
+# Convert a markdown file to an html file
+#
+# usage: ms2html 
+#
+# html content is output to stdout.
+#
+# github style sheet from: 
+#    https://github.com/sindresorhus/github-markdown-css
+# Markdown.pl is from:
+#    http://daringfireball.net/projects/downloads/Markdown_1.0.1.zip
+#
+echo ''
+echo ''
+echo '  '
+echo "    Zumo CC3200 `basename $1`"
+echo '    '
+echo '    '
+echo '  '
+echo '  
    '
+perl etc/Markdown.pl $1
+echo '  
    '
+echo ''
+
diff --git a/license.html b/license.html
new file mode 100644
index 0000000..582bc0f
--- /dev/null
+++ b/license.html
@@ -0,0 +1,473 @@
+
+
+
+
+
+
+
+
+
+ 
+
+
+
+Texas Instruments Manifest
+
+
+
+
    
+
    
+  
+    
+      
+      
+    
+  
+
    
+        
+          
+		  
+        
+      
+        
+		
+      
    
+
    
+
    
+
+zumo cc3200 Manifest
+
+
    
+
+
    
+
+09-11-2015
+
+
    
+
+
+
    
+
+Manifest ID - SRAS00002135
+
+
    
+
    
+
    Legend
    
+
    (explanation of the fields in the Manifest Table below)
    
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
    
+Software Name 
+
+The name of the application or file
+
    
+Version
+
+Version of the application or file
+
    
+License Type
+
+Type of license(s) under which TI will be providing
+software to the licensee (e.g. BSD-3-Clause, GPL-2.0, TI TSPA License, TI
+Commercial License). The license could be under Commercial terms or Open Source. See Open Source Reference License Disclaimer in
+the Disclaimers Section. Whenever possible, TI will use an  SPDX Short Identifier  for an Open Source
+License. TI Commercial license terms are not usually included in the manifest and are conveyed through a variety 
+of means such as a clickwrap license upon install, 
+a signed license agreement and so forth.
+
    
+Location
+
+The directory name and path on the media or a specific file where the Software is located. Typically fully qualified path names 
+are not used and instead the relevant top level directory of the application is given. 
+A notation often used in the manifests is [as installed]/directory/*. Note that the asterisk implies that all
+files under that directory are licensed as the License Type field denotes. Any exceptions to this will 
+generally be denoted as [as installed]/directory/* except as noted below which means as shown in subsequent rows of 
+the manifest.
+
    
+Delivered As
+
+This field will either be “Source”, “Binary” or “Source
+and Binary” and is the primary form the content of the Software is delivered
+in. If the Software is delivered in an archive format, this field
+applies to the contents of the archive. If the word Limited is used
+with Source, as in “Limited Source” or “Limited Source and Binary” then
+only portions of the Source for the application are provided.
+
    
+Modified by TI
+
+This field will either be “Yes” or “No”. A “Yes” means
+TI has made changes to the Software. A “No” means TI has not made any
+changes. Note: This field is not applicable for Software “Obtained
+from” TI.
+
    
+Obtained from
+
+This field specifies from where or from whom TI obtained
+the Software. It may be a URL to an Open Source site, a 3rd
+party licensor, or TI. See Links Disclaimer in the Disclaimers
+Section.
+
    
+
    
+
    Disclaimers
    
+
    Export Control Classification Number (ECCN)
    
+
    Any use of ECCNs listed in the Manifest is at the user’s risk
+and without recourse to TI. Your
+company, as the exporter of record, is responsible for determining the
+correct classification of any item at
+the time of export. Any export classification by TI of Software is for
+TI’s internal use only and shall not be construed as a representation
+or warranty
+regarding the proper export classification for such Software or whether
+an export
+license or other documentation is required for exporting such Software
    
+
    Links in the Manifest
    
+
    Any
+links appearing on this Manifest
+(for example in the “Obtained from” field) were verified at the time
+the Manifest was created. TI makes no guarantee that any listed links
+will
+remain active in the future.
    
+
    Open Source License References
    
+
    Your company is responsible for confirming the
+applicable license terms for any open source Software
+listed in this Manifest that was not “Obtained from” TI. Any open
+source license
+specified in this Manifest for Software that was
+not “Obtained from” TI is for TI’s internal use only and shall not be
+construed as a representation or warranty regarding the proper open
+source license terms
+for such Software.
    
+
    
+
    Export Information
    
+
    ECCN for Software included in this release:
    
+Publicly Available  - Open Source or TI TSPA License
+
    
+
+ 
+  
+  
+ 
+ 
     
+  zumo cc3200 Manifest Table 
+ 
     
+ 
+  
+ 
     
+ 
+ See the Legend above for a description of these columns. 
+ 
+ 
     
+  
+ 
     
+   
+ 	 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 	 
+   
+ 
+ 
+  
+ 	 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 	 
+ 	 
+ 		 
+ 		 
+ 	 
+ 
+  
+ 	 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 	 
+ 	 
+ 		 
+ 		 
+ 	 
+ 
+  
+ 	 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 	 
+ 	 
+ 		 
+ 		 
+ 	 
+ 
+  
+ 	 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 	 
+ 	 
+ 		 
+ 		 
+ 	 
+ 
+  
+ 	 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 	 
+ 	 
+ 		 
+ 		 
+ 	 
+ 
+  
+ 	 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 		 
+ 	 
+ 	 
+ 		 
+ 		 
+ 	 
+ 
+  
+ 
    Software NameVersionLicense TypeDelivered AsModified by TI
     
+ Zumo demos 
+  
+ 1.0 
+  
+ BSD-3-clause 
+  
+ source 
+  
+ na 
+ Location 
+ [as installed]/* except as noted in this manifest 
+ 
    Obtained from 
+ TI 
+ 
     
+ LSM303 
+  
+  
+  
+ MIT 
+  
+ source 
+  
+ yes 
+ Location 
+ [as installed]/src/Processing/*/GraphClass.pde 
+ 
    Obtained from 
+ https://github.com/pololu/lsm303-arduino 
+ 
     
+ L3G 
+  
+  
+  
+ MIT 
+  
+ source 
+  
+ yes 
+ Location 
+ [as installed]/src/Processing/*/GraphClass.pde 
+ 
    Obtained from 
+ https://github.com/pololu/l3g-arduino 
+ 
     
+ Pushbutton and ZumoMotors 
+  
+  
+  
+ MIT 
+  
+ source 
+  
+ yes 
+ Location 
+ [as installed]/src/Processing/*/GraphClass.pde 
+ 
    Obtained from 
+ https://github.com/pololu/zumo-shield 
+ 
     
+ Line Graphing class 
+  
+  
+  
+ MIT 
+  
+ source 
+  
+ no 
+ Location 
+ [as installed]/src/Processing/*/GraphClass.pde 
+ 
    Obtained from 
+ https://github.com/sebnil/RealtimePlotter 
+ 
     
+ Cubic Spline interpolation in C++ 
+  
+  
+  
+ GPL2 
+  
+ source 
+  
+ no 
+ Location 
+ [as installed]/src/Energia/ZumoCC3200/utility/Spline.h 
+ 
    Obtained from 
+ http://kluge.in-chemnitz.de/opensource/spline/ 
+ 
     
+  
+ 
     
+ 
     
+ 
     
+
+
    
+
    Credits
    
+
     



    
+
    
+
    Licenses
    
+
     zumo cc3200 Licenses 

     

    LSM303

    Copyright (c) 2014 Pololu Corporation.  For more information, see

    http://www.pololu.com/
    http://forum.pololu.com/

    Permission is hereby granted, free of charge, to any person
    obtaining a copy of this software and associated documentation
    files (the "Software"), to deal in the Software without
    restriction, including without limitation the rights to use,
    copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the
    Software is furnished to do so, subject to the following
    conditions:

    The above copyright notice and this permission notice shall be
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
    OTHER DEALINGS IN THE SOFTWARE.

    L3G
    Copyright (c) 2014 Pololu Corporation.  For more information, see

    http://www.pololu.com/
    http://forum.pololu.com/

    Permission is hereby granted, free of charge, to any person
    obtaining a copy of this software and associated documentation
    files (the "Software"), to deal in the Software without
    restriction, including without limitation the rights to use,
    copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the
    Software is furnished to do so, subject to the following
    conditions:

    The above copyright notice and this permission notice shall be
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
    OTHER DEALINGS IN THE SOFTWARE.

    Pushbutton and ZumoMotors
    Copyright (c) 2014 Pololu Corporation.  For more information, see

    http://www.pololu.com/
    http://forum.pololu.com/

    Permission is hereby granted, free of charge, to any person
    obtaining a copy of this software and associated documentation
    files (the "Software"), to deal in the Software without
    restriction, including without limitation the rights to use,
    copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the
    Software is furnished to do so, subject to the following
    conditions:

    The above copyright notice and this permission notice shall be
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
    OTHER DEALINGS IN THE SOFTWARE.

    Line Graphing class
    The MIT License (MIT)

    Copyright (c) 2013 Sebatian Nilsson

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.

    Cubic Spline interpolation in C++
                        GNU GENERAL PUBLIC LICENSE
                           Version 2, June 1991

     Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
     Everyone is permitted to copy and distribute verbatim copies
     of this license document, but changing it is not allowed.

                                Preamble

      The licenses for most software are designed to take away your
    freedom to share and change it.  By contrast, the GNU General Public
    License is intended to guarantee your freedom to share and change free
    software--to make sure the software is free for all its users.  This
    General Public License applies to most of the Free Software
    Foundation's software and to any other program whose authors commit to
    using it.  (Some other Free Software Foundation software is covered by
    the GNU Lesser General Public License instead.)  You can apply it to
    your programs, too.

      When we speak of free software, we are referring to freedom, not
    price.  Our General Public Licenses are designed to make sure that you
    have the freedom to distribute copies of free software (and charge for
    this service if you wish), that you receive source code or can get it
    if you want it, that you can change the software or use pieces of it
    in new free programs; and that you know you can do these things.

      To protect your rights, we need to make restrictions that forbid
    anyone to deny you these rights or to ask you to surrender the rights.
    These restrictions translate to certain responsibilities for you if you
    distribute copies of the software, or if you modify it.

      For example, if you distribute copies of such a program, whether
    gratis or for a fee, you must give the recipients all the rights that
    you have.  You must make sure that they, too, receive or can get the
    source code.  And you must show them these terms so they know their
    rights.

      We protect your rights with two steps: (1) copyright the software, and
    (2) offer you this license which gives you legal permission to copy,
    distribute and/or modify the software.

      Also, for each author's protection and ours, we want to make certain
    that everyone understands that there is no warranty for this free
    software.  If the software is modified by someone else and passed on, we
    want its recipients to know that what they have is not the original, so
    that any problems introduced by others will not reflect on the original
    authors' reputations.

      Finally, any free program is threatened constantly by software
    patents.  We wish to avoid the danger that redistributors of a free
    program will individually obtain patent licenses, in effect making the
    program proprietary.  To prevent this, we have made it clear that any
    patent must be licensed for everyone's free use or not licensed at all.

      The precise terms and conditions for copying, distribution and
    modification follow.

                        GNU GENERAL PUBLIC LICENSE
       TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION

      0. This License applies to any program or other work which contains
    a notice placed by the copyright holder saying it may be distributed
    under the terms of this General Public License.  The "Program", below,
    refers to any such program or work, and a "work based on the Program"
    means either the Program or any derivative work under copyright law:
    that is to say, a work containing the Program or a portion of it,
    either verbatim or with modifications and/or translated into another
    language.  (Hereinafter, translation is included without limitation in
    the term "modification".)  Each licensee is addressed as "you".

    Activities other than copying, distribution and modification are not
    covered by this License; they are outside its scope.  The act of
    running the Program is not restricted, and the output from the Program
    is covered only if its contents constitute a work based on the
    Program (independent of having been made by running the Program).
    Whether that is true depends on what the Program does.

      1. You may copy and distribute verbatim copies of the Program's
    source code as you receive it, in any medium, provided that you
    conspicuously and appropriately publish on each copy an appropriate
    copyright notice and disclaimer of warranty; keep intact all the
    notices that refer to this License and to the absence of any warranty;
    and give any other recipients of the Program a copy of this License
    along with the Program.

    You may charge a fee for the physical act of transferring a copy, and
    you may at your option offer warranty protection in exchange for a fee.

      2. You may modify your copy or copies of the Program or any portion
    of it, thus forming a work based on the Program, and copy and
    distribute such modifications or work under the terms of Section 1
    above, provided that you also meet all of these conditions:

        a) You must cause the modified files to carry prominent notices
        stating that you changed the files and the date of any change.

        b) You must cause any work that you distribute or publish, that in
        whole or in part contains or is derived from the Program or any
        part thereof, to be licensed as a whole at no charge to all third
        parties under the terms of this License.

        c) If the modified program normally reads commands interactively
        when run, you must cause it, when started running for such
        interactive use in the most ordinary way, to print or display an
        announcement including an appropriate copyright notice and a
        notice that there is no warranty (or else, saying that you provide
        a warranty) and that users may redistribute the program under
        these conditions, and telling the user how to view a copy of this
        License.  (Exception: if the Program itself is interactive but
        does not normally print such an announcement, your work based on
        the Program is not required to print an announcement.)

    These requirements apply to the modified work as a whole.  If
    identifiable sections of that work are not derived from the Program,
    and can be reasonably considered independent and separate works in
    themselves, then this License, and its terms, do not apply to those
    sections when you distribute them as separate works.  But when you
    distribute the same sections as part of a whole which is a work based
    on the Program, the distribution of the whole must be on the terms of
    this License, whose permissions for other licensees extend to the
    entire whole, and thus to each and every part regardless of who wrote it.

    Thus, it is not the intent of this section to claim rights or contest
    your rights to work written entirely by you; rather, the intent is to
    exercise the right to control the distribution of derivative or
    collective works based on the Program.

    In addition, mere aggregation of another work not based on the Program
    with the Program (or with a work based on the Program) on a volume of
    a storage or distribution medium does not bring the other work under
    the scope of this License.

      3. You may copy and distribute the Program (or a work based on it,
    under Section 2) in object code or executable form under the terms of
    Sections 1 and 2 above provided that you also do one of the following:

        a) Accompany it with the complete corresponding machine-readable
        source code, which must be distributed under the terms of Sections
        1 and 2 above on a medium customarily used for software interchange; or,

        b) Accompany it with a written offer, valid for at least three
        years, to give any third party, for a charge no more than your
        cost of physically performing source distribution, a complete
        machine-readable copy of the corresponding source code, to be
        distributed under the terms of Sections 1 and 2 above on a medium
        customarily used for software interchange; or,

        c) Accompany it with the information you received as to the offer
        to distribute corresponding source code.  (This alternative is
        allowed only for noncommercial distribution and only if you
        received the program in object code or executable form with such
        an offer, in accord with Subsection b above.)

    The source code for a work means the preferred form of the work for
    making modifications to it.  For an executable work, complete source
    code means all the source code for all modules it contains, plus any
    associated interface definition files, plus the scripts used to
    control compilation and installation of the executable.  However, as a
    special exception, the source code distributed need not include
    anything that is normally distributed (in either source or binary
    form) with the major components (compiler, kernel, and so on) of the
    operating system on which the executable runs, unless that component
    itself accompanies the executable.

    If distribution of executable or object code is made by offering
    access to copy from a designated place, then offering equivalent
    access to copy the source code from the same place counts as
    distribution of the source code, even though third parties are not
    compelled to copy the source along with the object code.

      4. You may not copy, modify, sublicense, or distribute the Program
    except as expressly provided under this License.  Any attempt
    otherwise to copy, modify, sublicense or distribute the Program is
    void, and will automatically terminate your rights under this License.
    However, parties who have received copies, or rights, from you under
    this License will not have their licenses terminated so long as such
    parties remain in full compliance.

      5. You are not required to accept this License, since you have not
    signed it.  However, nothing else grants you permission to modify or
    distribute the Program or its derivative works.  These actions are
    prohibited by law if you do not accept this License.  Therefore, by
    modifying or distributing the Program (or any work based on the
    Program), you indicate your acceptance of this License to do so, and
    all its terms and conditions for copying, distributing or modifying
    the Program or works based on it.

      6. Each time you redistribute the Program (or any work based on the
    Program), the recipient automatically receives a license from the
    original licensor to copy, distribute or modify the Program subject to
    these terms and conditions.  You may not impose any further
    restrictions on the recipients' exercise of the rights granted herein.
    You are not responsible for enforcing compliance by third parties to
    this License.

      7. If, as a consequence of a court judgment or allegation of patent
    infringement or for any other reason (not limited to patent issues),
    conditions are imposed on you (whether by court order, agreement or
    otherwise) that contradict the conditions of this License, they do not
    excuse you from the conditions of this License.  If you cannot
    distribute so as to satisfy simultaneously your obligations under this
    License and any other pertinent obligations, then as a consequence you
    may not distribute the Program at all.  For example, if a patent
    license would not permit royalty-free redistribution of the Program by
    all those who receive copies directly or indirectly through you, then
    the only way you could satisfy both it and this License would be to
    refrain entirely from distribution of the Program.

    If any portion of this section is held invalid or unenforceable under
    any particular circumstance, the balance of the section is intended to
    apply and the section as a whole is intended to apply in other
    circumstances.

    It is not the purpose of this section to induce you to infringe any
    patents or other property right claims or to contest validity of any
    such claims; this section has the sole purpose of protecting the
    integrity of the free software distribution system, which is
    implemented by public license practices.  Many people have made
    generous contributions to the wide range of software distributed
    through that system in reliance on consistent application of that
    system; it is up to the author/donor to decide if he or she is willing
    to distribute software through any other system and a licensee cannot
    impose that choice.

    This section is intended to make thoroughly clear what is believed to
    be a consequence of the rest of this License.

      8. If the distribution and/or use of the Program is restricted in
    certain countries either by patents or by copyrighted interfaces, the
    original copyright holder who places the Program under this License
    may add an explicit geographical distribution limitation excluding
    those countries, so that distribution is permitted only in or among
    countries not thus excluded.  In such case, this License incorporates
    the limitation as if written in the body of this License.

      9. The Free Software Foundation may publish revised and/or new versions
    of the General Public License from time to time.  Such new versions will
    be similar in spirit to the present version, but may differ in detail to
    address new problems or concerns.

    Each version is given a distinguishing version number.  If the Program
    specifies a version number of this License which applies to it and "any
    later version", you have the option of following the terms and conditions
    either of that version or of any later version published by the Free
    Software Foundation.  If the Program does not specify a version number of
    this License, you may choose any version ever published by the Free Software
    Foundation.

      10. If you wish to incorporate parts of the Program into other free
    programs whose distribution conditions are different, write to the author
    to ask for permission.  For software which is copyrighted by the Free
    Software Foundation, write to the Free Software Foundation; we sometimes
    make exceptions for this.  Our decision will be guided by the two goals
    of preserving the free status of all derivatives of our free software and
    of promoting the sharing and reuse of software generally.

                                NO WARRANTY

      11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
    FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
    OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
    PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
    OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
    TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
    PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
    REPAIR OR CORRECTION.

      12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
    WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
    REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
    INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
    OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
    TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
    YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
    PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
    POSSIBILITY OF SUCH DAMAGES.

                         END OF TERMS AND CONDITIONS

                How to Apply These Terms to Your New Programs

      If you develop a new program, and you want it to be of the greatest
    possible use to the public, the best way to achieve this is to make it
    free software which everyone can redistribute and change under these terms.

      To do so, attach the following notices to the program.  It is safest
    to attach them to the start of each source file to most effectively
    convey the exclusion of warranty; and each file should have at least
    the "copyright" line and a pointer to where the full notice is found.

        <one line to give the program's name and a brief idea of what it does.>
        Copyright (C) <year>  <name of author>

        This program is free software; you can redistribute it and/or modify
        it under the terms of the GNU General Public License as published by
        the Free Software Foundation; either version 2 of the License, or
        (at your option) any later version.

        This program is distributed in the hope that it will be useful,
        but WITHOUT ANY WARRANTY; without even the implied warranty of
        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
        GNU General Public License for more details.

        You should have received a copy of the GNU General Public License along
        with this program; if not, write to the Free Software Foundation, Inc.,
        51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

    Also add information on how to contact you by electronic and paper mail.

    If the program is interactive, make it output a short notice like this
    when it starts in an interactive mode:

        Gnomovision version 69, Copyright (C) year name of author
        Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
        This is free software, and you are welcome to redistribute it
        under certain conditions; type `show c' for details.

    The hypothetical commands `show w' and `show c' should show the appropriate
    parts of the General Public License.  Of course, the commands you use may
    be called something other than `show w' and `show c'; they could even be
    mouse-clicks or menu items--whatever suits your program.

    You should also get your employer (if you work as a programmer) or your
    school, if any, to sign a "copyright disclaimer" for the program, if
    necessary.  Here is a sample; alter the names:

      Yoyodyne, Inc., hereby disclaims all copyright interest in the program
      `Gnomovision' (which makes passes at compilers) written by James Hacker.

      <signature of Ty Coon>, 1 April 1989
      Ty Coon, President of Vice

    This General Public License does not permit incorporating your program into
    proprietary programs.  If your program is a subroutine library, you may
    consider it more useful to permit linking proprietary applications with the
    library.  If this is what you want to do, use the GNU Lesser General
    Public License instead of this License.




    
+
    
+
+
diff --git a/makefile b/makefile
new file mode 100644
index 0000000..818331e
--- /dev/null
+++ b/makefile
@@ -0,0 +1,10 @@
+GOALS = README.html
+
+all: $(GOALS)
+
+%.html:%.md
+	rm -f $@
+	etc/md2html.ksh $< > $@
+
+clean:
+	rm -f $(GOALS)
diff --git a/src/Energia/libraries/ZumoCC3200/ZumoCC3200.h b/src/Energia/libraries/ZumoCC3200/ZumoCC3200.h
new file mode 100644
index 0000000..a2efdc9
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/ZumoCC3200.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef ZumoCC3200_h
+#define ZumoCC3200_h 1
+
+#include 
+
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/AssistedDrive.ino b/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/AssistedDrive.ino
new file mode 100644
index 0000000..5ade3ee
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/AssistedDrive.ino
@@ -0,0 +1,36 @@
+
+/*  ======== AssistedDrive.ino ========
+ *  Simple demo of IMU-assisted driving of the Zumo CC3200
+ *
+ *  This example creates an access point named zumo-drive 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, drive in stright lines and turn 90-degrees (as determined by
+ *  IMU sensors), and acquire IMU telemetry data for real-time display by a
+ *  client program running on a laptop.
+ *
+ *  An example of such a client is provided in the ./Processing/AssistedDrive
+ *  sub-directory.  This is a Processing application (which
+ *  uses the development IDE that inspired the Energia/Wiring development
+ *  environment; see https://processing.org).
+ */
+
+#include 
+#include 
+
+/* imu sketch external declarations */
+extern IMUManager imu;
+extern bool isZeroing;
+extern Utilities::MotorInfo motorStatus;
+
+/* motor sketch external declarations */
+extern char motorWASD;
+extern bool needToZero;
+extern bool readyToRun;
+
+/* AP sketch external declarations */
+#define MAIN_LED_PIN RED_LED
+extern float driveRate;
+extern float driveTime;
+extern int angleToTurn;
diff --git a/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/MotorControlLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/MotorControlLoop.ino
new file mode 100644
index 0000000..f74c02e
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/MotorControlLoop.ino
@@ -0,0 +1,331 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorWASD, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w' - drive forward
+ *      's' - drive backward
+ *      'a' - turn left
+ *      'd' - turn right
+ *      ' ' - stop
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorWASD.
+ */
+#include 
+#include 
+#include 
+#include 
+
+#define PERIOD  1           /* period of motor control updates */
+#define TEST 0
+
+char motorWASD = ' '; /* current motor drive command */
+float targetAngle = 0.0;
+bool targetAngleSet = false;
+
+static ZumoMotors motors; /* Zumo motor driver provided by Pololu */
+
+PIDController steerPID(0.01f, 0.0f, 0.0f);
+PIDController spinPID(2.0f, 0.0f, 0.0f);
+
+Utilities::MotorInfo motorStatus;
+int count = 0;
+bool timedOut = false;
+Utilities util;
+long initTime;
+
+static int   clip(int speed);
+static float clip(float speed);
+static void  drive(char wasd, int speed, unsigned int duration);
+static int   next(int cur, int goal);
+
+/*
+ *  ======== motorSetup ========
+ */
+void motorSetup(void)
+{
+    Serial.begin(9600);
+    Serial.println("motorSetup ...");
+
+    /* initialize the Pololu driver motor library */
+    ZumoMotors::setRightSpeed(0);
+    ZumoMotors::setLeftSpeed(0);
+    motorStatus.leftSpeed = 0;
+    motorStatus.rightSpeed = 0;
+
+    Serial.println("motorSetup done.");
+    util = Utilities();
+}
+
+/*
+ *  ======== motorLoop ========
+ *
+ *  Based on user input, drive open loop or
+ *  perform closed-loop maneuvers
+ */
+void motorLoop(void)
+{
+    static char lastWASD = ' ';
+    if (lastWASD != motorWASD) {
+        lastWASD = motorWASD;
+        
+        Serial.print("new motor cmd: "); Serial.println(motorWASD);
+        Serial.print("  is angle set: "); Serial.println(targetAngleSet);
+        Serial.print("  target angle: "); Serial.println(targetAngle);
+        Serial.print("  timed out   : "); Serial.println(timedOut);
+    }
+
+    if (isZeroing) {
+        spinPID = PIDController(2.0f, 0.0f, 0.0f);
+        steerPID = PIDController(0.01f, 0.0f, 0.0f);
+        ZumoMotors::setLeftSpeed(0);
+        ZumoMotors::setRightSpeed(0);
+        motorStatus.leftSpeed = 0;
+        motorStatus.rightSpeed = 0;
+        delay(500);
+    }
+    else {
+        switch (motorWASD) {
+            case 's':
+            case 'w': {
+                targetAngleSet = false;
+                drive(motorWASD, 250, PERIOD);
+                break;
+            }
+
+            case 'd':
+            case 'a': {
+                targetAngleSet = false;
+                drive(motorWASD, 100, PERIOD);
+                break;
+            }
+
+            case 't': {
+                turnAngle(angleToTurn, driveRate);
+                break;
+            }
+            case 'g': {
+                driveStraight(driveTime, driveRate);
+                break;
+            }
+
+            default: {
+                targetAngleSet = false;
+                motorWASD = ' ';
+                drive(' ', 0, 10);
+                break;
+            }
+        }
+    }
+}
+
+/*
+ *  ======== clip ========
+ */
+static int clip(int speed)
+{
+    if (speed < -400) {
+        speed = -400;
+    }
+    else if (speed > 400) {
+        speed = 400;
+    }
+    return (speed);
+}
+
+static float clip(float speed)
+{
+    if (speed < -400) {
+        speed = -400.0;
+    }
+    else if (speed > 400) {
+        speed = 400;
+    }
+    return (speed);
+}
+
+/*
+ *  ======== drive ========
+ *  Drive motors in the direction and speed specified by wasd and goal
+ *
+ *  Note: negative goal values imply a reversal of the wasd direction
+ */
+static void drive(char wasd, int goal, unsigned int duration)
+{
+    static int leftSpeed = 0;
+    static int rightSpeed = 0;
+    
+    while (duration > 0) {
+        duration--;
+
+        /* gradually adjust curent speeds to goal */
+        switch (wasd) {
+            case ' ': { /* stop */
+                leftSpeed = next(leftSpeed, 0);
+                rightSpeed = next(rightSpeed, 0);
+                break;
+            }
+            case 'w': { /* forward */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            case 's': { /* backward */
+                leftSpeed = next(leftSpeed, -goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'd': { /* turn right */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'a': { /* turn left */
+                leftSpeed = next(leftSpeed, -goal);;
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            default: {
+                break;
+            }
+        }
+
+        /* clip speeds to allowable range */
+        leftSpeed = clip(leftSpeed);
+        rightSpeed = clip(rightSpeed);
+    
+        /* set motor speeds */
+        ZumoMotors::setLeftSpeed(leftSpeed);
+        ZumoMotors::setRightSpeed(rightSpeed);
+        motorStatus.leftSpeed = leftSpeed;
+        motorStatus.rightSpeed = rightSpeed;
+
+        /* sleep for 1 ms (so duration is in milliseconds) */
+        delay(1);
+    }
+}
+
+/*
+ *  ======== next ========
+ *  Compute the next motor speed value given the cur speed and a new goal
+ */
+static int next(int cur, int goal)
+{
+    int tmp = (goal - cur) * 0.0625f + cur;
+    return (tmp == cur ? goal : tmp);
+}
+
+/*
+ *  ======== turnAngle ========
+ *  Turn an angle using a PID controller
+ */
+void turnAngle(float angle, float rate)
+{
+    float error;
+    float power;
+
+    if (!targetAngleSet) {
+        Utilities::reInitErrorVals();
+        float currHeading = IMUManager::getGyroYaw();
+        targetAngle = Utilities::wrapAngle(currHeading + angle);
+        targetAngleSet = true;
+    }
+    else {
+        error = Utilities::wrapAngle(IMUManager::getGyroYaw() - targetAngle);
+        power = spinPID.calculate(error);
+        bool done = Utilities::inRange(error, -0.5f, 0.5f);
+        if (done) {
+            motorWASD = ' ';
+            targetAngleSet = false;
+            power = 0;
+        }
+
+        /* if power is too low, the bot doesn't turn */
+        if (power > 0 && power < 70) {
+            power = 70;
+        }
+        else if (power < 0 && power > -70) {
+            power = -70;
+        }
+
+        /* if power is higher than 250, a buildup of error in the
+         * integrated gyro angle occurs
+         */
+        if (power > 250) {
+            power = 250;
+        }
+        else if (power < -250) {
+            power = -250;
+        }
+
+        motorStatus.leftSpeed = power;
+        motorStatus.rightSpeed = -power;
+
+        ZumoMotors::setLeftSpeed(power);
+        ZumoMotors::setRightSpeed(-power);
+
+        motorStatus.error = error;
+        motorStatus.time = (float)millis();
+    }
+
+    delay(10);
+}
+
+/*
+ *  ======== driveStraight ========
+ *  Use the gyro and PID control to drive the robot straight
+ */
+void driveStraight(float numSeconds, float rate)
+{
+    bool forward = true;
+    
+    if (rate < 0) {
+        forward = false;
+        rate = -rate;
+    }
+
+    if (!targetAngleSet) {
+        float currHeading = IMUManager::getGyroYaw();
+        targetAngle = currHeading;
+        timedOut = false;
+        targetAngleSet = true;
+        initTime = millis();
+    }
+    else {
+        float error = util.wrapAngle(IMUManager::getGyroYaw() - targetAngle);
+        float power = steerPID.calculate(error);
+        power = util.saturate(power, rate - 1.0f, 1.0f - rate);
+        float lTotal = clip((rate + power) * 400);
+        float rTotal = clip((rate - power) * 400);
+
+        if (!forward) {
+            float temp = lTotal;
+            lTotal = -rTotal;
+            rTotal = -temp;
+        }
+
+        if (timedOut) {
+            lTotal = 0;
+            rTotal = 0;
+            motorWASD = ' ';
+        }
+        else {
+            timedOut = ((millis() - initTime) > ((int)(numSeconds * 1000.0f)));
+        }
+
+        ZumoMotors::setLeftSpeed(lTotal);
+        ZumoMotors::setRightSpeed(rTotal);
+
+        motorStatus.error = error;
+        motorStatus.leftSpeed = lTotal;
+        motorStatus.rightSpeed = rTotal;
+        motorStatus.time = (float)millis();
+    }
+
+    delay(10);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/SensorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/SensorLoop.ino
new file mode 100644
index 0000000..c6a38bc
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/SensorLoop.ino
@@ -0,0 +1,99 @@
+/*
+ *  ======== imuLoop ========
+ *  This sketch simply reads the Zumo IMU sensors at 200Hz and prints
+ *  the current value once per second to the UART.
+ */
+
+#include 
+
+/* Pololu IMU data instance objects */
+IMUManager imu;
+
+bool needToZero = false; /* set by WiFi to recalibrate Gyro */
+bool isZeroing = true;   /* flag indicating we are in "zero'ing mode */
+
+static void calibrateMagnetometer();
+
+/*
+ *  ======== sensorSetup ========
+ */
+void sensorSetup()
+{
+    Serial.begin(9600);
+    Serial.println("sensorSetup ...");
+
+    Wire.begin();
+    imu = IMUManager();
+
+    /* initialize Zumo accelerometer and magnetometer */
+    imu.initAccel();
+    imu.enableAccelDefault();
+
+    /* initialize Zumo gyro */
+    if (!imu.initGyro()) {
+        Serial.print("Failed to autodetect gyro type!");
+        delay(1000);
+    }
+    imu.enableGyroDefault();
+    
+    Serial.println("Calibrating gyro for 2 seconds: keep zumo still during calibration period");
+    isZeroing = true;
+    imu.calibrateGyro(2);
+    imu.zeroGyroXAxis();
+    imu.zeroGyroYAxis();
+    imu.zeroGyroZAxis();
+    isZeroing = false;    
+    
+    //calibrateMagnetometer();
+
+    Serial.println("sensorSetup done.");
+}
+
+/*
+ *  ======== sensorLoop ========
+ */
+void sensorLoop()
+{
+    static int imuCount = 0;
+
+    /* update IMU data every 5 ms (200 Hz) */
+
+    if (needToZero) {
+        imu.calibrateGyro(1);
+        imu.zeroGyroXAxis();
+        imu.zeroGyroYAxis();
+        imu.zeroGyroZAxis();
+        needToZero = false;
+    }
+
+    /* read data from all IMU sensors */
+    imu.readGyro();
+    imu.readAccel();
+    imu.readMag();
+
+    delay(5);
+}
+
+/*
+ *  ======== calibrateMagnetometer ========
+ *  Calibrate the magnetometer 
+ */
+static void calibrateMagnetometer()
+{
+    static bool calibrated = 0;
+    if (!calibrated) {
+        calibrated = 1;
+        
+        /* calibrate magnetometer */
+        Serial.print("Calibrating magnetometer: rotate the zumo along all axes during calibration period ... ");
+        
+        /* illuminate LED while calibrating */
+        digitalWrite(MAIN_LED_PIN, HIGH);
+        
+        imu.calibrateMagnetometer(200);
+        Serial.println("done.");
+        
+        /* shutoff LED */
+        digitalWrite(MAIN_LED_PIN, LOW);
+    }
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/WifiLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/WifiLoop.ino
new file mode 100644
index 0000000..20dc455
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/WifiLoop.ino
@@ -0,0 +1,219 @@
+/*
+ *  ======== wifiLoop ========
+ *  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 
+#include 
+
+#include 
+
+/* name of the network and its password */
+static const char ssid[] = "zumo-drive";
+static const char wifipw[] = "password";
+
+/* port number of the server listening for commands at 192.168.1.1 */
+#define PORTNUM 8080
+
+float driveRate;
+float driveTime;
+int angleToTurn;
+bool readyToRun;
+
+/* create data server on port PORTNUM */
+static WiFiServer server(PORTNUM);
+
+static void doWASD(char wasd, WiFiClient client);
+static int readBuf(WiFiClient client, uint8_t *buf, unsigned int len);
+
+/*
+ *  ======== wifiSetup ========
+ */
+void wifiSetup()
+{
+    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);
+    readyToRun = false;
+}
+
+/*
+ *  ======== readBuf ========
+ */
+static int readBuf(WiFiClient client, uint8_t *buf, unsigned int len)
+{
+    /* wait for len characters to arrive */
+    for (unsigned int i = 0; client.available() < len; i++) {
+        /* yield or sleep for an ever increasing period to save power */
+        i > 0 ? delay(i) : Task_yield();
+
+        if (!client.connected()) {
+            return -1;
+        }
+    }
+    client.read(buf, len);
+    return 0;
+}
+
+/*
+ *  ======== wifiLoop ========
+ */
+void wifiLoop()
+{
+    /* 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) {
+            readyToRun = true;
+
+            /* if there's a client, read and process commands */
+            static char buffer[64] = { 0 };
+            int bufLen = 0;
+            doWASD(' ', client);
+
+            /* 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 (c == 'g') {
+                        uint8_t cmdBuf[2] = { 0 };
+                        if (readBuf(client, cmdBuf, sizeof(cmdBuf)) < 0) {
+                            break;
+                        }
+                        processDriveCommand(cmdBuf);
+                        doWASD('g', client);
+                    }
+                    else if (c == 't') {
+                        Serial.println("Received t, going to send IMU data");
+                        uint8_t cmdBuf[3] = { 0 };
+                        if (readBuf(client, cmdBuf, sizeof(cmdBuf)) < 0) {
+                            break;
+                        }
+                        processTurnCommand(cmdBuf);
+                        doWASD('t', client);
+                    }
+
+                    /* if there's a new line, we have a complete command */
+                    if (c == '\n') {
+                        doWASD(buffer[0], 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 */
+            motorWASD = ' ';
+        }
+    }
+
+    /* check for new connections 2 times per second */
+    delay(500);
+}
+
+/*
+ *  ======== doWASD ========
+ */
+static void doWASD(char wasd, WiFiClient client)
+{
+    static char report[124];
+    static char lastCmd = ' ';
+    
+    if (wasd == 'z') {
+        if (lastCmd != 'z') {
+            needToZero = true;
+        }
+    }
+    lastCmd = wasd;
+
+    if (wasd != '.') {     /* set new motor command */
+        motorWASD = wasd == 'z' ? ' ' : wasd;
+    }
+
+    int angle = (int16_t)imu.getGyroYaw() * 100;
+    int error = motorStatus.error * 100;
+    int leftSpeed = motorStatus.leftSpeed * 100;
+    int rightSpeed = motorStatus.rightSpeed * 100;
+    long time = millis();
+    
+    int magX = (int16_t)(imu.getMagX() * 100);
+    int magY = (int16_t)(imu.getMagY() * 100);
+    int magZ = (int16_t)(imu.getMagZ() * 100);
+
+    /* send current IMU data */
+    int len = System_snprintf(report, sizeof(report),
+                    "A: %6d %6d %6d G: %6d %6d %6d M: %6d %6d %6d I: %6d U: %6d %6d %6d %6d",
+                    imu.accel_x, imu.accel_y, imu.accel_z,
+                    imu.gyro_x,  imu.gyro_y,  imu.gyro_z,
+                    magX,        magY,        magZ,
+                    angle,
+                    error, leftSpeed, rightSpeed, time);
+    if (client.write((unsigned char *)report, 120) != 120) {
+        Serial.println("Error: reply failed, status != 120");
+    }
+}
+
+/*
+ *  ======== processDriveCommand ========
+ */
+static void processDriveCommand(uint8_t cmd[])
+{
+    driveRate = (int8_t)cmd[0] / 128.0f;
+    driveTime = cmd[1];
+    Serial.print("drive rate: "); Serial.println(driveRate);
+}
+
+/*
+ *  ======== processTurnCommand ========
+ */
+static void processTurnCommand(uint8_t cmd[])
+{
+    driveRate = (int8_t)cmd[0] / 128.0f;
+
+    int combined = (cmd[2] << 8) | cmd[1];
+    if (driveRate < 0) {
+        combined *= -1;
+    }
+    angleToTurn = combined;
+    Serial.print("Turn angle: "); Serial.println(angleToTurn);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/AttitudeDisplay.ino b/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/AttitudeDisplay.ino
new file mode 100644
index 0000000..905d192
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/AttitudeDisplay.ino
@@ -0,0 +1,26 @@
+/*  ======== AttitudeDisplay.ino ========
+ *  Example that calculates the direction cosine matrix from IMU data
+ *
+ *  This example creates an access point named zumo-attitude with password
+ *  "password" and a simple command/telemetry server on port 8080.
+ *
+ *  The command server sends the IMU and DCM data to a WiFi client for 
+ *  display purposes, while listening for any motor commands.
+ *
+ *  An example of such a client are provided in the ./Processing
+ *  sub-directory: AttitudeDisplay.  This is a Processing application (which
+ *  uses the development IDE that inspired the Energia/Wiring development
+ *  environment; see https://processing.org).
+ */
+#include 
+#include 
+
+/* imu sketch external declarations */
+extern IMUManager imu;
+extern DCM_Handle imuDCM;
+
+/* motor sketch external declarations */
+extern char motorWASD;
+
+/* main external declarations */
+#define MAIN_LED_PIN RED_LED
diff --git a/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/apLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/apLoop.ino
new file mode 100644
index 0000000..86a15ae
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/apLoop.ino
@@ -0,0 +1,169 @@
+/*
+ *  ======== 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 
+#include 
+
+#include 
+#include 
+
+
+/* name of the network and its password */
+static const char ssid[] = "zumo-attitude";
+static const char wifipw[] = "password";
+
+/* port number of the server listening for commands at 192.168.1.1 */
+#define PORTNUM 8080
+
+/* create data server on port PORTNUM */
+static WiFiServer server(PORTNUM);
+
+static void doWASD(char wasd);
+static void writeData(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') {
+                        doWASD(buffer[0]);
+                        writeData(client);
+                        /* reset command buffer index to get next command */
+                        bufLen = 0;
+                    }
+                }
+                else{
+                    //writeData(client);
+                    delay(2);
+                }
+            }
+
+            /* disconnect => implicitly stop the motor */
+            motorWASD = ' ';
+            
+            /* client disconnected or timed out, close the connection */
+            client.flush();
+            client.stop();
+        }
+    }
+
+    /* check for new connections 2 times per second */
+    delay(500);
+}
+
+/*
+ *  ======== doWASD ========
+ */
+static void doWASD(char wasd)
+{
+    /* set new motor command */
+    motorWASD = wasd;
+}
+
+/*
+ *  ======== writeData ========
+ */
+static void writeData(WiFiClient client)
+{
+    static char report[146];
+    
+    float matrix[3][3];
+    DCM_getMatrix(imuDCM, matrix);
+    
+    /* convert DCM data for ease of tranfer */
+    int matrix00 = (int16_t)(matrix[0][0] * 100);
+    int matrix01 = (int16_t)(matrix[0][1] * 100);
+    int matrix02 = (int16_t)(matrix[0][2] * 100);
+    int matrix10 = (int16_t)(matrix[1][0] * 100);
+    int matrix11 = (int16_t)(matrix[1][1] * 100);
+    int matrix12 = (int16_t)(matrix[1][2] * 100);
+    int matrix20 = (int16_t)(matrix[2][0] * 100);
+    int matrix21 = (int16_t)(matrix[2][1] * 100);
+    int matrix22 = (int16_t)(matrix[2][2] * 100);
+    
+//    /* print the DCM data to console */
+//    Serial.println("--------DCM--------");
+//    Serial.print(matrix00);Serial.print(" ");Serial.print(matrix01);Serial.print(" ");Serial.println(matrix02);
+//    Serial.print(matrix10);Serial.print(" ");Serial.print(matrix11);Serial.print(" ");Serial.println(matrix12);
+//    Serial.print(matrix20);Serial.print(" ");Serial.print(matrix21);Serial.print(" ");Serial.println(matrix22);
+//    Serial.println("-------------------");
+    
+    //Serial.println("***************************");
+    //Serial.print(imu.mag_x);Serial.print(" ");Serial.print(imu.mag_y);Serial.print(" ");Serial.println(imu.mag_z);
+    //Serial.print(imu.getMagX());Serial.print(" ");Serial.print(imu.getMagY());Serial.print(" ");Serial.println(imu.getMagZ());
+    //Serial.println("***************************");
+
+    /* send current IMU data */
+    System_snprintf(report, sizeof(report),
+      /* accelerometer, gyro,          magnetometer   DCM */
+      /* 0   1   2   3  4   5   6   7  8   9   10  11 12  13  14  15  16  17  18  19  20  21 */
+        "A: %6d %6d %6d G: %6d %6d %6d M: %6d %6d %6d D: %6d %6d %6d %6d %6d %6d %6d %6d %6d",
+                    imu.accel_x, imu.accel_y, imu.accel_z,
+                    imu.gyro_x,  imu.gyro_y,  imu.gyro_z, 
+                    imu.mag_x,   imu.mag_y,   imu.mag_z,
+                    matrix00,    matrix01,    matrix02,
+                    matrix10,    matrix11,    matrix12,
+                    matrix20,    matrix21,    matrix22);
+    if (client.write((unsigned char *)report, 138) != 138) {
+        Serial.println("Error: reply failed, status != 138");
+    }
+ }
diff --git a/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/imuLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/imuLoop.ino
new file mode 100644
index 0000000..e1ef13c
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/imuLoop.ino
@@ -0,0 +1,133 @@
+/*
+*  ======== imuLoop ========
+*  This sketch intializes the direction cosine matrix state with initial
+*  sensor readings, then continuously reads the sensors at 200 Hz and 
+*  updates the DCM based on the sensor data.
+*
+*  In addition, this sketch implements the magnetometer calibration sequence
+*  which normalizes the raw magnetometer data for use in DCM calculations. During
+*  setup, an LED will light up, indicating that the calibration sequence is running.
+*  rotating the Zumo sufficiently will complete the calibration and the LED will
+*  turn off.
+*/
+
+#include 
+#include 
+
+#include 
+#include 
+
+/* IMU data instance objects */
+IMUManager imu;
+DCM_Handle imuDCM;
+
+static void calibrateDCM(DCM_Handle dcm);
+static void getIMUData(DCM_Handle dcm);
+
+/*
+ *  ======== imuSetup ========
+ */
+void imuSetup()
+{
+    Serial.begin(9600);
+    Serial.println("imuSetup ...");
+
+    Wire.begin();
+    
+    imu = IMUManager();
+
+    /* initialize Zumo accelerometer and magnetometer */
+    imu.initAccel();
+    imu.enableAccelDefault();
+
+    /* initialize Zumo gyro */
+    if (!imu.initGyro()) {
+        Serial.print("Failed to autodetect gyro type!");
+        delay(1000);
+    }
+    imu.enableGyroDefault();
+
+    /* calibrate the gyro */
+    Serial.println("Calibrating gyro for 2 seconds: keep zumo still during calibration period");
+    imu.calibrateGyro(2);
+    imu.zeroGyroZAxis();
+    imu.zeroGyroYAxis();
+    imu.zeroGyroZAxis();
+    
+    /* create a new DCM 'object' with
+     * update time 5 ms, and scaling constants (0.03, 0.97, 0.001)
+     */
+    imuDCM = DCM_create(0.005f, 0.03f, 0.97f, 0.01f);
+}
+
+/*
+ *  ======== imuLoop ========
+ */
+void imuLoop()
+{
+    /* calibrate DCM filters */
+    calibrateDCM(imuDCM);
+    
+    /* update imuDCM's IMU data every 5 ms (200 Hz) */
+    getIMUData(imuDCM);
+    
+    /* re-compute imuDCM's values */
+    DCM_update(imuDCM);
+
+    delay(5);
+}
+
+/*
+ *  ======== calibrateDCM ========
+ *  Calibrate the magnetometer and initialize the DCM object (if necessary)
+ */
+static void calibrateDCM(DCM_Handle dcm)
+{
+    static bool calibrated = 0;
+    if (!calibrated) {
+        calibrated = 1;
+        
+        /* calibrate magnetometer */
+        Serial.print("Calibrating magnetometer: rotate the zumo along all axes during calibration period ... ");
+        
+        /* illuminate LED while calibrating */
+        digitalWrite(MAIN_LED_PIN, HIGH);
+        
+        imu.calibrateMagnetometer(200);
+        Serial.println("done.");
+        
+        /* shutoff LED */
+        digitalWrite(MAIN_LED_PIN, LOW);
+
+        /* get initial calibrated IMU data for dcm */
+        getIMUData(dcm);
+
+        /* reset the dcm's filters starting from initial estimates */
+        DCM_start(dcm);
+    }   
+}
+
+/*
+ *  ======== getIMUData ========
+ *  Provide a new reading of the IMU data to the DCM object
+ */
+static void getIMUData(DCM_Handle dcm)
+{
+    float gyroX, gyroY, gyroZ;
+    
+    /* read data from the sensors */
+    imu.readGyro();
+    imu.readAccel();
+    imu.readMag();
+    
+    /* convert gyro angular velocity from deg/s to rad/s  */
+    gyroX = 0.0174532925f * (imu.getGyroX());
+    gyroY = 0.0174532925f * (imu.getGyroY());
+    gyroZ = 0.0174532925f * (imu.getGyroZ());
+    
+    /* push initial IMU data into the DCM "filter" */
+    DCM_updateAccelData(dcm, imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
+    DCM_updateGyroData(dcm, gyroX, gyroY, gyroZ);
+    DCM_updateMagnetoData(dcm, imu.getMagX(), imu.getMagY(), imu.getMagZ());
+}
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/motorLoop.ino
new file mode 100644
index 0000000..ab0dd08
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/AttitudeDisplay/motorLoop.ino
@@ -0,0 +1,175 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorWASD, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w'  - drive forward
+ *      's'  - drive backward
+ *      'a'  - turn left
+ *      'd'  - turn right
+ *      ' '  - stop
+ *      '\0' - idle (sleep for 10 ms witout making any changes to current motor power settings)
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorWASD.
+ */
+#include 
+
+#include 
+
+#define PERIOD  1           /* period of motor control updates */
+
+char motorWASD = '\0';       /* current motor drive command */
+
+static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
+
+static int clip(int speed);
+static void drive(char wasd, int goal, unsigned int duration);
+static int next(int cur, int goal);
+
+/*
+ *  ======== 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 (motorWASD) {
+        case 's':
+        case 'w': {
+            /* illuminate LED while driving */
+            digitalWrite(MAIN_LED_PIN, HIGH);
+            drive(motorWASD, 200, PERIOD);
+            break;
+        }
+
+        case 'd':
+        case 'a': {
+            /* blink LED while turning */
+            if (count == ((count / 100) * 100)) {
+                state ^= 1;
+                digitalWrite(MAIN_LED_PIN, state);
+            }   
+            drive(motorWASD, 100, PERIOD);
+            break;
+        }
+        
+        case '\0':
+            delay(10); /* delay so that the other threads have a chance to run */  
+            break;
+            
+        default: {
+            /* turn off LED while stopped */
+            motorWASD = ' ';
+            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 ========
+ *  Drive motors in the direction and speed specified by wasd and goal
+ *
+ *  Note: negative goal values imply a reversal of the wasd direction
+ */
+static void drive(char wasd, int goal, unsigned int duration)
+{
+    static int leftSpeed = 0;
+    static int rightSpeed = 0;
+    
+    while (duration > 0) {
+        duration--;
+
+        /* gradually adjust curent speeds to goal */
+        switch (wasd) {
+            case ' ': { /* stop */
+                leftSpeed = next(leftSpeed, 0);
+                rightSpeed = next(rightSpeed, 0);
+                break;
+            }
+            case 'w': { /* forward */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            case 's': { /* backward */
+                leftSpeed = next(leftSpeed, -goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'd': { /* turn right */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'a': { /* turn left */
+                leftSpeed = next(leftSpeed, -goal);;
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            default: {
+                break;
+            }
+        }
+
+        /* clip speeds to allowable range */
+        leftSpeed = clip(leftSpeed);
+        rightSpeed = clip(rightSpeed);
+    
+        /* set motor speeds */
+        ZumoMotors::setLeftSpeed(leftSpeed);
+        ZumoMotors::setRightSpeed(rightSpeed);
+
+        /* sleep for 1 ms (so duration is in milliseconds) */
+        delay(1);
+    }
+}
+
+/*
+ *  ======== next ========
+ *  Compute the next motor speed value given the cur speed and a new goal
+ */
+static int next(int cur, int goal)
+{
+    int tmp = (goal - cur) * 0.0625f + cur;
+    return (tmp == cur ? goal : tmp);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Balancing/Balancing.ino b/src/Energia/libraries/ZumoCC3200/examples/Balancing/Balancing.ino
new file mode 100644
index 0000000..a41f57e
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Balancing/Balancing.ino
@@ -0,0 +1,23 @@
+/*  ======== Balancing.ino ========
+ *  Project which implements a "Balancer" class with methods to balance 
+ *  the zumo horizontally and vertically.
+ *  
+ *  This example uses a complementary filter on the gyroscope and 
+ *  accelerometer data to form an accurate tilt angle reading, then
+ *  employs PID feedback control to regulate that angle to 0.
+ *  
+ *  This example interacts with the Processing application titled 
+ *  "Balancing", which retains most of the functionality of the 
+ *  basic graphing application but enables the user to start/stop the 
+ *  balancing loop and tune the PID gains in real-time.
+ */
+
+#include 
+#include 
+
+/* imu sketch external declarations */
+extern IMUManager imu;
+
+/* motor sketch external declarations */
+extern char     motorWASD;
+extern Balancer motorBal;
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Balancing/apLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/Balancing/apLoop.ino
new file mode 100644
index 0000000..da61629
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Balancing/apLoop.ino
@@ -0,0 +1,160 @@
+/*
+ *  ======== 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 
+#include 
+#include 
+#include 
+
+/* name of the network and its password */
+static const char ssid[] = "zumo-balance";
+static const char wifipw[] = "password";
+
+/* port number of the server listening for commands at 192.168.1.1 */
+#define PORTNUM 8080
+
+/* create data server on port PORTNUM */
+static WiFiServer server(PORTNUM);
+static void doWASD(char cmd[], WiFiClient client);
+static void readBuf(WiFiClient client, uint8_t *buf, unsigned int len);
+static void readString(WiFiClient client, char *buf);
+
+/*
+ *  ======== 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') {
+                        /* do the command and send status */
+                        doWASD(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 */
+            motorWASD = ' ';
+        }
+    }
+
+    /* check for new connections 2 times per second */
+    delay(500);
+}
+
+/*
+ *  ======== doWASD ========
+ */
+static void doWASD(char cmd[], WiFiClient client)
+{
+    static char report[104];
+    char c = cmd[0];
+    
+    if (c == 'P') {
+        double newP = atof(cmd + 1);
+        Serial.print("new p gain: "); Serial.println(newP);
+        motorBal.verticalPID.setP(newP);
+    }
+    else if (c == 'I') {
+        double newI = atof(cmd + 1);
+        Serial.print("new i gain: "); Serial.println(newI);
+        motorBal.verticalPID.setI(newI);
+    }
+    else if (c == 'D') {
+        Serial.print("new d gain: ");
+        double newD = atof(cmd + 1);
+        Serial.print("new d gain: "); Serial.println(newD);
+        motorBal.verticalPID.setD(newD);
+    }
+    else if (c == 'w' || c == 'a' || c == 's'  || c == 'd' || c == ' '
+             || c == 'b' || c == 'B') {
+        motorWASD = c;
+    }
+ 
+    /* get current IMU data */
+    int tiltAngle = (int16_t)(imu.getFilteredTiltAngle() * 100);
+    int motorSpeed = (int16_t)motorBal.balanceStatus.motorPower;
+    int balanceError = (int16_t)(motorBal.balanceStatus.error * 100);
+    int angularVel = (int16_t)(imu.getGyroY() * 100);
+
+    /* send current IMU data */
+    int len = System_snprintf(report, sizeof(report),
+        "A: %6d %6d %6d G: %6d %6d %6d M: %6d %6d %6d B: %6d %6d %6d",
+                    imu.accel_x, imu.accel_y, imu.accel_z,
+                    imu.gyro_x,  imu.gyro_y,  imu.gyro_z, 
+                    imu.mag_x,   imu.mag_y,   imu.mag_z, 
+                    tiltAngle,   motorSpeed,  angularVel);
+
+    if (client.write((unsigned char *)report, 96) != 96) {
+        Serial.println("Error: reply failed, status != 96");
+    }
+}
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Balancing/imuLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/Balancing/imuLoop.ino
new file mode 100644
index 0000000..ef1b675
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Balancing/imuLoop.ino
@@ -0,0 +1,58 @@
+/*
+ *  ======== imuLoop ========
+ *  This sketch reads the Zumo IMU sensors at 20Hz and prints
+ *  the current value once per second to the UART.
+ */
+
+#include 
+
+/* IMU sketch global data */
+IMUManager imu;
+
+/*
+ *  ======== imuSetup ========
+ */
+void imuSetup()
+{
+    Serial.begin(9600);
+    Serial.println("imuSetup ...");
+
+    Wire.begin();
+    
+    imu = IMUManager();
+
+    /* initialize Zumo accelerometer and magnetometer */
+    imu.initAccel();
+    imu.enableAccelDefault();
+
+    /* initialize Zumo gyro */
+
+    if (!imu.initGyro()) {
+        Serial.print("Failed to autodetect gyro type!");
+        delay(1000);
+    }
+    imu.enableGyroDefault();
+    Serial.println("sensorSetup done.");
+    
+    imu.calibrateGyro(2);
+    imu.zeroGyroZAxis();
+    imu.zeroGyroYAxis();
+    imu.zeroGyroZAxis();
+    
+    imu.readGyro();
+    imu.readAccel();
+    imu.readMag();
+}
+
+/*
+ *  ======== imuLoop ========
+ */
+void imuLoop()
+{
+    /* update IMU data every 5 ms (200 Hz) */
+    imu.readGyro();
+    imu.readAccel();
+    imu.readMag();
+
+    delay(5);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Balancing/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/Balancing/motorLoop.ino
new file mode 100644
index 0000000..ce48f87
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Balancing/motorLoop.ino
@@ -0,0 +1,175 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorWASD, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w' - drive forward
+ *      's' - drive backward
+ *      'a' - turn left
+ *      'd' - turn right
+ *      'b' - start balancing vertically
+ *      'B' - start balancing horizontally
+ *      ' ' - stop
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorWASD.
+ */
+
+#include 
+#include 
+
+#include 
+#include 
+#include 
+
+#define PERIOD  0           /* period of motor control updates */
+char motorWASD = ' ';       /* current motor drive command */
+
+static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
+
+static void drive(char wasd, int goal, unsigned int duration);
+static int next(int cur, int goal);
+
+Balancer motorBal;
+
+/*
+ *  ======== motorSetup ========
+ */
+void motorSetup(void)
+{
+    Serial.println("motorSetup ...");
+  
+    /* initialize the Pololu driver motor library */
+    motors.setRightSpeed(0);
+    motors.setLeftSpeed(0);
+
+    Serial.println("motorSetup done.");
+  
+    motorBal = Balancer();
+}
+
+/*
+ *  ======== motorLoop ========
+ */
+void motorLoop(void)
+{
+    switch (motorWASD) {
+    case 's':
+    case 'w': 
+      {
+        drive(motorWASD, 200, PERIOD);
+        break;
+      }
+    case 'd':
+    case 'a': 
+      {
+        drive(motorWASD, 100, PERIOD);
+        break;
+      }
+    case 'b':
+      {
+        motorBal.verticalBalance(motors, imu);
+        break;
+      }  
+    case 'B':
+      {
+        motorBal.horizontalBalance(motors, imu);
+        break;
+      }
+    default: 
+      {
+        motorWASD = ' ';
+        drive(' ', 0, 10);
+        break;
+      }
+    }
+
+    delay(5);
+}
+
+/*
+ *  ======== clip ========
+ */
+static int clip(int speed)
+{
+    if (speed < -400) {
+        speed = -400;
+    }
+    else if (speed > 400) {
+        speed = 400;
+    }
+    return (speed);
+}
+
+/*
+ *  ======== drive ========
+ *  Drive motors in the direction and speed specified by wasd and goal
+ *
+ *  Note: negative goal values imply a reversal of the wasd direction
+ */
+static void drive(char wasd, int goal, unsigned int duration)
+{
+    static int leftSpeed = 0;
+    static int rightSpeed = 0;
+    
+    while (duration > 0) {
+        duration--;
+
+        /* gradually adjust curent speeds to goal */
+        switch (wasd) {
+            case ' ': { /* stop */
+                leftSpeed = next(leftSpeed, 0);
+                rightSpeed = next(rightSpeed, 0);
+                break;
+            }
+            case 'w': { /* forward */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            case 's': { /* backward */
+                leftSpeed = next(leftSpeed, -goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'd': { /* turn right */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'a': { /* turn left */
+                leftSpeed = next(leftSpeed, -goal);;
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            default: {
+                break;
+            }
+        }
+
+        /* clip speeds to allowable range */
+        leftSpeed = clip(leftSpeed);
+        rightSpeed = clip(rightSpeed);
+    
+        /* set motor speeds */
+        ZumoMotors::setLeftSpeed(leftSpeed);
+        ZumoMotors::setRightSpeed(rightSpeed);
+
+        /* sleep for 1 ms (so duration is in milliseconds) */
+        delay(1);
+    }
+}
+
+/*
+ *  ======== next ========
+ *  Compute the next motor speed value given the cur speed and a new goal
+ */
+static int next(int cur, int goal)
+{
+    int tmp = (goal - cur) * 0.0625f + cur;
+    return (tmp == cur ? goal : tmp);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/CommandExample/CommandExample.ino b/src/Energia/libraries/ZumoCC3200/examples/CommandExample/CommandExample.ino
new file mode 100644
index 0000000..c778dee
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/CommandExample/CommandExample.ino
@@ -0,0 +1,28 @@
+/*  ======== CommandExample.ino ========
+ *  Simple demo of the CC3200 Zumo platform
+ *
+ *  This example creates an access point named zumo-command with password
+ *  "password" and a simple command/telemetry server on port 8080.
+ */
+
+#include 
+#include 
+#include 
+#include 
+
+/* imu sketch external declarations */
+extern IMUManager imu;
+extern bool isZeroing;
+extern Utilities::MotorInfo motorStatus;
+extern bool needToZero;
+extern bool readyToRun;
+
+/* motor sketch external declarations */
+extern char motorWASD;
+extern float motorInfo[4];
+
+
+/* AP sketch external declarations */
+#define MAIN_LED_PIN RED_LED
+extern float driveRate;
+extern int angleToTurn;
diff --git a/src/Energia/libraries/ZumoCC3200/examples/CommandExample/MotorControlLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/CommandExample/MotorControlLoop.ino
new file mode 100644
index 0000000..aac92e4
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/CommandExample/MotorControlLoop.ino
@@ -0,0 +1,91 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorWASD, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w' - drive forward
+ *      's' - drive backward
+ *      'a' - turn left
+ *      'd' - turn right
+ *      ' ' - stop
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorWASD.
+ */
+#include 
+#include 
+#include 
+#include 
+#include 
+#include 
+#include 
+#include 
+
+#define PERIOD  0           /* period of motor control updates */
+
+char motorWASD = ' '; /* current motor drive command */
+static ZumoMotors motors; /* Zumo motor driver provided by Pololu */
+Utilities util;
+CommandManager manager;
+Command* turnCommand;
+Command* driveCommand;
+Command* waitCommand;
+Utilities::MotorInfo motorStatus;
+
+/*
+ *  ======== motorSetup ========
+ */
+void motorSetup(void)
+{
+    Serial.begin(9600);
+    Serial.println("motorSetup ...");
+
+    /* initialize the Pololu driver motor library */
+    ZumoMotors::setRightSpeed(0);
+    ZumoMotors::setLeftSpeed(0);
+    /* setup an LED to indcate forward/backward movement or turning */
+    pinMode(MAIN_LED_PIN, OUTPUT);
+
+    util = Utilities();
+    manager = CommandManager();
+    turnCommand = (Command*) new TurnAngleCommand(-90, 2.5f);
+    driveCommand = (Command*) new DriveLineCommand(0.5f, 3.0f);
+    waitCommand = (Command*) new WaitCommand(1.0f);
+    manager.addCommand(driveCommand);
+    manager.addCommand(waitCommand);
+    manager.addCommand(turnCommand);
+    manager.addCommand(waitCommand);
+    manager.addCommand(driveCommand);
+    manager.addCommand(waitCommand);
+    manager.addCommand(turnCommand);
+    manager.addCommand(waitCommand);
+    manager.addCommand(driveCommand);
+    manager.addCommand(waitCommand);
+    manager.addCommand(turnCommand);
+    manager.addCommand(waitCommand);
+    manager.addCommand(driveCommand);
+    motorStatus.error = 0;
+    motorStatus.leftSpeed = 0;
+    motorStatus.rightSpeed = 0;
+    motorStatus.time = 0;
+    Serial.println("motorSetup done.");
+}
+
+/*
+ *  ======== motorLoop ========
+ *
+ *  Based on user input, drive open loop or
+ *  perform closed-loop maneuvers
+ */
+void motorLoop(void)
+{
+    if (isZeroing) {
+        motors.setLeftSpeed(0);
+        motors.setRightSpeed(0);
+    }
+
+    else {
+        manager.run(motorStatus);
+    }
+    delay(25);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/CommandExample/SensorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/CommandExample/SensorLoop.ino
new file mode 100644
index 0000000..a6a41c8
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/CommandExample/SensorLoop.ino
@@ -0,0 +1,70 @@
+/*
+ *  ======== imuLoop ========
+ *  This sketch simply reads the Zumo IMU sensors at 200Hz and prints
+ *  the current value once per second to the UART.
+ */
+
+#include 
+
+/* Pololu IMU data instance objects */
+IMUManager imu;
+float angle = 0;
+int dc_offset = 0;
+float noise = 0;
+bool hasCalibrated = 0;
+bool needToZero = false; /* set by WiFi to recalibrate Gyro */
+bool isZeroing = true;   /* flag indicating we are in "zero'ing mode */
+
+/*
+ *  ======== sensorSetup ========
+ */
+void sensorSetup()
+{
+    Serial.begin(9600);
+    Serial.println("sensorSetup ...");
+    Wire.begin();
+    imu = IMUManager();
+
+    /* initialize Zumo accelerometer and magnetometer */
+    imu.initAccel();
+    imu.enableAccelDefault();
+
+    /* initialize Zumo gyro */
+    if (!imu.initGyro()) {
+        Serial.print("Failed to autodetect gyro type!");
+        delay(1000);
+    }
+    imu.enableGyroDefault();
+
+    isZeroing = true;
+    imu.calibrateGyro(2);
+    imu.zeroGyroXAxis();
+    imu.zeroGyroYAxis();
+    imu.zeroGyroZAxis();
+    isZeroing = false;
+
+    Serial.println("sensorSetup done.");
+}
+
+/*
+ *  ======== sensorLoop ========
+ */
+void sensorLoop()
+{
+    static int imuCount = 0;
+
+    /* update IMU data every 10 ms (100 Hz) */
+    if (needToZero) {
+        imu.zeroGyroZAxis();
+        imu.zeroGyroYAxis();
+        needToZero = false;
+    }
+
+    /* read data from all IMU sensors */
+    imu.readGyro();
+    imu.readAccel();
+    imu.readMag();
+    
+    delay(10);
+    imuCount++;
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/CommandExample/WifiLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/CommandExample/WifiLoop.ino
new file mode 100644
index 0000000..a31c069
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/CommandExample/WifiLoop.ino
@@ -0,0 +1,199 @@
+/*
+ *  ======== wifiLoop ========
+ *  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 
+#include 
+
+#include 
+
+/* name of the network and its password */
+static const char ssid[] = "zumo-command";
+static const char wifipw[] = "password";
+
+/* port number of the server listening for commands at 192.168.1.1 */
+#define PORTNUM 8080
+
+float driveRate;
+int angleToTurn;
+char lastCmd;
+bool readyToRun;
+
+/* create data server on port PORTNUM */
+static WiFiServer server(PORTNUM);
+
+static void doWASD(char wasd, WiFiClient client);
+
+/*
+ *  ======== wifiSetup ========
+ */
+void wifiSetup()
+{
+    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);
+    readyToRun = false;
+}
+
+static int readBuf(WiFiClient client, uint8_t *buf, unsigned int len)
+{
+    /* wait for len characters to arrive */
+    for (unsigned int i = 0; client.available() < len; i++) {
+        /* yield or sleep for an ever increasing period to save power */
+        i > 0 ? delay(i) : Task_yield();
+
+        if (!client.connected()) {
+            return -1;
+        }
+    }
+    client.read(buf, len);
+    return 0;
+}
+
+/*
+ *  ======== wifiLoop ========
+ */
+void wifiLoop()
+{
+    /* 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) {
+            readyToRun = true;
+
+            /* if there's a client, read and process commands */
+            static char buffer[64] = { 0 };
+            int bufLen = 0;
+            doWASD(' ', client);
+
+            /* 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 (c == 'g') {
+                        uint8_t cmdBuf[2] = { 0 };
+                        if (readBuf(client, cmdBuf, sizeof(cmdBuf)) < 0) {
+                            break;
+                        }
+                        processDriveCommand(cmdBuf);
+                        doWASD('g', client);
+                    }
+                    else if (c == 't') {
+                        Serial.println("Received t, going to send IMU data");
+                        uint8_t cmdBuf[3] = { 0 };
+                        if (readBuf(client, cmdBuf, sizeof(cmdBuf)) < 0) {
+                            break;
+                        }
+                        processTurnCommand(cmdBuf);
+                        doWASD('t', client);
+                    }
+                    /* if there's a new line, we have a complete command */
+                    if (c == '\n') {
+                        doWASD(buffer[0], 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 */
+            motorWASD = ' ';
+        }
+    }
+
+    /* check for new connections 2 times per second */
+    delay(500);
+}
+
+/*
+ *  ======== doWASD ========
+ */
+static void doWASD(char wasd, WiFiClient client)
+{
+    static char report[124];
+
+    if (wasd == 'z' && lastCmd != 'z') {
+        //Serial.println("Z received");
+        needToZero = true;
+    }
+    else {     /* set new motor command */
+        motorWASD = wasd;
+    }
+    lastCmd = wasd;
+
+    int angle = (int16_t) imu.getGyroYaw() * 100;
+    int error = motorStatus.error * 100;
+    int leftSpeed = motorStatus.leftSpeed * 100;
+    int rightSpeed = motorStatus.rightSpeed * 100;
+    long time = millis();
+
+    /* send current IMU data */
+    int len = System_snprintf(report, sizeof(report),
+                    "A: %6d %6d %6d G: %6d %6d %6d M: %6d %6d %6d I: %6d U: %6d %6d %6d %6d",
+                    imu.accel_x, imu.accel_y, imu.accel_z,
+                    imu.gyro_x,  imu.gyro_y,  imu.gyro_z,
+                    imu.mag_x,   imu.mag_y,   imu.mag_z,
+                    angle,
+                    error, leftSpeed, rightSpeed, time);
+    if (client.write((unsigned char *)report, 120) != 120) {
+        Serial.println("Error: reply failed, status != 120");
+    }
+}
+
+static void processDriveCommand(uint8_t cmd[])
+{
+    float rate = (cmd[1] / 100.0f);
+    driveRate = rate;
+    Serial.print("driveRate: "); Serial.println(driveRate);
+}
+
+static void processTurnCommand(uint8_t cmd[])
+{
+    int combined = (cmd[2] << 8) | cmd[1];
+    if (cmd[0]) {
+        combined *= -1;
+    }
+    angleToTurn = combined;
+    Serial.print("Turn angle: "); Serial.println(angleToTurn);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/ManualDrive.ino b/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/ManualDrive.ino
new file mode 100644
index 0000000..20fe4dd
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/ManualDrive.ino
@@ -0,0 +1,30 @@
+/*  ======== ManualDrive.ino ========
+ *  Simple demo of the CC3200 Zumo platform's manual drive
+ *
+ *  This example creates an access point named zumo-manual 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.
+ *
+ *  Two examples of such a client are provided in the src/Processing
+ *  sub-directory of git@gitorious.design.ti.com:sb/zumo.git: zgraph and
+ *  zecho.  Both are Processing applications (the development IDE that
+ *  originally inspired the Energia/Wiring development environment; see
+ *  https://processing.org).
+ */
+
+#include 
+#include 
+#include 
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G    imuGyro;    /* gyro data */
+
+/* motor sketch external declarations */
+extern char motorWASD;
+
+/* ap sketch external declarations */
+#define MAIN_LED_PIN RED_LED
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/apLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/apLoop.ino
new file mode 100644
index 0000000..5b9747a
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/apLoop.ino
@@ -0,0 +1,123 @@
+/*
+ *  ======== 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 
+#include 
+
+#include 
+
+/* name of the network and its password */
+static const char ssid[] = "zumo-manual";
+static const char wifipw[] = "password";
+
+/* port number of the server listening for commands at 192.168.1.1 */
+#define PORTNUM 8080
+
+/* create data server on port PORTNUM */
+static WiFiServer server(PORTNUM);
+
+static void doWASD(char wasd, 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') {
+                        doWASD(buffer[0], 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 */
+            motorWASD = ' ';
+        }
+    }
+
+    /* check for new connections 2 times per second */
+    delay(500);
+}
+
+/*
+ *  ======== doWASD ========
+ */
+static void doWASD(char wasd, WiFiClient client)
+{
+    static char report[80];
+
+    /* set new motor command */
+    motorWASD = wasd;
+
+    /* 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/libraries/ZumoCC3200/examples/ManualDrive/imuLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/imuLoop.ino
new file mode 100644
index 0000000..be55408
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/imuLoop.ino
@@ -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 
+
+#include 
+#include 
+
+/* 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/libraries/ZumoCC3200/examples/ManualDrive/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/motorLoop.ino
new file mode 100644
index 0000000..69b2dd1
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/motorLoop.ino
@@ -0,0 +1,170 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorWASD, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w' - drive forward
+ *      's' - drive backward
+ *      'a' - turn left
+ *      'd' - turn right
+ *      ' ' - stop
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorWASD.
+ */
+#include 
+
+#include 
+
+#define PERIOD  1           /* period of motor control updates */
+
+char motorWASD = ' ';       /* current motor drive command */
+
+static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
+
+static int clip(int speed);
+static void drive(char wasd, int goal, unsigned int duration);
+static int next(int cur, int goal);
+
+/*
+ *  ======== 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 (motorWASD) {
+        case 's':
+        case 'w': {
+            /* illuminate LED while driving */
+            digitalWrite(MAIN_LED_PIN, HIGH);
+            drive(motorWASD, 200, PERIOD);
+            break;
+        }
+
+        case 'd':
+        case 'a': {
+            /* blink LED while turning */
+            if (count == ((count / 100) * 100)) {
+                state ^= 1;
+                digitalWrite(MAIN_LED_PIN, state);
+            }   
+            drive(motorWASD, 100, PERIOD);
+            break;
+        }
+
+        default: {
+            /* turn off LED while stopped */
+            motorWASD = ' ';
+            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 ========
+ *  Drive motors in the direction and speed specified by wasd and goal
+ *
+ *  Note: negative goal values imply a reversal of the wasd direction
+ */
+static void drive(char wasd, int goal, unsigned int duration)
+{
+    static int leftSpeed = 0;
+    static int rightSpeed = 0;
+    
+    while (duration > 0) {
+        duration--;
+
+        /* gradually adjust curent speeds to goal */
+        switch (wasd) {
+            case ' ': { /* stop */
+                leftSpeed = next(leftSpeed, 0);
+                rightSpeed = next(rightSpeed, 0);
+                break;
+            }
+            case 'w': { /* forward */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            case 's': { /* backward */
+                leftSpeed = next(leftSpeed, -goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'd': { /* turn right */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'a': { /* turn left */
+                leftSpeed = next(leftSpeed, -goal);;
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            default: {
+                break;
+            }
+        }
+
+        /* clip speeds to allowable range */
+        leftSpeed = clip(leftSpeed);
+        rightSpeed = clip(rightSpeed);
+    
+        /* set motor speeds */
+        ZumoMotors::setLeftSpeed(leftSpeed);
+        ZumoMotors::setRightSpeed(rightSpeed);
+
+        /* sleep for 1 ms (so duration is in milliseconds) */
+        delay(1);
+    }
+}
+
+/*
+ *  ======== next ========
+ *  Compute the next motor speed value given the cur speed and a new goal
+ */
+static int next(int cur, int goal)
+{
+    int tmp = (goal - cur) * 0.0625f + cur;
+    return (tmp == cur ? goal : tmp);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/MasterSlave.ino b/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/MasterSlave.ino
new file mode 100644
index 0000000..9e0c952
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/MasterSlave.ino
@@ -0,0 +1,30 @@
+/*  ======== MasterSlave.ino ========
+ *  Simple demo of the CC3200 Zumo platform basics
+ *
+ *  This example creates an access point named zumo_group 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.
+ *
+ *  Two examples of such a client are provided in the src/Processing
+ *  sub-directory of git@gitorious.design.ti.com:sb/zumo.git: zgraph and
+ *  zecho.  Both are Processing applications (the development IDE that
+ *  originally inspired the Energia/Wiring development environment; see
+ *  https://processing.org).
+ */
+
+#include 
+#include 
+#include 
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G    imuGyro;    /* gyro data */
+
+/* motor sketch external declarations */
+extern char motorWASD;
+
+/* ap sketch external declarations */
+#define MAIN_LED_PIN RED_LED
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/apLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/apLoop.ino
new file mode 100644
index 0000000..46e1c20
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/apLoop.ino
@@ -0,0 +1,216 @@
+/*
+ *  ======== apLoop ========
+ *  This sketch waits on either a button press or a 
+ *
+ *  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 
+#include 
+#include 
+
+#include 
+
+/* name of the network and its password */
+char ssid[] = "zumo_group";
+char wifipw[] = "password";
+
+/* zumo command port */
+#define CMD_PORT 8080
+
+/* IMU data port */
+#define DATA_PORT 6000
+
+/* Create a Pushbutton object for pin 12 (the Zumo user pushbutton pin) */
+Pushbutton button(ZUMO_BUTTON);
+
+IPAddress broadcastIP(192,168,1,255);
+
+
+static bool waiting = true;
+
+/* 1 is master, 0 is slave, -1 is unassigned */
+int rank = -1;
+
+static void sendIMUPacket(WiFiUDP Udp, IPAddress ip, int localPort);
+
+//WiFiUDP UdpRX; /* UDP connection for receiving */ 
+//WiFiUDP UdpTX; /* UDP connection for transmitting */
+
+WiFiUDP Udp;
+
+/*
+ *  ======== apSetup ========
+ */
+void apSetup()
+{
+    Serial.begin(9600);
+
+    /* set priority of this task to be lower than other tasks */
+    //Task_setPri(Task_self(), 1);
+    
+    // attempt to connect to group network:
+    Serial.print("Attempting to connect to Network named: ");
+    Serial.println(ssid); 
+    WiFi.begin(ssid, wifipw);
+    
+    while (waiting) {
+ 
+      /* if group network is found */
+      if(WiFi.status() == WL_CONNECTED){
+         Serial.println("Connected to the group network");
+         Serial.println("Waiting for an ip address");
+  
+         while (WiFi.localIP() == INADDR_NONE) {
+           // print dots while we wait for an ip addresss
+           Serial.print(".");
+           delay(300);
+         }
+         
+         Serial.println("\nIP Address obtained");
+         printWifiStatus();
+         
+         /* this zumo is a slave */
+         rank = 0;
+         
+         waiting = false;
+      }
+      /* if zumo button is pressed */
+      else if(button.getSingleDebouncedRelease()){
+         Serial.println("Button press detected");
+         /* 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);
+         }
+         
+         /* this zumo is the master */
+         rank = 1;
+         
+         waiting = false;
+      }
+      else{
+         WiFi.begin(ssid, wifipw);
+         Serial.print(".");
+         delay(300);
+      }
+    }
+    
+    Udp.begin(CMD_PORT);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              
+}
+
+/*
+ *  ======== apLoop ========
+ */
+ 
+void apLoop()
+{
+  int packetSize = 0;
+  
+  switch(rank){
+      case -1: /* unassigned case: should theoretically never happen */
+        Serial.println("rank was not assigned, please exit the program and try again");
+        break;
+        
+      case 0: /* slave case: listen for motor commands from group network */
+        // if there's data available, read a packet
+        packetSize = Udp.parsePacket();
+        if (packetSize){
+           static char buffer[64] = {0}; 
+          
+           Serial.print("Received packet from ");
+           IPAddress remoteIp = Udp.remoteIP();
+           Serial.print(remoteIp);
+           Serial.print(", port ");
+           Serial.println(Udp.remotePort());
+        
+           /* if packet is from master */
+           if(Udp.remotePort() == CMD_PORT){
+             // read the packet into packetBufffer
+             int len = Udp.read(buffer, 64);
+             
+             if (len > 0) buffer[len] = 0;
+             Serial.print("Contents: ");
+             Serial.println(buffer);
+             
+             motorWASD = buffer[0];  
+           }  
+           
+        }      
+        break;
+        
+      case 1: /* master case: listen for motor commands from processing, and multicast them over the group network */
+        // if there's data available, read a packet
+        packetSize = Udp.parsePacket();
+        if (packetSize){
+           static char buffer[64] = {0}; 
+          
+           Serial.print("Received packet from ");
+           IPAddress remoteIp = Udp.remoteIP();
+           Serial.print(remoteIp);
+           Serial.print(", port ");
+           Serial.println(Udp.remotePort());
+           
+           /* if packet is from processing */
+           if(Udp.remotePort() == DATA_PORT){
+             // read the packet into packetBufffer
+             int len = Udp.read(buffer, 64);
+             
+             if (len > 0) buffer[len] = 0;
+             Serial.print("Contents: ");
+             Serial.println(buffer);
+             
+             motorWASD = buffer[0];  
+             
+             /* broadcast the command */
+             Udp.beginPacket(broadcastIP, CMD_PORT);
+             Udp.write(motorWASD);
+             Udp.endPacket();
+           }
+        }
+        /* broadcast the command */
+        Udp.beginPacket(broadcastIP, CMD_PORT);
+        Udp.write("hello slaves");
+        Udp.endPacket();
+        break;
+  }
+}
+
+/*
+ *  ======== doWASD ========
+ */
+static void sendIMUPacket(WiFiUDP Udp, IPAddress ip, int localPort)
+{
+    static char report[80];
+
+    /* 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);
+    
+    Udp.beginPacket(ip, localPort);
+    Udp.write(report);
+    Udp.endPacket();
+}
+
+void printWifiStatus() {
+  // print the SSID of the network you're attached to:
+  Serial.print("SSID: ");
+  Serial.println(WiFi.SSID());
+
+  // print your WiFi IP address:
+  IPAddress ip = WiFi.localIP();
+  Serial.print("IP Address: ");
+  Serial.println(ip);
+
+  // print the received signal strength:
+  long rssi = WiFi.RSSI();
+  Serial.print("signal strength (RSSI):");
+  Serial.print(rssi);
+  Serial.println(" dBm");
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/imuLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/imuLoop.ino
new file mode 100644
index 0000000..be55408
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/imuLoop.ino
@@ -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 
+
+#include 
+#include 
+
+/* 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/libraries/ZumoCC3200/examples/MasterSlave/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/motorLoop.ino
new file mode 100644
index 0000000..e7705e1
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/motorLoop.ino
@@ -0,0 +1,169 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorWASD, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w' - drive forward
+ *      's' - drive backward
+ *      'a' - turn left
+ *      'd' - turn right
+ *      ' ' - stop
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorWASD.
+ */
+#include 
+
+#include 
+
+#define PERIOD  1           /* period of motor control updates */
+
+char motorWASD = ' ';       /* current motor drive command */
+
+static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
+
+static int clip(int speed);
+static void drive(char wasd, 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 (motorWASD) {
+        case 's':
+        case 'w': {
+            /* illuminate LED while driving */
+            digitalWrite(MAIN_LED_PIN, HIGH);
+            drive(motorWASD, 200, PERIOD);
+            break;
+        }
+
+        case 'd':
+        case 'a': {
+            /* blink LED while turning */
+            if (count == ((count / 100) * 100)) {
+                state ^= 1;
+                digitalWrite(MAIN_LED_PIN, state);
+            }   
+            drive(motorWASD, 100, PERIOD);
+            break;
+        }
+
+        default: {
+            /* turn off LED while stopped */
+            motorWASD = ' ';
+            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 ========
+ *  Drive motors in the direction and speed specified by wasd and goal
+ *
+ *  Note: negative goal values imply a reversal of the wasd direction
+ */
+static void drive(char wasd, int goal, unsigned int duration)
+{
+    static int leftSpeed = 0;
+    static int rightSpeed = 0;
+    
+    while (duration > 0) {
+        duration--;
+
+        /* gradually adjust curent speeds to goal */
+        switch (wasd) {
+            case ' ': { /* stop */
+                leftSpeed = next(leftSpeed, 0);
+                rightSpeed = next(rightSpeed, 0);
+                break;
+            }
+            case 'w': { /* forward */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            case 's': { /* backward */
+                leftSpeed = next(leftSpeed, -goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'd': { /* turn right */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'a': { /* turn left */
+                leftSpeed = next(leftSpeed, -goal);;
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            default: {
+                break;
+            }
+        }
+
+        /* clip speeds to allowable range */
+        leftSpeed = clip(leftSpeed);
+        rightSpeed = clip(rightSpeed);
+    
+        /* set motor speeds */
+        ZumoMotors::setLeftSpeed(leftSpeed);
+        ZumoMotors::setRightSpeed(rightSpeed);
+
+        /* sleep for 1 ms (so duration is in milliseconds) */
+        delay(1);
+    }
+}
+
+/*
+ *  ======== next ========
+ *  Compute the next motor speed value given the cur speed and a new goal
+ */
+static int next(int cur, int goal)
+{
+    int tmp = (goal - cur) * 0.0625f + cur;
+    return (tmp == cur ? goal : tmp);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Multicast/MCP.cpp b/src/Energia/libraries/ZumoCC3200/examples/Multicast/MCP.cpp
new file mode 100644
index 0000000..ae3afb9
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Multicast/MCP.cpp
@@ -0,0 +1,25 @@
+#include 
+#include 
+#include 
+
+#include "MCP.h"
+#include 
+
+/* multicast address and port */
+IPAddress MC_IP(226, 1, 1, 1);
+int MC_PORT = 1717;
+
+WiFiUDP Udp;
+
+void MCP_init()
+{
+    Udp.begin(MC_PORT);    
+}
+
+int MCP_printf(char *message)
+{
+    Udp.beginPacket(MC_IP, MC_PORT);
+    Udp.write(message);
+    Udp.endPacket();
+}
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Multicast/MCP.h b/src/Energia/libraries/ZumoCC3200/examples/Multicast/MCP.h
new file mode 100644
index 0000000..804659c
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Multicast/MCP.h
@@ -0,0 +1,8 @@
+#ifndef MCP_h
+#define MCP_h
+
+extern void MCP_init();
+
+extern int MCP_printf(char *message);
+
+#endif /* MCP_h */
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Multicast/Multicast.ino b/src/Energia/libraries/ZumoCC3200/examples/Multicast/Multicast.ino
new file mode 100644
index 0000000..ccd053f
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Multicast/Multicast.ino
@@ -0,0 +1,22 @@
+/*  ======== Multicast.ino ========
+ *  Simple demo of the CC3200 Zumo platform basics
+ *
+ *  This example creates an access point named zumo-multicast with password
+ *  "password" and a simple command/telemetry server on port 8080.
+ */
+
+#include 
+#include 
+#include 
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G    imuGyro;    /* gyro data */
+
+/* motor sketch external declarations */
+extern char motorWASD;
+
+/* ap sketch external declarations */
+#define MAIN_LED_PIN RED_LED
+extern long signalStrength;
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Multicast/apLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/Multicast/apLoop.ino
new file mode 100644
index 0000000..c0174d2
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Multicast/apLoop.ino
@@ -0,0 +1,104 @@
+/*
+ *  ======== apLoop ========
+ *  This sketch waits on either a button press or a 
+ *
+ *  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 
+#include 
+#include 
+#include "MCP.h"
+
+#include 
+#include 
+#include 
+
+/* name of the network and its password */
+char ssid[] = "zumo-multicast";
+char wifipw[] = "password";
+
+/* shared network */
+char netID[] = "TINK-NET";
+
+/* zumo command port */
+#define CMD_PORT 8080
+
+/* IMU data port */
+#define DATA_PORT 6000
+
+IPAddress broadcastIP(192,168,1,255);
+
+long signalStrength = 0;
+
+/*
+ *  ======== apSetup ========
+ */
+void apSetup()
+{
+    Serial.begin(9600);
+
+    /* set priority of this task to be lower than other tasks */
+    //Task_setPri(Task_self(), 1);
+    
+    // attempt to connect to Wifi network:
+    Serial.print("Connecting to target: ");
+    // print the network name (SSID);
+    Serial.println(netID); 
+    // Connect to target zumo
+    //WiFi.begin(ssid, wifipw);
+    WiFi.begin(netID);
+    while ( WiFi.status() != WL_CONNECTED) {
+      // print dots while we wait to connect
+      Serial.print(".");
+      delay(300);
+    }
+    
+    Serial.println("Connected");
+    Serial.println("Waiting for an ip address");
+    
+    while (WiFi.localIP() == INADDR_NONE) {
+      // print dots while we wait for an ip addresss
+      Serial.print(".");
+      delay(300);
+    }
+  
+    Serial.println("\nIP Address obtained");
+    printWifiStatus();
+    
+    MCP_init();
+                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      
+}
+
+/*
+ *  ======== apLoop ========
+ */
+ 
+void apLoop()
+{  
+    signalStrength = WiFi.RSSI();
+    Serial.println(signalStrength);
+    static char buf[16] = {0};
+    snprintf(buf, sizeof(buf), "%d", signalStrength);
+    MCP_printf(buf); 
+
+    delay(1000);    
+}
+
+void printWifiStatus() {
+    // print the SSID of the network you're attached to:
+    Serial.print("SSID: ");
+    Serial.println(WiFi.SSID());
+
+    // print your WiFi IP address:
+    IPAddress ip = WiFi.localIP();
+    Serial.print("IP Address: ");
+    Serial.println(ip);
+
+    // print the received signal strength:
+    long rssi = WiFi.RSSI();
+    Serial.print("signal strength (RSSI):");
+    Serial.print(rssi);
+    Serial.println(" dBm");
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Multicast/imuLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/Multicast/imuLoop.ino
new file mode 100644
index 0000000..be55408
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Multicast/imuLoop.ino
@@ -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 
+
+#include 
+#include 
+
+/* 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/libraries/ZumoCC3200/examples/Multicast/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/Multicast/motorLoop.ino
new file mode 100644
index 0000000..69b2dd1
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/Multicast/motorLoop.ino
@@ -0,0 +1,170 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorWASD, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w' - drive forward
+ *      's' - drive backward
+ *      'a' - turn left
+ *      'd' - turn right
+ *      ' ' - stop
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorWASD.
+ */
+#include 
+
+#include 
+
+#define PERIOD  1           /* period of motor control updates */
+
+char motorWASD = ' ';       /* current motor drive command */
+
+static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
+
+static int clip(int speed);
+static void drive(char wasd, int goal, unsigned int duration);
+static int next(int cur, int goal);
+
+/*
+ *  ======== 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 (motorWASD) {
+        case 's':
+        case 'w': {
+            /* illuminate LED while driving */
+            digitalWrite(MAIN_LED_PIN, HIGH);
+            drive(motorWASD, 200, PERIOD);
+            break;
+        }
+
+        case 'd':
+        case 'a': {
+            /* blink LED while turning */
+            if (count == ((count / 100) * 100)) {
+                state ^= 1;
+                digitalWrite(MAIN_LED_PIN, state);
+            }   
+            drive(motorWASD, 100, PERIOD);
+            break;
+        }
+
+        default: {
+            /* turn off LED while stopped */
+            motorWASD = ' ';
+            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 ========
+ *  Drive motors in the direction and speed specified by wasd and goal
+ *
+ *  Note: negative goal values imply a reversal of the wasd direction
+ */
+static void drive(char wasd, int goal, unsigned int duration)
+{
+    static int leftSpeed = 0;
+    static int rightSpeed = 0;
+    
+    while (duration > 0) {
+        duration--;
+
+        /* gradually adjust curent speeds to goal */
+        switch (wasd) {
+            case ' ': { /* stop */
+                leftSpeed = next(leftSpeed, 0);
+                rightSpeed = next(rightSpeed, 0);
+                break;
+            }
+            case 'w': { /* forward */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            case 's': { /* backward */
+                leftSpeed = next(leftSpeed, -goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'd': { /* turn right */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'a': { /* turn left */
+                leftSpeed = next(leftSpeed, -goal);;
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            default: {
+                break;
+            }
+        }
+
+        /* clip speeds to allowable range */
+        leftSpeed = clip(leftSpeed);
+        rightSpeed = clip(rightSpeed);
+    
+        /* set motor speeds */
+        ZumoMotors::setLeftSpeed(leftSpeed);
+        ZumoMotors::setRightSpeed(rightSpeed);
+
+        /* sleep for 1 ms (so duration is in milliseconds) */
+        delay(1);
+    }
+}
+
+/*
+ *  ======== next ========
+ *  Compute the next motor speed value given the cur speed and a new goal
+ */
+static int next(int cur, int goal)
+{
+    int tmp = (goal - cur) * 0.0625f + cur;
+    return (tmp == cur ? goal : tmp);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/MotionPlanner.cpp b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/MotionPlanner.cpp
new file mode 100644
index 0000000..25a0f27
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/MotionPlanner.cpp
@@ -0,0 +1,301 @@
+#include 
+#include 
+#include 
+
+#include "MotionPlanner.h"
+
+#include 
+#include 
+#include 
+#include 
+#include 
+
+MotionPlanner::MotionPlanner()
+{
+    dataReceived  = false;
+    trajCalculated = false;
+    iteration = 0;
+}
+
+/*
+ *  ======== setPoints ========
+ *  update the list of points and reset the trajectory and iteration
+ *
+ */
+void MotionPlanner::setPoints(std::vector inputPoints)
+{ 
+    /* ensure that input set has equal number of x and y values */
+    assert(inputPoints.size() % 2 == 0);
+  
+    iteration = 0;
+    points.clear();
+    trajectory.clear();
+  
+    dataReceived = true;
+  
+    /* convert each pair of entries of inputPoints into a Point and add it to
+     * points
+     */
+    for (int i = 0; i < inputPoints.size(); i += 2) {
+        MotionPlanner::Point newPoint = {inputPoints[i], inputPoints[i+1]};
+        points.push_back(newPoint);
+    }
+}
+
+/*
+ *  ======== generateSpline ========
+ *  Generate a single spline and calculate the corresponding trajectory
+ *
+ *  Since length is determined by iterations (which translates to drive time),
+ *  this algorithm treats each segment of the spline as equal "length" since it
+ *  allocates an equal number of iterations to the trajectory vector for each
+ *  segment, even if the actual distance between segments is different.
+ */
+void MotionPlanner::generateSpline()
+{
+    /* if points vector contains points */
+    if (dataReceived) {
+        /* manipulate data and feed it into the spline class */
+        int length = points.size(); //number of points
+    
+        int orderedX[length], orderedY[length]; /* x and y points in order of trajectory */
+    
+        for (int i = 0; i < length; i++) {
+            orderedX[i] = points[i].x;
+            orderedY[i] = points[i].y;
+        }
+    
+        int sortedX[length], sortedY[length]; /* x is sorted, y corresponds to x */
+    
+        for (int i = 0; i < length; i++) {
+            sortedX[i] = orderedX[i];
+            sortedY[i] = orderedY[i];  
+        }
+    
+        /* sort the X and Y data sets */
+        slowSort(sortedX, sortedY, length);
+
+        std::vector X(length), Y(length); /* inputs to the spline */
+        for (int i =0; i < length; i++) {
+            X[i] = (double) sortedX[i];
+            Y[i] = (double) sortedY[i];  
+        }
+    
+        /* create the spline */
+        tk::spline s;
+        s.set_points(X, Y);
+    
+        /* use spline to calculate a series of desired headings */
+    
+        /* for each segment of the spline */
+        for (int i = 0; i < (length-1); i++) { /* numbers of segments = number of points - 1 */
+      
+            /* split the segment into *resolution* number of intervals */
+            double interval = (orderedX[i+1]-orderedX[i])/((double) RESOLUTION);
+      
+            /* poll for the discrete time rate of change at evenly spaced intervals */
+            for (int j = 0; j < RESOLUTION; j++) { 
+                Serial.print(orderedX[i]+j*interval);Serial.print(", ");Serial.println(s(orderedX[i]+j*interval));
+        
+                double slope = (s(orderedX[i]+(j+1)*interval)-s(orderedX[i]+j*interval))/interval;
+                double heading = Utilities::toDegrees(atan2(s(orderedX[i]+(j+1)*interval)-s(orderedX[i]+j*interval),interval));
+                trajectory.push_back(Utilities::wrapAngle(heading-90));
+            }
+            Serial.println("");
+        }
+        Serial.print("trajectory data points: ");Serial.println(trajectory.size());
+        trajCalculated = true;
+    }
+    else {
+        Serial.println("no data to generate spline with"); 
+    }
+}
+
+/*
+ *  ======== followSpline ========
+ *  Drive the zumo along the calculated trajectory  
+ */
+void MotionPlanner::followSpline(ZumoMotors motors, PIDController pid, double rate)
+{    
+    if (iteration < trajectory.size()) {
+        double error = Utilities::wrapAngle(IMUManager::getGyroYaw() - trajectory[iteration]); 
+        Serial.print(trajectory[iteration]);Serial.print(", ");Serial.print(IMUManager::getGyroYaw());Serial.print(", ");Serial.println(iteration);
+        float power = pid.calculate(error);
+        power = Utilities::saturate(power, rate - 1.0, 1.0 - rate);
+       
+        float lTotal = Utilities::saturate((rate + power) * 400, -400, 400);
+        float rTotal = Utilities::saturate((rate - power) * 400, -400, 400);
+     
+        motors.setLeftSpeed(lTotal);
+        motors.setRightSpeed(rTotal); 
+        iteration++;
+    }
+    else {
+        motors.setLeftSpeed(0);
+        motors.setRightSpeed(0); 
+    }
+    delay(1);
+}
+
+/*
+ *  ======== intervalInterpolate ========
+ *  Given an interval and two tangents, create a spline
+ *
+ *  leftPoint.x < rightPoint.x
+ */
+tk::spline MotionPlanner::intervalInterpolate(
+    MotionPlanner::Point leftPoint,
+    MotionPlanner::Point rightPoint, 
+    double leftTangent, double rightTangent)
+{
+}
+
+/*
+ *  ======== generateTrajectory ========
+ *  Calculate a spline for each interval between control points
+ */
+void MotionPlanner::generateTrajectory()
+{
+    std::vector splineChain;
+  
+    for (int i = 0; i < (points.size()-1); i++) {
+        double t1, t2;
+     
+        tk::spline s;
+     
+        /* calculate tangents using a finite three-point difference */
+     
+        /* calculate Catmull-Rom spline tangents */
+     
+        if (points[i].x < points[i+1].x) {
+            s = intervalInterpolate(points[i], points[i+1], t1, t2); 
+            splineChain.push_back(s);
+        }
+        else if (points[i].x > points[i+1].x) {
+            s = intervalInterpolate(points[i+1], points[i], t2, t1); 
+            splineChain.push_back(s);
+        }
+        else {
+            /* x values are equal */
+        }
+     
+        double segmentLen = dist(points[i], points[i+1]);
+     
+        /* conversion: 800 pixels = 8 ft = 800 iterations 
+           1 pixel = 1 iteration */
+    }
+}
+
+
+/*
+ *  ======== generateParameterizedSpline ========
+ *  Calculate a spline for both X and Y data sets to create a trajectory
+ *
+ *  Parameterize the X and Y data sets according to a approximated distance
+ *  parameter t.
+ */
+void MotionPlanner::generateParameterizedSpline()
+{
+    /* if points vector contains points */
+    if (dataReceived) {
+        /* manipulate data and feed it into the spline class */
+        int length = points.size(); /* number of points */
+
+        std::vector X(length), Y(length); /* X and Y coordinates vectors */
+
+        /* initialize the vectors */
+        for (int i = 0; i < length; i++) {
+            X[i] = (double) points[i].x;
+            Y[i] = (double) points[i].y;  
+        }
+
+        std::vector t(length); /* vector of parameter t values, depende data set to X and Y splines */
+        double splineLength = 0.0; /* total length, sum of individual segment length */
+  
+        /* approximate the unitized arc length of each segment of the trajectory in order to assign 
+           the appropriate distance parameter value t to each point */
+
+        t[0] = 0.0; /* first value is always 0 */
+
+        for (int i = 1; i < length; i++) {
+
+            /* linear apporximation of the segment length */
+            double segLen = (double) dist(points[i-1], points[i]);
+            splineLength += segLen;
+      
+            /* i-th t parameter is the current spline length at the end of the i-th segment */
+            t[i] = splineLength;
+        }
+    
+    
+        /* create the X vs. t spline */
+        tk::spline xS;
+        xS.set_points(t, X);
+
+        /* create the Y vs. t spline */
+        tk::spline yS;
+        yS.set_points(t, Y);
+    
+        /* use splines to calculate a series of desired headings */
+    
+        /* for every 4 pixels of spline length, add a heading data point to the trajectory vector
+           this way, we end up with trajectory vector whose size corrsponds to the length of the spline */
+        for (double pos = 0.0; pos < splineLength; pos += INTERVAL_LENGTH) { 
+
+            /* print out the interpolated coordinate */
+            //Serial.print(xS(pos));Serial.print(", ");Serial.println(yS(pos));
+        
+            /* use trig to calculate the angle formed by the discrete rate of change vector WRTO normal */ 
+            double heading = Utilities::toDegrees(atan2(yS(pos) - yS(pos - INTERVAL_LENGTH), xS(pos) - xS(pos - INTERVAL_LENGTH)));
+
+            /* shift the heading 90 degrees counter-clockwise so that 0 is facing forward for the zumo */
+            trajectory.push_back(Utilities::wrapAngle(heading - 90));
+      
+        }
+        Serial.print("trajectory data points: ");Serial.println(trajectory.size());
+        trajCalculated = true;
+    }
+    else {
+        Serial.println("no data to generate spline with"); 
+    }
+}
+
+bool MotionPlanner::ready()
+{
+    return dataReceived && trajCalculated;
+}
+
+/*
+ *  ======== slowSort ========
+ *  Used to sort data before creating spline
+ *
+ *  Sorts vector X into increasing order, and reorders Y's elements to 
+ *  correspond to the sorted X
+ */
+void MotionPlanner::slowSort(int X[], int Y[], int length)
+{
+    int tempX[length];
+    for (int i = 0; i < length; i++) {
+        tempX[i] = X[i];
+    }
+    int tempY[length];
+    for (int i = 0; i < length; i++) {
+        tempY[i] = Y[i];
+    }
+    for (int i = 0; i < length; i++) {
+        int indexOfMin = 0;
+        for (int j = 0; j < length; j++) {
+            if (tempX[j] < tempX[indexOfMin]) {
+                indexOfMin = j;
+            }
+        }
+        X[i] = tempX[indexOfMin];
+        Y[i] = tempY[indexOfMin];
+        tempX[indexOfMin] = 1000;
+    }
+}
+
+double MotionPlanner::dist(MotionPlanner::Point a, MotionPlanner::Point b)
+{
+    return hypot(a.x - b.x, a.y - b.y);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/MotionPlanner.h b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/MotionPlanner.h
new file mode 100644
index 0000000..203b743
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/MotionPlanner.h
@@ -0,0 +1,49 @@
+#ifndef MotionPlanner_h
+#define MotionPlanner_h
+
+#include 
+
+#include 
+#include 
+#include 
+
+class MotionPlanner
+{
+  public:
+    struct Point {
+        double x;
+        double y; 
+    };
+
+    MotionPlanner();
+
+    void setPoints(std::vector inputPoints);
+    void generateSpline();
+    void followSpline(ZumoMotors motors, PIDController pid, double rate);
+    tk::spline intervalInterpolate(MotionPlanner::Point leftPoint, MotionPlanner::Point rightPoint, double leftTangent, double rightTangent);
+    void generateTrajectory();
+    void generateParameterizedSpline();
+    bool ready();
+
+  private:
+    int iteration; /* current iteration of followSpline */
+    std::vector points;
+    std::vector trajectory; /* list of headings to regulate to while driving spline */
+    bool dataReceived;
+    bool trajCalculated;
+
+    /* private member methods */
+
+    void slowSort(int X[], int Y[], int length); /* sort data before creating spline */
+    double dist(MotionPlanner::Point a, MotionPlanner::Point b);
+
+    /* constants */
+        
+    /* 800 pixels = 8 ft = 800 iterations -> 1 pixel = 1 iteration */
+    static const float INTERVAL_LENGTH = 1.0f; 
+
+    /* number of segments to evaulate between each pair of input points */
+    static const int RESOLUTION; 
+};   
+
+#endif /* MotionPlanner_h */
diff --git a/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/SplineDrive.ino b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/SplineDrive.ino
new file mode 100644
index 0000000..a8081c0
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/SplineDrive.ino
@@ -0,0 +1,31 @@
+/*  ======== SplineDrive.ino ========
+ *  This sketch interacts with a Processing application to enable
+ *  the Zumo to drive along a spline path specified by the user.
+ *   
+ *  The Processing client connects to the Zumo's network then sends 
+ *  list of points to create a trajectory from. The Zumo then uses
+ *  a cubic spline interpolation library to generate trajectory 
+ *  (a list of headings), and attempts to follow that trajectory. 
+ *
+ *  NOTE: Currently there are possible memory issues with large data
+ *  sets/spline curves that cause the program to crash mid-calculation
+ */
+
+#include 
+#include 
+#include 
+#include "MotionPlanner.h"
+#include 
+
+/* imu sketch external declarations */
+extern IMUManager imu;
+extern bool   isZeroing;
+
+/* motor sketch external declarations */
+extern Utilities util;
+
+/* AP sketch external declarations */
+#define MAIN_LED_PIN LED_LED
+extern int targetPoints[64];
+extern MotionPlanner mp;
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/apLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/apLoop.ino
new file mode 100644
index 0000000..4d819c2
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/apLoop.ino
@@ -0,0 +1,159 @@
+/*
+ *  ======== apLoop ========
+ *  This sketch starts a network and listens on port PORTNUM for
+ *  a new set of points from a Processing client.
+ *
+ *  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 
+#include 
+#include 
+#include "MotionPlanner.h"
+#include 
+#include 
+#include 
+#include 
+
+/* name of the network and its password */
+static const char ssid[] = "zumo-spline";
+static const char wifipw[] = "password";
+
+/* port number of the server listening for commands at 192.168.1.1 */
+#define PORTNUM 8080
+
+MotionPlanner mp;
+
+/* create data server on port PORTNUM */
+static WiFiServer server(PORTNUM);
+
+static int readString(WiFiClient client, char *buf);
+
+/*
+ *  ======== 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);
+    }
+    
+    mp = MotionPlanner();
+
+    /* 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, listen for incoming point data */
+            static int buffer[64] = {0};
+            static char temp[16] = {0};
+
+            int bufLen = 0;
+            
+            /* while connected to the client */
+            while (client.connected()) {
+
+                /* if there's a byte to read from the client .. */
+                if (client.available()) {
+                     
+                    char c = client.read();  
+                    
+                    /* if there's a new line, we have a complete set of points */
+                    if(c == '\n'){
+                        std::vector points(bufLen);
+                        for(int i=0; i < bufLen; i++){
+                           points[i] = buffer[i];
+                           if(i%2==0){
+                              Serial.print("(");Serial.print(buffer[i]);Serial.print(",");
+                           }
+                           else{
+                              Serial.print(buffer[i]);Serial.println(")");
+                           }
+                           
+                        } 
+                        mp.setPoints(points);
+                        mp.generateParameterizedSpline();
+
+                        /* reset command buffer index to get next command */
+                        bufLen = 0;
+                        memset(buffer, 0, sizeof(buffer));
+                        points.clear();                      
+                    }
+                    /* there's a new point, add it to the buffer */
+                    else if(c == 'p'){
+                        readString(client, temp); 
+                        
+                        int newPoint = atoi(temp);
+                        Serial.print("new point: ");
+                        Serial.println(newPoint);
+                        
+                        /* never overrun the buffer */
+                        if (bufLen >= (int)(sizeof (buffer))) { 
+                            bufLen = sizeof (buffer) - 1;
+                        }
+    
+                        buffer[bufLen++] = newPoint;    
+      
+                        memset(temp, 0, sizeof(temp));                  
+                    }
+
+                }
+
+            }
+
+            /* client disconnected or timed out, close the connection */
+            client.flush();
+            client.stop();
+
+        }
+
+    }
+
+    /* check for new connections 2 times per second */
+    delay(500);
+
+}
+
+/*
+ *  ======== readString ========
+ * read a null-terminated string from the client into a char buffer 
+ */
+static int readString(WiFiClient client, char *buf){
+    for (unsigned int i = 0; ; ) {
+      if(client.available()){
+        char c = client.read();
+        if(c != '\0'){
+          buf[i] = c;
+        }
+        else{
+          return 0;
+        }
+        i++;
+      }
+    }
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/imuLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/imuLoop.ino
new file mode 100644
index 0000000..4d7a366
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/imuLoop.ino
@@ -0,0 +1,68 @@
+/*
+*  ======== imuLoop ========
+*  This sketch simply reads the Zumo IMU sensors at 20Hz and prints
+*  the current value once per second to the UART.
+*/
+
+#include 
+
+/* Pololu IMU data instance objects */
+IMUManager imu;
+float angle = 0;
+int imuCount = 0;
+int dc_offset = 0;
+double noise = 0;
+
+bool hasCalibrated = 0;
+bool needToZero = false;
+bool isZeroing = false;
+
+/*
+*  ======== imuSetup ========
+*/
+
+void imuSetup()
+{
+    Serial.begin(9600);
+    Serial.println("imuSetup ...");
+    Wire.begin();
+
+    imu = IMUManager();
+
+    /* initialize Zumo accelerometer and magnetometer */
+    imu.initAccel();
+    imu.enableAccelDefault();
+
+    /* initialize Zumo gyro */
+    if (!imu.initGyro()) {
+        Serial.print("Failed to autodetect gyro type!");
+        delay(1000);
+    }
+    imu.enableGyroDefault();
+    Serial.println("imuSetup done.");
+    imuCount = 0;
+    
+    imu.calibrateGyro(2);
+    imu.zeroGyroXAxis();
+    imu.zeroGyroYAxis();
+    imu.zeroGyroZAxis();
+    
+
+}
+
+/*
+*  ======== imuLoop ========
+*/
+void imuLoop()
+{
+    
+    /*update IMU data every 50 ms (20 Hz) */
+    imu.readGyro();
+    imu.readAccel();
+    imu.readMag();
+
+    delay(50);
+
+    imuCount++;
+
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/motorLoop.ino
new file mode 100644
index 0000000..3bcd4e0
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/SplineDrive/motorLoop.ino
@@ -0,0 +1,70 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch waits until trajectory is generated, then proceeeds
+ *  to use PID heading control to follow the calculated trajectory
+ */
+
+#include 
+#include 
+
+#include 
+#include 
+#include "MotionPlanner.h"
+
+#define PERIOD  0           /* period of motor control updates */
+#define TEST 0
+
+float targetAngle = 0.0;
+bool targetAngleSet = false;
+
+static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
+
+PIDController steerPID;
+
+Utilities util;
+
+/*
+ *  ======== motorSetup ========
+ */
+
+void motorSetup(void)
+{
+  Serial.begin(9600);
+  Serial.println("motorSetup ...");
+  
+  /* initialize the Pololu driver motor library */
+  motors.setRightSpeed(0);
+  motors.setLeftSpeed(0);
+
+  Serial.println("motorSetup done.");
+
+  util = Utilities();
+  
+  steerPID = PIDController(0.01717, 0.0, 0.003);
+
+}
+
+/*
+
+ *  ======== motorLoop ========
+ 
+ */
+
+void motorLoop(void)
+{
+  
+  if(mp.ready()){
+
+    mp.followSpline(motors, steerPID, 0.35); 
+    
+  }
+
+  delay(20);
+}
+
+
+
+
+
+
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/UDPBroadcast.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/UDPBroadcast.ino
new file mode 100644
index 0000000..ad766d5
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/UDPBroadcast.ino
@@ -0,0 +1,28 @@
+/*  ======== UDPBroadcast.ino ========
+ *  A platform for controlling mutiple Zumos using a single Processing 
+ *  application.
+ *
+ *  All Zumos involved and the PC running Processing must connect to a shared
+ *  network, in this case, the open network "TINK-NET".
+ *
+ *  Broadcasting the commands using UDP from the Processing side results in
+ *  all Zumos currently running the example to simultaneously drive. Additionally,
+ *  all Zumos receiving commands will send back their current IMU readings,
+ *  allowing the Processing sketch to simultaneously display data from the Zumos.
+ *
+ */
+
+#include 
+#include 
+#include 
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G    imuGyro;    /* gyro data */
+
+/* motor sketch external declarations */
+extern char motorWASD;
+
+/* ap sketch external declarations */
+#define MAIN_LED_PIN RED_LED
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/apLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/apLoop.ino
new file mode 100644
index 0000000..25d5f51
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/apLoop.ino
@@ -0,0 +1,137 @@
+/*
+ *  ======== apLoop ========
+ *  This sketch connects to an open network named "TINK-NET" and listens
+ *  for motor control commands over a UDP socket connection. Upon receiving
+ *  a command, the sketch sends a IMU data packet as a reply to the sender.
+ *
+ */
+
+#include 
+#include 
+#include 
+
+#include 
+
+/* name of the network and its password */
+char ssid[] = "TINK-NET";
+
+/* zumo command port */
+#define CMD_PORT 8080
+
+/* IMU data port */
+#define DATA_PORT 6000
+
+IPAddress broadcastIP(192,168,1,255);
+
+static void sendIMUPacket(WiFiUDP Udp, IPAddress ip, int localPort);
+
+WiFiUDP Udp;
+
+/*
+ *  ======== apSetup ========
+ */
+void apSetup()
+{
+    Serial.begin(9600);
+
+    /* set priority of this task to be lower than other tasks */
+    Task_setPri(Task_self(), 1);
+    
+    // attempt to connect to Wifi network:
+    Serial.print("Attempting to connect to Network named: ");
+    // print the network name (SSID);
+    Serial.println(ssid); 
+    // Connect to open network
+    WiFi.begin(ssid);
+    while ( WiFi.status() != WL_CONNECTED) {
+      // print dots while we wait to connect
+      Serial.print(".");
+      delay(300);
+    }
+    
+    Serial.println("Connected");
+    Serial.println("Waiting for an ip address");
+    
+    while (WiFi.localIP() == INADDR_NONE) {
+      // print dots while we wait for an ip addresss
+      Serial.print(".");
+      delay(300);
+    }
+  
+    Serial.println("\nIP Address obtained");
+    printWifiStatus();
+    
+    /* socket listening for motor commands */
+    Udp.begin(CMD_PORT);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          
+}
+
+/*
+ *  ======== apLoop ========
+ */
+ 
+void apLoop()
+{
+
+  /* listen for motor commands from group network */
+  // if there's data available, read a packet
+  int packetSize = Udp.parsePacket();
+  if (packetSize){
+      static char buffer[16] = {0}; 
+          
+      Serial.print("Received packet from ");
+      IPAddress remoteIp = Udp.remoteIP();
+      Serial.print(remoteIp);
+      Serial.print(", port ");
+      Serial.println(Udp.remotePort());
+        
+      
+      // read the packet into packetBufffer
+      int len = Udp.read(buffer, 64);
+             
+      if (len > 0) buffer[len] = 0;
+      Serial.print("Contents: ");
+      Serial.println(buffer);
+             
+      motorWASD = buffer[0];  
+      
+      /* send back an IMU packet */ 
+      sendIMUPacket(Udp, Udp.remoteIP(), Udp.remotePort());     
+   }      
+           
+}
+
+/*
+ *  ======== doWASD ========
+ */
+static void sendIMUPacket(WiFiUDP Udp, IPAddress ip, int localPort)
+{
+    static char report[80];
+
+    /* 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);
+    
+    Udp.beginPacket(ip, localPort);
+    Udp.write(report);
+    Udp.endPacket();
+}
+
+void printWifiStatus() {
+  // print the SSID of the network you're attached to:
+  Serial.print("SSID: ");
+  Serial.println(WiFi.SSID());
+
+  // print your WiFi IP address:
+  IPAddress ip = WiFi.localIP();
+  Serial.print("IP Address: ");
+  Serial.println(ip);
+
+  // print the received signal strength:
+  long rssi = WiFi.RSSI();
+  Serial.print("signal strength (RSSI):");
+  Serial.print(rssi);
+  Serial.println(" dBm");
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/imuLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/imuLoop.ino
new file mode 100644
index 0000000..be55408
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/imuLoop.ino
@@ -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 
+
+#include 
+#include 
+
+/* 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/libraries/ZumoCC3200/examples/UDPBroadcast/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/motorLoop.ino
new file mode 100644
index 0000000..8d34fba
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/motorLoop.ino
@@ -0,0 +1,173 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorWASD, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w' - drive forward
+ *      's' - drive backward
+ *      'a' - turn left
+ *      'd' - turn right
+ *      ' ' - stop
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorWASD.
+ */
+#include 
+#include 
+#include 
+
+#define PERIOD  1           /* period of motor control updates */
+
+char motorWASD = ' ';       /* current motor drive command */
+
+static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
+
+PIDController steerPID;
+
+static int clip(int speed);
+static void drive(char wasd, 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.");
+    
+    steerPID = PIDController(0.01717, 0.0, 0.003);
+}
+
+/*
+ *  ======== motorLoop ========
+ */
+void motorLoop(void)
+{
+    /* state used to blink LED */
+    static unsigned count = 0;
+    static char state = 0;
+    
+    switch (motorWASD) {
+        case 's':
+        case 'w': {
+            /* illuminate LED while driving */
+            digitalWrite(MAIN_LED_PIN, HIGH);
+            drive(motorWASD, 200, PERIOD);
+            break;
+        }
+
+        case 'd':
+        case 'a': {
+            /* blink LED while turning */
+            if (count == ((count / 100) * 100)) {
+                state ^= 1;
+                digitalWrite(MAIN_LED_PIN, state);
+            }   
+            drive(motorWASD, 100, PERIOD);
+            break;
+        }
+
+        default: {
+            /* turn off LED while stopped */
+            motorWASD = ' ';
+            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 ========
+ *  Drive motors in the direction and speed specified by wasd and goal
+ *
+ *  Note: negative goal values imply a reversal of the wasd direction
+ */
+static void drive(char wasd, int goal, unsigned int duration)
+{
+    static int leftSpeed = 0;
+    static int rightSpeed = 0;
+    
+    while (duration > 0) {
+        duration--;
+
+        /* gradually adjust curent speeds to goal */
+        switch (wasd) {
+            case ' ': { /* stop */
+                leftSpeed = next(leftSpeed, 0);
+                rightSpeed = next(rightSpeed, 0);
+                break;
+            }
+            case 'w': { /* forward */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            case 's': { /* backward */
+                leftSpeed = next(leftSpeed, -goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'd': { /* turn right */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'a': { /* turn left */
+                leftSpeed = next(leftSpeed, -goal);;
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            default: {
+                break;
+            }
+        }
+
+        /* clip speeds to allowable range */
+        leftSpeed = clip(leftSpeed);
+        rightSpeed = clip(rightSpeed);
+    
+        /* set motor speeds */
+        ZumoMotors::setLeftSpeed(leftSpeed);
+        ZumoMotors::setRightSpeed(rightSpeed);
+
+        /* sleep for 1 ms (so duration is in milliseconds) */
+        delay(1);
+    }
+}
+
+/*
+ *  ======== next ========
+ *  Compute the next motor speed value given the cur speed and a new goal
+ */
+static int next(int cur, int goal)
+{
+    int tmp = (goal - cur) * 0.0625f + cur;
+    return (tmp == cur ? goal : tmp);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/UDPExample/UDPExample.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPExample/UDPExample.ino
new file mode 100644
index 0000000..d8d7d72
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/UDPExample/UDPExample.ino
@@ -0,0 +1,24 @@
+/*  ======== basics.ino ========
+ *  Simple demo of the CC3200 Zumo platform basics
+ *
+ *  This example creates an access point named zumo-udp with password
+ *  "password" and a UDP socket on port 8080.
+ *
+ *  The datagram connections listens for incoming packets and interprets
+ *  them as motor commands, and send packets containing IMU data.
+ */
+
+#include 
+#include 
+#include 
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G    imuGyro;    /* gyro data */
+
+/* motor sketch external declarations */
+extern char motorWASD;
+
+/* ap sketch external declarations */
+#define MAIN_LED_PIN RED_LED
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/UDPExample/apLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPExample/apLoop.ino
new file mode 100644
index 0000000..ab1c843
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/UDPExample/apLoop.ino
@@ -0,0 +1,99 @@
+/*
+ *  ======== apLoop ========
+ *  Unicast UDP communicating with Processing
+ *
+ *  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 
+#include 
+#include 
+
+#include 
+
+/* name of the network and its password */
+static const char ssid[] = "zumo-udp";
+static const char wifipw[] = "password";
+
+/* port number for UDP communications */
+#define PORTNUM 8080
+
+/* IP Address of processing app */
+IPAddress IP(192,168,1,2);
+
+static void sendIMUPacket(WiFiUDP Udp, IPAddress ip, int localPort);
+
+WiFiUDP Udp;
+
+/*
+ *  ======== 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);
+    } 
+    Serial.println("Datagram socket opened on port 8080");
+    Udp.begin(PORTNUM);
+}
+
+/*
+ *  ======== apLoop ========
+ */
+void apLoop()
+{
+    // if there's data available, read a packet
+    int packetSize = Udp.parsePacket();
+    if (packetSize){
+       static char buffer[64] = {0}; 
+      
+       Serial.print("Received packet from ");
+       IPAddress remoteIp = Udp.remoteIP();
+       Serial.print(remoteIp);
+       Serial.print(", port ");
+       Serial.println(Udp.remotePort());
+    
+       // read the packet into packetBufffer
+       int len = Udp.read(buffer, 64);
+       
+       if (len > 0) buffer[len] = 0;
+       Serial.print("Contents: ");
+       Serial.println(buffer);
+       
+       motorWASD = buffer[0];
+       
+    }
+    
+    /* send IMU data */
+    sendIMUPacket(Udp, IP, PORTNUM);
+}
+
+/*
+ *  ======== sendIMUPacket ========
+ */
+static void sendIMUPacket(WiFiUDP Udp, IPAddress ip, int localPort)
+{
+    static char report[80];
+
+    /* 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);
+    
+    Udp.beginPacket(ip, localPort);
+    Udp.write(report);
+    Udp.endPacket();
+}
+
diff --git a/src/Energia/libraries/ZumoCC3200/examples/UDPExample/imuLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPExample/imuLoop.ino
new file mode 100644
index 0000000..be55408
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/UDPExample/imuLoop.ino
@@ -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 
+
+#include 
+#include 
+
+/* 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/libraries/ZumoCC3200/examples/UDPExample/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPExample/motorLoop.ino
new file mode 100644
index 0000000..69b2dd1
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/examples/UDPExample/motorLoop.ino
@@ -0,0 +1,170 @@
+/*
+ *  ======== motorLoop ========
+ *  This sketch controls the motors on the Zumo by polling a global
+ *  variable, motorWASD, once per PERIOD milliseconds for one of the
+ *  following motor commands:
+ *      'w' - drive forward
+ *      's' - drive backward
+ *      'a' - turn left
+ *      'd' - turn right
+ *      ' ' - stop
+ *
+ *  Other sketches or interrupts can control the zumo by simply writing the
+ *  desired command to motorWASD.
+ */
+#include 
+
+#include 
+
+#define PERIOD  1           /* period of motor control updates */
+
+char motorWASD = ' ';       /* current motor drive command */
+
+static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
+
+static int clip(int speed);
+static void drive(char wasd, int goal, unsigned int duration);
+static int next(int cur, int goal);
+
+/*
+ *  ======== 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 (motorWASD) {
+        case 's':
+        case 'w': {
+            /* illuminate LED while driving */
+            digitalWrite(MAIN_LED_PIN, HIGH);
+            drive(motorWASD, 200, PERIOD);
+            break;
+        }
+
+        case 'd':
+        case 'a': {
+            /* blink LED while turning */
+            if (count == ((count / 100) * 100)) {
+                state ^= 1;
+                digitalWrite(MAIN_LED_PIN, state);
+            }   
+            drive(motorWASD, 100, PERIOD);
+            break;
+        }
+
+        default: {
+            /* turn off LED while stopped */
+            motorWASD = ' ';
+            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 ========
+ *  Drive motors in the direction and speed specified by wasd and goal
+ *
+ *  Note: negative goal values imply a reversal of the wasd direction
+ */
+static void drive(char wasd, int goal, unsigned int duration)
+{
+    static int leftSpeed = 0;
+    static int rightSpeed = 0;
+    
+    while (duration > 0) {
+        duration--;
+
+        /* gradually adjust curent speeds to goal */
+        switch (wasd) {
+            case ' ': { /* stop */
+                leftSpeed = next(leftSpeed, 0);
+                rightSpeed = next(rightSpeed, 0);
+                break;
+            }
+            case 'w': { /* forward */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            case 's': { /* backward */
+                leftSpeed = next(leftSpeed, -goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'd': { /* turn right */
+                leftSpeed = next(leftSpeed, goal);
+                rightSpeed = next(rightSpeed, -goal);
+                break;
+            }
+
+            case 'a': { /* turn left */
+                leftSpeed = next(leftSpeed, -goal);;
+                rightSpeed = next(rightSpeed, goal);
+                break;
+            }
+
+            default: {
+                break;
+            }
+        }
+
+        /* clip speeds to allowable range */
+        leftSpeed = clip(leftSpeed);
+        rightSpeed = clip(rightSpeed);
+    
+        /* set motor speeds */
+        ZumoMotors::setLeftSpeed(leftSpeed);
+        ZumoMotors::setRightSpeed(rightSpeed);
+
+        /* sleep for 1 ms (so duration is in milliseconds) */
+        delay(1);
+    }
+}
+
+/*
+ *  ======== next ========
+ *  Compute the next motor speed value given the cur speed and a new goal
+ */
+static int next(int cur, int goal)
+{
+    int tmp = (goal - cur) * 0.0625f + cur;
+    return (tmp == cur ? goal : tmp);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/Balancer.cpp b/src/Energia/libraries/ZumoCC3200/utility/Balancer.cpp
new file mode 100644
index 0000000..65c8212
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/Balancer.cpp
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ *  ======== Balancer ========
+ *  This class provides methods for balancing the zumo using PID
+ *  control as well as modifying the PID gains and accessing 
+ *  state information for display and/or debugging purposes
+ */
+
+#include "Balancer.h"
+#include "PIDController.h"
+#include "ZumoMotors.h"
+#include "IMUManager.h"
+
+#include 
+
+PIDController Balancer::horizontalPID;
+PIDController Balancer::verticalPID;
+
+Balancer::BalanceInfo Balancer::balanceStatus;
+
+Balancer::Balancer()
+{
+     horizontalPID = PIDController(10.6f, 0.0f, 0.0f);
+
+     /* The D term for vertical balancing is not calculated by the PID 
+      * controller. Instead it is taken directly from the gyro y-axis
+      * angular velocity measurements.
+      */
+     verticalPID = PIDController(27.6f, 0.27f, 0.2f);
+
+     balanceStatus.error = 0;
+     balanceStatus.motorPower = 0;
+     balanceStatus.PIDTerms[0] = 0;
+     balanceStatus.PIDTerms[1] = 0;
+     balanceStatus.PIDTerms[2] = 0;
+}
+
+/*
+ *  ======== horizontalBalance ========
+ *  Balance horizontally using PID feedback control
+ *
+ *  Meant to be used on a pivoting balance board, so 
+ *  the robot will use drive actions to find the center
+ *  of balance.
+ */
+void Balancer::horizontalBalance(ZumoMotors motors, IMUManager imu)
+{
+     float angle = imu.getFilteredTiltAngle();
+     
+     /* regulate the error to -89 degrees */
+     float power = horizontalPID.calculate(HORIZONTAL_BALANCING_ANGLE - angle);
+
+     power = Utilities::saturate(power, -40, 40);
+     motors.setLeftSpeed((int)power);
+     motors.setRightSpeed((int)power);
+}
+
+/*
+ *  ======== verticalBalance ========
+ *  Balance vertically using PID feedback control
+ *
+ *  The robot uses its front wheels to stabilize itself
+ *  vertically on a medium-friction surface
+ */
+void Balancer::verticalBalance(ZumoMotors motors, IMUManager imu)
+{
+     float angle = imu.getFilteredTiltAngle();
+     float angularVelocity = imu.getGyroY();
+     float error = angle - VERTICAL_BALANCING_ANGLE;
+     int power = 0;
+     
+     /* if the robot hasn't fallen over */
+     if (abs(error) < 30) {
+         
+         /* calculate the reactionary output with the derivative term being
+          * calculated separately */
+         power = (int) (verticalPID.calculate(error) + verticalPID.getD() * angularVelocity);
+         
+         /* limit oscillations in the stable regime */
+//         if(util.abs(error) < 1.5){
+//            power = util.saturate(power, -50, 50); 
+//         }
+         
+         /* if error changes signs drastically, i.e. significant oscillation detected, limit power */
+//         if((error * prevError < 0) && util.abs(error-prevError) > 1.2){
+//            power = util.saturate(power, -60, 60); 
+//         }
+
+     }
+     else {
+         power = 0;
+     }
+
+     motors.setLeftSpeed(power);
+     motors.setRightSpeed(power);
+     
+     /* store new data in balanceInfo */
+     balanceStatus.error = error;
+     balanceStatus.motorPower = power;
+     balanceStatus.PIDTerms[0] = verticalPID.getPContribution();
+     balanceStatus.PIDTerms[1] = verticalPID.getIContribution();
+     balanceStatus.PIDTerms[2] = verticalPID.getDContribution();
+}
+
+/*
+ *  ======== verticalDrive ========
+ *  Drive the zumo while it is balanced vertically
+ *
+ *  The method takes in a WASD command and moves in the
+ *  corresponding direction while using a PID controller
+ *  to balance vertically
+ */
+void Balancer::verticalDrive(ZumoMotors motors, char cmd)
+{
+  
+  /* unimplemented */
+  
+}
+
diff --git a/src/Energia/libraries/ZumoCC3200/utility/Balancer.h b/src/Energia/libraries/ZumoCC3200/utility/Balancer.h
new file mode 100644
index 0000000..52e00b5
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/Balancer.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef Balancer_h
+#define Balancer_h
+
+#include "ZumoMotors.h"
+#include "PIDController.h"
+#include "IMUManager.h"
+
+class Balancer
+{
+  public:
+    struct BalanceInfo {
+    	float error;
+    	int motorPower;
+    	float PIDTerms[3];
+    };
+
+    Balancer();  
+
+    static void horizontalBalance(ZumoMotors motors, IMUManager imu);
+    static void verticalBalance(ZumoMotors motors, IMUManager imu);
+    static void verticalDrive(ZumoMotors motors, char cmd);
+    static void setHorizontalGains(float p, float i, float d);
+    static void setVerticalGains(float p, float i, float d);
+
+    static BalanceInfo balanceStatus;
+    static PIDController horizontalPID;
+    static PIDController verticalPID;
+
+  private:
+    /* the angle at which the zumo balances vertically */
+    const static float VERTICAL_BALANCING_ANGLE = -1.337f;
+
+    /* the angle at which the zumo balances horizontally */
+    const static float HORIZONTAL_BALANCING_ANGLE = -89.0f;
+};
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/Command.h b/src/Energia/libraries/ZumoCC3200/utility/Command.h
new file mode 100644
index 0000000..abf6909
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/Command.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef Command_h
+#define Command_h
+
+#include "Utilities.h"
+
+class Command {
+public:
+    virtual void run(Utilities::MotorInfo &info) = 0; //format should be {angle/error, left motor power, right motor power, time}
+    virtual void initialize() = 0;
+    virtual void end() = 0;
+    virtual bool isFinished() = 0;
+    virtual bool hasBeenInitialized() = 0;
+    virtual bool isTimedOut() = 0;
+
+private:
+    long initTime;
+    long finishTime;
+};
+
+#endif
+
diff --git a/src/Energia/libraries/ZumoCC3200/utility/CommandManager.cpp b/src/Energia/libraries/ZumoCC3200/utility/CommandManager.cpp
new file mode 100644
index 0000000..3a8103e
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/CommandManager.cpp
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include 
+#include "CommandManager.h"
+
+vector CommandManager::commandList;
+CommandManager::CommandManager()
+{
+}
+
+void CommandManager::addCommand(Command* cmd)
+{
+    commandList.push_back(cmd);
+}
+
+void CommandManager::run(Utilities::MotorInfo &info)
+{
+    if (commandList.size() != 0) {
+        Command* currCommand = commandList.at(0);
+        if (!(currCommand->hasBeenInitialized())) {
+            Serial.println("Initialize");
+            currCommand->initialize();
+        }
+        else if (currCommand->hasBeenInitialized()
+                && !(currCommand->isFinished())) {
+            Serial.println("Running");
+            currCommand->run(info);
+        }
+        else if (currCommand->isFinished()) {
+            Serial.println("End");
+            currCommand->end();
+            commandList.erase(commandList.begin());
+        }
+    }
+    else {
+        info.error = 0;
+        info.leftSpeed = 0;
+        info.rightSpeed = 0;
+        info.time = (float) millis();
+    }
+}
+
+void CommandManager::clearAllCommands(void)
+{
+    for (int i = 0; i < commandList.size(); i++) {
+        Command* cmd = commandList[i];
+        cmd->end();
+    }
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/CommandManager.h b/src/Energia/libraries/ZumoCC3200/utility/CommandManager.h
new file mode 100644
index 0000000..0c4c414
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/CommandManager.h
@@ -0,0 +1,19 @@
+#ifndef CommandManager_h
+#define CommandManager_h
+
+#include 
+#include "Command.h"
+#include "Utilities.h"
+
+using namespace std;
+
+class CommandManager {
+public:
+    CommandManager();
+    void addCommand(Command* cmd);
+    void clearAllCommands();
+    void run(Utilities::MotorInfo &info);
+private:
+    static vector commandList;
+};
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/DCM.cpp b/src/Energia/libraries/ZumoCC3200/utility/DCM.cpp
new file mode 100644
index 0000000..90f1fbe
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/DCM.cpp
@@ -0,0 +1,660 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * DCM.c
+ *
+ *  Created on: Jul 22, 2015
+ *      Author: x0236197
+ */
+#include 
+
+#include 
+
+#include 
+#include 
+#include 
+#include 
+
+#include "DCM.h"
+
+typedef struct DCM_Object
+{
+    /* The state of the direction cosine matrix */
+    float dcm[3][3];
+
+    /* The time delta between updates to the DCM */
+    float deltaT;
+
+    /* The scaling factor for DCM update based on the accelerometer reading */
+    float scaleA;
+
+    /* The scaling factor for DCM update based on the gyroscope reading */
+    float scaleG;
+
+    /* The scaling factor for DCM update based on the magnetometer reading */
+    float scaleM;
+
+    /* The most recent accelerometer readings */
+    float pfAccel[3];
+
+    /* The most recent gyroscope readings */
+    float pfGyro[3];
+
+    /* The most recent magnetometer readings */
+    float pfMagneto[3];
+} DCM_Object;
+
+static float dotProduct(float v1[3], float v2[3]);
+static void  crossProduct(float vout[3], float v1[3], float v2[3]);
+static void  scaleVector(float vout[3], float vin[3], float fScale);
+static void  addVector(float vout[3], float v1[3], float v2[3]);
+static void  normalize(float vin[3]);
+
+/*
+ *  ======== DCM_create ========
+ */
+DCM_Handle DCM_create(float deltaT,
+                      float scaleA, float scaleG, float scaleM)
+{
+    DCM_Handle handle = (DCM_Object *)malloc(sizeof(struct DCM_Object));
+
+    /* Initialize the DCM matrix to the identity matrix */
+    handle->dcm[0][0] = 1.0f;
+    handle->dcm[0][1] = 0.0f;
+    handle->dcm[0][2] = 0.0f;
+    handle->dcm[1][0] = 0.0f;
+    handle->dcm[1][1] = 1.0f;
+    handle->dcm[1][2] = 0.0f;
+    handle->dcm[2][0] = 0.0f;
+    handle->dcm[2][1] = 0.0f;
+    handle->dcm[2][2] = 1.0f;
+
+    /* Save the time delta between DCM updates */
+    handle->deltaT = deltaT;
+
+    /* Save the scaling factors that are applied to the accelerometer,
+       gyroscope, and magnetometer readings */
+    handle->scaleA = scaleA;
+    handle->scaleG = scaleG;
+    handle->scaleM = scaleM;
+
+    return handle;
+}
+
+/*
+ *  ======== DCM_delete ========
+ */
+void DCM_delete(DCM_Handle handle)
+{
+    free(handle);
+}
+
+/*
+ *  ======== DCM_updateAccelData ========
+ *  Updates the accelerometer reading used by the complementary filter DCM
+ *  algorithm.
+ * 
+ *  handle is a pointer to the DCM state structure.
+ *  fAccelX is the accelerometer reading in the X body axis.
+ *  fAccelY is the accelerometer reading in the Y body axis.
+ *  fAccelZ is the accelerometer reading in the Z body axis.
+ * 
+ *  This function updates the accelerometer reading used by the complementary
+ *  filter DCM algorithm.  The accelerometer readings provided to this function
+ *  are used by subsequent calls to DCM_start() and DCM_update() to
+ *  compute the attitude estimate.
+ */
+void DCM_updateAccelData(DCM_Handle handle,
+                         float fAccelX, float fAccelY, float fAccelZ)
+{
+    /* The user should never pass in values that are not-a-number */
+    assert(!isnan(fAccelX));
+    assert(!isnan(fAccelY));
+    assert(!isnan(fAccelZ));
+
+    /* Save the new accelerometer reading */
+    handle->pfAccel[0] = fAccelX;
+    handle->pfAccel[1] = fAccelY;
+    handle->pfAccel[2] = fAccelZ;
+}
+
+/*
+ *  ======== DCM_updateGyroData ========
+ *  Updates the gyroscope reading used by the complementary filter DCM
+ *  algorithm.
+ * 
+ *  handle is a pointer to the DCM state structure.
+ *  fGyroX is the gyroscope reading in the X body axis.
+ *  fGyroY is the gyroscope reading in the Y body axis.
+ *  fGyroZ is the gyroscope reading in the Z body axis.
+ * 
+ *  This function updates the gyroscope reading used by the complementary
+ *  filter DCM algorithm.  The gyroscope readings provided to this function are
+ *  used by subsequent calls to DCM_update() to compute the attitude
+ *  estimate.
+ */
+void DCM_updateGyroData(DCM_Handle handle,
+                        float fGyroX, float fGyroY, float fGyroZ)
+{
+    /* The user should never pass in values that are not-a-number */
+    assert(!isnan(fGyroX));
+    assert(!isnan(fGyroY));
+    assert(!isnan(fGyroZ));
+
+    /* Save the new gyroscope reading */
+    handle->pfGyro[0] = fGyroX;
+    handle->pfGyro[1] = fGyroY;
+    handle->pfGyro[2] = fGyroZ;
+}
+
+/*
+ *  ======== DCM_updateMagnetoData ========
+ *  Updates the magnetometer reading used by the complementary filter DCM
+ *  algorithm.
+ * 
+ *  handle is a pointer to the DCM state structure.
+ *  fMagnetoX is the magnetometer reading in the X body axis.
+ *  fMagnetoY is the magnetometer reading in the Y body axis.
+ *  fMagnetoZ is the magnetometer reading in the Z body axis.
+ * 
+ *  This function updates the magnetometer reading used by the complementary
+ *  filter DCM algorithm.  The magnetometer readings provided to this function
+ *  are used by subsequent calls to DCM_start() and DCM_update() to
+ *  compute the attitude estimate.
+ */
+void DCM_updateMagnetoData(DCM_Handle handle,
+                           float fMagnetoX, float fMagnetoY, float fMagnetoZ)
+{
+    /* The user should never pass in values that are not-a-number */
+    assert(!isnan(fMagnetoX));
+    assert(!isnan(fMagnetoY));
+    assert(!isnan(fMagnetoZ));
+
+    /* Save the new magnetometer reading */
+    handle->pfMagneto[0] = fMagnetoX;
+    handle->pfMagneto[1] = fMagnetoY;
+    handle->pfMagneto[2] = fMagnetoZ;
+}
+
+/*
+ *  ======== cancelDrift ========
+ *  Use orientation reference vectors to cancel out gyro drift with a PI
+ *  feedback controller. This method assumes that the device experiences 
+ *  negligible acceleration. Adjustments can be made to allow this algorithm
+ *  to work with accelerating systems. Only called internally within the 
+ *  DCM calculuation.
+ */
+static void cancelDrift(DCM_Handle handle)
+{
+    /* two correction vectors fed into the controller */
+    float pfKError[3], pfIError[3]; 
+        
+    /* calcluate the Z-axis correction based on accelerometer reading */
+    crossProduct(pfKError, handle->dcm[2], handle->pfAccel); 
+        
+    /* calcluate the X-axis correction based on magnetometer reading */
+    crossProduct(pfIError, handle->dcm[0], handle->pfMagneto); 
+}
+
+/*
+ *  ======== DCM_start ========
+ *  Starts the complementary filter DCM attitude estimation from an initial
+ *  sensor reading.
+ * 
+ *  handle is a pointer to the DCM state structure.
+ * 
+ *  This function computes the initial complementary filter DCM attitude
+ *  estimation state based on the initial accelerometer and magnetometer
+ *  reading.  While not necessary for the attitude estimation to converge,
+ *  using an initial state based on sensor readings results in quicker
+ *  convergence.
+ */
+void DCM_start(DCM_Handle handle)
+{
+    /* The I, J and K basis vectors */
+    float pfI[3], pfJ[3], pfK[3];
+
+    /* The magnetometer reading forms the initial I vector, pointing north */
+    pfI[0] = handle->pfMagneto[0];
+    pfI[1] = handle->pfMagneto[1];
+    pfI[2] = handle->pfMagneto[2];
+
+    /* The accelerometer reading forms the initial K vector, pointing down */
+    pfK[0] = handle->pfAccel[0];
+    pfK[1] = handle->pfAccel[1];
+    pfK[2] = handle->pfAccel[2];
+
+    /* Compute the initial J vector, which is the cross product of the 
+     * K and I vectors
+     */
+    crossProduct(pfJ, pfK, pfI);
+
+    /* Recompute the I vector from the cross product of the J and K vectors.
+     * This makes it fully orthogonal, which it wasn't before since magnetic
+     * north points inside the Earth in many places. 
+     */ 
+    crossProduct(pfI, pfJ, pfK);
+
+    /* Normalize the I, J, and K vectors */
+    normalize(pfI);
+    normalize(pfJ);
+    normalize(pfK);
+
+    /* Initialize the DCM matrix from the I, J, and K vectors */
+    handle->dcm[0][0] = pfI[0];
+    handle->dcm[0][1] = pfI[1];
+    handle->dcm[0][2] = pfI[2];
+    handle->dcm[1][0] = pfJ[0];
+    handle->dcm[1][1] = pfJ[1];
+    handle->dcm[1][2] = pfJ[2];
+    handle->dcm[2][0] = pfK[0];
+    handle->dcm[2][1] = pfK[1];
+    handle->dcm[2][2] = pfK[2];
+}
+
+/*
+ *  ======== DCM_update ========
+ *  Updates the complementary filter DCM attitude estimation based on an
+ *  updated set of sensor readings.
+ * 
+ *  handle is a pointer to the DCM state structure.
+ * 
+ *  This function updates the complementary filter DCM attitude estimation
+ *  state based on the current sensor readings.  This function must be called
+ *  at the rate specified in initialization, with new readings supplied at an
+ *  appropriate rate (for example, magnetometers typically sample at a much
+ *  slower rate than accelerometers and gyroscopes).
+ * 
+ *  Notes: split up into smaller operations
+ */
+
+void DCM_update(DCM_Handle handle)
+{
+    /* The I, J and K basis vectors */
+    float pfI[3], pfJ[3], pfK[3];
+
+    /* The rotation from the previous state */
+    float pfDelta[3]; 
+        
+    /* A temporary storage vector */
+    float pfTemp[3];
+        
+    /* The orthogonality error */
+    float fError;
+    bool bNAN = false;
+
+    /* Form the I, J, K global unit vectors as measured from the body frame
+     * of reference
+     */
+
+    /* The magnetometer reading forms the new Im vector, pointing north */
+    pfI[0] = handle->pfMagneto[0];
+    pfI[1] = handle->pfMagneto[1];
+    pfI[2] = handle->pfMagneto[2];
+
+    /* The accelerometer reading forms the new Ka vector, pointing down */
+    pfK[0] = handle->pfAccel[0];
+    pfK[1] = handle->pfAccel[1];
+    pfK[2] = handle->pfAccel[2];
+
+    /* Compute the new J vector, which is the cross product of the Ka and Im
+     * vectors 
+     */
+    crossProduct(pfJ, pfK, pfI);
+
+    /* Recompute the Im vector from the cross product of the J and Ka vectors.
+     * This makes it fully orthogonal, which it wasn't before since magnetic
+     * north points inside the Earth in many places 
+     */
+    crossProduct(pfI, pfJ, pfK);
+
+    /* Normalize the Im and Ka vectors */
+    normalize(pfI);
+    normalize(pfK);
+
+    /* Compute and scale the rotation as inferred from the accelerometer,
+     * storing it in the rotation accumulator 
+     */
+
+    /* old K crossed with new K -> axis of K rotation */
+    crossProduct(pfTemp, handle->dcm[2], pfK); 
+    scaleVector(pfDelta, pfTemp, handle->scaleA);
+
+    /* Compute and scale the rotation as measured by the gyroscope, adding
+     * it to the rotation accumulator 
+     */
+    pfTemp[0] = -(handle->pfGyro[0]) * (handle->deltaT) * (handle->scaleG); 
+    pfTemp[1] = -(handle->pfGyro[1]) * (handle->deltaT) * (handle->scaleG); 
+    pfTemp[2] = -(handle->pfGyro[2]) * (handle->deltaT) * (handle->scaleG); 
+    addVector(pfDelta, pfDelta, pfTemp);
+
+    /* Compute and scale the rotation as inferred from the magnetometer, 
+     * adding it to the rotation accumulator.
+     * 
+     * Note: old I crossed with new I -> axis of I rotation
+     */
+    crossProduct(pfTemp, handle->dcm[0], pfI);
+    scaleVector(pfTemp, pfTemp, handle->scaleM);
+    addVector(pfDelta, pfDelta, pfTemp);
+
+    /* Rotate the I vector from the DCM matrix by the scaled rotation
+     *     rotation vector crossed with old I
+     *     old I += new I
+     */
+    crossProduct(pfI, pfDelta, handle->dcm[0]); 
+    addVector(handle->dcm[0], handle->dcm[0], pfI); 
+
+    /* Rotate the K vector from the DCM matrix by the scaled rotation
+     *     rotation vector crossed with old K
+     *     old K += new K
+     */
+    crossProduct(pfK, pfDelta, handle->dcm[2]); 
+    addVector(handle->dcm[2], handle->dcm[2], pfK); 
+
+    /* Compute the orthogonality error between the rotated I and K vectors and
+     * adjust each by half the error, bringing them closer to orthogonality
+     * cross couple to cancel out excess rotation:
+     *     I = I + err*K
+     *     K = K + err*I
+     */
+    fError = dotProduct(handle->dcm[0], handle->dcm[2]) / -2.0f;
+    scaleVector(pfI, handle->dcm[0], fError);
+    scaleVector(pfK, handle->dcm[2], fError);
+    addVector(handle->dcm[0], handle->dcm[0], pfK);  
+    addVector(handle->dcm[2], handle->dcm[2], pfI);
+
+    /* Normalize the I and K vectors.
+     *     Use the Taylor expansion around 1 to simplify calculations
+     */
+    scaleVector(handle->dcm[0], handle->dcm[0],
+                0.5f * (3.0f - dotProduct(handle->dcm[0], handle->dcm[0])));
+    scaleVector(handle->dcm[2], handle->dcm[2],
+                0.5f * (3.0f - dotProduct(handle->dcm[2], handle->dcm[2])));
+
+    /* Compute the rotated J vector from the cross product of the rotated,
+     * corrected K and I vectors 
+     */
+    crossProduct(handle->dcm[1], handle->dcm[2], handle->dcm[0]);
+
+    /* Determine if the newly updated DCM contains any invalid (i.e.,
+     * NaN) values 
+     */
+    bNAN = (isnan(handle->dcm[0][0]) || isnan(handle->dcm[0][1])
+            || isnan(handle->dcm[0][2]) || isnan(handle->dcm[1][0])
+            || isnan(handle->dcm[1][1]) || isnan(handle->dcm[1][2])
+            || isnan(handle->dcm[2][0]) || isnan(handle->dcm[2][1])
+            || isnan(handle->dcm[2][2]));
+
+    /* As a debug measure, we check for NaN in the DCM.  The user can trap
+     * this event depending on their implementation of __error__.  Should they
+     * choose to disable interrupts and loop forever then they will have
+     * preserved the stack and can analyze how they arrived at NaN 
+     */
+    assert(!bNAN);
+
+    /* If any part of the matrix is not-a-number then reset the DCM back to 
+     * the identity matrix 
+     */
+    if (bNAN) {
+        handle->dcm[0][0] = 1.0f;
+        handle->dcm[0][1] = 0.0f;
+        handle->dcm[0][2] = 0.0f;
+        handle->dcm[1][0] = 0.0f;
+        handle->dcm[1][1] = 1.0f;
+        handle->dcm[1][2] = 0.0f;
+        handle->dcm[2][0] = 0.0f;
+        handle->dcm[2][1] = 0.0f;
+        handle->dcm[2][2] = 1.0f;
+    }
+}
+
+/*
+ *  ======== DCM_getMatrix ========
+ *  Assigns the input matrix with the current DCM values.
+ * 
+ *  handle is a pointer to the DCM state structure.
+ *  dcm is a pointer to the array into which to store the DCM matrix
+ *  values.
+ */
+void DCM_getMatrix(DCM_Handle handle, float dcm[3][3])
+{
+    /* set the input matrix as the DCM */
+    dcm[0][0] = handle->dcm[0][0];
+    dcm[0][1] = handle->dcm[0][1];
+    dcm[0][2] = handle->dcm[0][2];
+    dcm[1][0] = handle->dcm[1][0];
+    dcm[1][1] = handle->dcm[1][1];
+    dcm[1][2] = handle->dcm[1][2];
+    dcm[2][0] = handle->dcm[2][0];
+    dcm[2][1] = handle->dcm[2][1];
+    dcm[2][2] = handle->dcm[2][2];
+}
+
+/*
+ *  ======== DCM_computeEulers ========
+ *  Computes the Euler angles from the DCM attitude estimation matrix.
+ * 
+ *  handle is a pointer to the DCM state structure.
+ *  pfRoll is a pointer to the value into which the roll is stored.
+ *  pfPitch is a pointer to the value into which the pitch is stored.
+ *  pfYaw is a pointer to the value into which the yaw is stored.
+ * 
+ *  This function computes the Euler angles that are represented by the DCM
+ *  attitude estimation matrix.  If any of the Euler angles is not required,
+ *  the corresponding parameter can be \b NULL.
+ */
+void DCM_computeEulers(DCM_Handle handle,
+                       float *pfRoll, float *pfPitch,  float *pfYaw)
+{
+    /* Compute the roll, pitch, and yaw as required */
+    if (pfRoll) {
+        *pfRoll = atan2f(handle->dcm[2][1], handle->dcm[2][2]);
+    }
+    if (pfPitch) {
+        *pfPitch = -asinf(handle->dcm[2][0]);
+    }
+    if (pfYaw) {
+        *pfYaw = atan2f(handle->dcm[1][0], handle->dcm[0][0]);
+    }
+}
+
+/*
+ *  ======== DCM_computeQuaternion ========
+ *  Computes the quaternion from the DCM attitude estimation matrix.
+ * 
+ *  handle is a pointer to the DCM state structure.
+ *  pfQuaternion is an array into which the quaternion is stored.
+ * 
+ *  This function computes the quaternion that is represented by the DCM
+ *  attitude estimation matrix.
+ */
+void DCM_computeQuaternion(DCM_Handle handle, float pfQuaternion[4])
+{
+    float fQs, fQx, fQy, fQz;
+
+    /* Partially compute Qs, Qx, Qy, and Qz based on the DCM diagonals.  The
+     * square root, an expensive operation, is computed for only one of these
+     * as determined later 
+     */
+    fQs = 1 + handle->dcm[0][0] + handle->dcm[1][1] + handle->dcm[2][2];
+    fQx = 1 + handle->dcm[0][0] - handle->dcm[1][1] - handle->dcm[2][2];
+    fQy = 1 - handle->dcm[0][0] + handle->dcm[1][1] - handle->dcm[2][2];
+    fQz = 1 - handle->dcm[0][0] - handle->dcm[1][1] + handle->dcm[2][2];
+
+    /* See if Qs is the largest of the diagonal values */
+    if ((fQs > fQx) && (fQs > fQy) && (fQs > fQz)) {
+        /* Finish the computation of Qs */
+        fQs = sqrtf(fQs) / 2;
+
+        /* Compute the values of the quaternion based on Qs */
+        pfQuaternion[0] = fQs;
+        pfQuaternion[1] = ((handle->dcm[2][1] - handle->dcm[1][2]) /
+                           (4 * fQs));
+        pfQuaternion[2] = ((handle->dcm[0][2] - handle->dcm[2][0]) /
+                           (4 * fQs));
+        pfQuaternion[3] = ((handle->dcm[1][0] - handle->dcm[0][1]) /
+                           (4 * fQs));
+    }
+    /* Qs is not the largest, so see if Qx is the largest remaining diagonal
+     * value 
+     */
+    else if ((fQx > fQy) && (fQx > fQz)) {
+        /* Finish the computation of Qx */
+        fQx = sqrtf(fQx) / 2;
+
+        /* Compute the values of the quaternion based on Qx */
+        pfQuaternion[0] = ((handle->dcm[2][1] - handle->dcm[1][2]) /
+                           (4 * fQx));
+        pfQuaternion[1] = fQx;
+        pfQuaternion[2] = ((handle->dcm[1][0] + handle->dcm[0][1]) /
+                           (4 * fQx));
+        pfQuaternion[3] = ((handle->dcm[0][2] + handle->dcm[2][0]) /
+                           (4 * fQx));
+    }
+    /* Qs and Qx are not the largest, so see if Qy is the largest remaining
+     * diagonal value 
+     */
+    else if (fQy > fQz)
+    {
+        /* Finish the computation of Qy */
+        fQy = sqrtf(fQy) / 2;
+
+        /* Compute the values of the quaternion based on Qy */
+        pfQuaternion[0] = ((handle->dcm[0][2] - handle->dcm[2][0]) /
+                           (4 * fQy));
+        pfQuaternion[1] = ((handle->dcm[1][0] + handle->dcm[0][1]) /
+                           (4 * fQy));
+        pfQuaternion[2] = fQy;
+        pfQuaternion[3] = ((handle->dcm[2][1] + handle->dcm[1][2]) /
+                           (4 * fQy));
+    }
+    /* Qz is the largest diagonal value */
+    else {
+        /* Finish the computation of Qz */
+        fQz = sqrtf(fQz) / 2;
+
+        /* Compute the values of the quaternion based on Qz */
+        pfQuaternion[0] = ((handle->dcm[1][0] - handle->dcm[0][1]) /
+                           (4 * fQz));
+        pfQuaternion[1] = ((handle->dcm[0][2] + handle->dcm[2][0]) /
+                           (4 * fQz));
+        pfQuaternion[2] = ((handle->dcm[2][1] + handle->dcm[1][2]) /
+                           (4 * fQz));
+        pfQuaternion[3] = fQz;
+    }
+}
+
+/*
+ *  ======== dotProduct ========
+ *  Computes the dot product of two vectors.
+ * 
+ *  v1 is the first vector.
+ *  v2 is the second vector.
+ * 
+ *  This function computes the dot product of two 3-dimensional vector.
+ * 
+ *  Returns the dot product of the two vectors.
+ */
+static float dotProduct(float v1[3], float v2[3])
+{
+    return ((v1[0] * v2[0]) + (v1[1] * v2[1]) + (v1[2] * v2[2]));
+}
+
+/*
+ *  ======== crossProduct ========
+ *  Computes the cross product of two vectors.
+ * 
+ *  vout is the output vector.
+ *  v1 is the first vector.
+ *  v2 is the second vector.
+ * 
+ *  This function computes the cross product of two 3-dimensional vectors.
+ */
+static void crossProduct(float vout[3], float v1[3], float v2[3])
+{
+    vout[0] = ((v1[1] * v2[2]) - (v1[2] * v2[1]));
+    vout[1] = ((v1[2] * v2[0]) - (v1[0] * v2[2]));
+    vout[2] = ((v1[0] * v2[1]) - (v1[1] * v2[0]));
+}
+
+/*
+ *  ======== scaleVector ========
+ *  Scales a vector.
+ * 
+ *  vout is the output vector.
+ *  vin is the input vector.
+ *  fScale is the scale factor.
+ * 
+ *  This function scales a 3-dimensional vector by multiplying each of its
+ *  components by the scale factor.
+ */
+static void scaleVector(float vout[3], float vin[3], float fScale)
+{
+    vout[0] = vin[0] * fScale;
+    vout[1] = vin[1] * fScale;
+    vout[2] = vin[2] * fScale;
+}
+
+/*
+ *  ======== addVector ========
+ *  Adds two vectors.
+ * 
+ *  vout is the output vector.
+ *  v1 is the first vector.
+ *  v2 is the second vector.
+ * 
+ *  This function adds two 3-dimensional vectors.
+ */
+static void addVector(float vout[3], float v1[3], float v2[3])
+{
+    vout[0] = v1[0] + v2[0];
+    vout[1] = v1[1] + v2[1];
+    vout[2] = v1[2] + v2[2];
+}
+
+/*
+ *  ======== normalize ========
+ *  Normalizes a vector
+ * 
+ *  vin is the input vector.
+ * 
+ *  This function normalizes a 3-dimensional vectors by dividing its
+ *  components by its magnitude.
+ */
+static void normalize(float vin[3])
+{
+    float magnitude = sqrtf(dotProduct(vin, vin));
+    vin[0] = (vin[0] / magnitude);
+    vin[1] = (vin[1] / magnitude);
+    vin[2] = (vin[2] / magnitude);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/DCM.h b/src/Energia/libraries/ZumoCC3200/utility/DCM.h
new file mode 100644
index 0000000..a4ff5ad
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/DCM.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * DCM.h
+ *
+ *  Created on: Jul 22, 2015
+ *      Author: x0236197
+ */
+
+#ifndef DCM_H_
+#define DCM_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef PI
+#define PI 3.14159265358979323846
+#endif
+
+typedef struct DCM_Object *DCM_Handle;
+
+//prototypes: public member functions
+
+//extern void DCM_construct()
+
+extern DCM_Handle DCM_create(float fDeltaT, float fScaleA, float fScaleG,
+		float fScaleM);
+
+extern void DCM_delete(DCM_Handle handle);
+
+//extern void DCM_destruct();
+
+extern void DCM_updateAccelData(DCM_Handle handle, float ax, float ay,
+		float az);
+
+extern void DCM_updateGyroData(DCM_Handle handle, float rx, float ry,
+		float rz);
+
+extern void DCM_updateMagnetoData(DCM_Handle handle, float mx, float my,
+		float mz);
+
+extern void DCM_start(DCM_Handle handle);
+
+extern void DCM_update(DCM_Handle handle);
+
+extern void DCM_getMatrix(DCM_Handle handle, float ppfDCM[3][3]);
+
+extern void DCM_computeEulers(DCM_Handle handle, float *pfRoll, float *pfPitch, float *pfYaw);
+
+extern void DCM_computeQuaternion(DCM_Handle handle, float pfQuaternion[4]);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* DCM_H_ */
diff --git a/src/Energia/libraries/ZumoCC3200/utility/DriveLineCommand.cpp b/src/Energia/libraries/ZumoCC3200/utility/DriveLineCommand.cpp
new file mode 100644
index 0000000..0b665bd
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/DriveLineCommand.cpp
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include 
+
+#include "Utilities.h"
+#include "ZumoMotors.h"
+#include "IMUManager.h"
+
+#include "DriveLineCommand.h"
+
+DriveLineCommand::DriveLineCommand(float speed, float timeout)
+{
+    _speed = speed;
+    _forward = _speed > 0;
+    steerController = PIDController(0.03, 0.0, 0.0);
+    numSeconds = timeout;
+    initialized = false;
+}
+
+void DriveLineCommand::run(Utilities::MotorInfo &info)
+{
+    if (!timedOut) {
+        float error = Utilities::wrapAngle(
+                IMUManager::getGyroYaw() - targetAngle);
+        float power = steerController.calculate(error);
+        power = Utilities::saturate(power, _speed - (float) 1.0,
+                (float) 1.0 - _speed);
+        float lTotal = Utilities::clip((_speed + power) * 400);
+        float rTotal = Utilities::clip((_speed - power) * 400);
+        if (!_forward) {
+            float temp = lTotal;
+            lTotal = -rTotal;
+            rTotal = -temp;
+        }
+        if (!timedOut) {
+            Serial.print("lTotal: ");
+            Serial.print(lTotal);
+            Serial.print(" rTotal: ");
+            Serial.println(rTotal);
+            ZumoMotors::setLeftSpeed(lTotal);
+            ZumoMotors::setRightSpeed(rTotal);
+        }
+        else {
+            ZumoMotors::setLeftSpeed(0);
+            ZumoMotors::setRightSpeed(0);
+        }
+        timedOut = ((millis() - initTime) > ((int) (numSeconds * 1000.0)));
+
+        info.error = error;
+        info.leftSpeed = lTotal;
+        info.rightSpeed = rTotal;
+        info.time = (float) millis();
+    }
+}
+
+void DriveLineCommand::initialize()
+{
+    targetAngle = IMUManager::getGyroYaw();
+    initTime = millis();
+    initialized = true;
+    timedOut = false;
+    Utilities::reInitErrorVals();
+}
+
+void DriveLineCommand::end()
+{
+    ZumoMotors::setLeftSpeed(0);
+    ZumoMotors::setRightSpeed(0);
+    timedOut = true;
+    initialized = false;
+}
+
+bool DriveLineCommand::isFinished()
+{
+    return timedOut;
+}
+
+bool DriveLineCommand::hasBeenInitialized()
+{
+    return initialized;
+}
+
+bool DriveLineCommand::isTimedOut()
+{
+    return timedOut;
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/DriveLineCommand.h b/src/Energia/libraries/ZumoCC3200/utility/DriveLineCommand.h
new file mode 100644
index 0000000..05f2d77
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/DriveLineCommand.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DriveLineCommand_h
+#define DriveLineCommand_h
+
+#include "PIDController.h"
+#include "Command.h"
+#include "Utilities.h"
+
+class DriveLineCommand: public Command {
+public:
+    DriveLineCommand(float speed = 0, float timeout = 0);
+    void run(Utilities::MotorInfo &info);
+    void initialize(void);
+    void end(void);
+    bool isFinished(void);
+    bool hasBeenInitialized(void);
+    bool isTimedOut(void);
+private:
+    float _heading;
+    float _speed;
+    float _distance;
+    float targetAngle;
+    float numSeconds;
+    long initTime;
+    bool _forward;
+    bool timedOut;
+    bool initialized;
+    PIDController steerController;
+};
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/IMUManager.cpp b/src/Energia/libraries/ZumoCC3200/utility/IMUManager.cpp
new file mode 100644
index 0000000..217e7f9
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/IMUManager.cpp
@@ -0,0 +1,455 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "IMUManager.h"
+
+#ifndef M_PI
+#define M_PI    3.14159265358979323846
+#endif
+
+LSM303 IMUManager::accel;
+L3G IMUManager::gyro;
+Utilities IMUManager::util;
+
+float IMUManager::zAngle_gyro = 0;
+float IMUManager::yAngle_gyro = 0;
+float IMUManager::xAngle_gyro = 0;
+
+float IMUManager::zReferenceAngle_gyro = 0;
+float IMUManager::yReferenceAngle_gyro = 0;
+float IMUManager::xReferenceAngle_gyro = 0;
+
+float IMUManager::tuningFactor_gyro = 0;
+float IMUManager::filteredAngle_gyro = 0;
+
+float IMUManager::accel_x_offset = 0;
+float IMUManager::accel_y_offset = 0;
+float IMUManager::accel_z_offset = 0;
+
+int IMUManager::gyro_x_offset = 0;
+int IMUManager::gyro_y_offset = 0;
+int IMUManager::gyro_z_offset = 0;
+
+bool IMUManager::finishedCalibration_accel = 0;
+long IMUManager::initCalibrationTime_accel = 0;
+
+int IMUManager::accel_x = 0;
+int IMUManager::accel_y = 0;
+int IMUManager::accel_z = 0;
+
+int IMUManager::gyro_x = 0;
+int IMUManager::gyro_y = 0;
+int IMUManager::gyro_z = 0;
+
+int IMUManager::mag_x = 0;
+int IMUManager::mag_y = 0;
+int IMUManager::mag_z = 0;
+
+/* initialize the minimum values to the highest possible values
+ * and the maximum values to the lowest possible values so that 
+ * their initial values will form a upper and lower bound for calibration
+ */
+LSM303::vector IMUManager::mag_min = {32767, 32767, 32767};
+LSM303::vector IMUManager::mag_max = {-32767, -32767, -32767};
+
+IMUManager::IMUManager()
+{
+    accel = LSM303();
+    gyro = L3G();
+}
+
+bool IMUManager::initGyro()
+{
+    return gyro.init();
+}
+
+bool IMUManager::initAccel()
+{
+    return accel.init();
+}
+
+bool IMUManager::initMag()
+{
+    return accel.init();
+}
+
+void IMUManager::enableGyroDefault()
+{
+    gyro.enableDefault();
+}
+
+void IMUManager::enableAccelDefault()
+{
+    accel.enableDefault();
+}
+
+void IMUManager::enableMagDefault()
+{
+    accel.enableDefault();
+}
+
+/*
+ * ======== readGyro ========
+ * Read gyroscope data and calculate integrated angles
+ */
+void IMUManager::readGyro()
+{
+    static long lastTime_gyro = 0;
+
+    gyro.read();
+
+    gyro_x = (int) (gyro.g.x - gyro_x_offset);
+    gyro_y = (int) (gyro.g.y - gyro_y_offset);
+    gyro_z = (int) (gyro.g.z - gyro_z_offset);
+
+    if (lastTime_gyro != 0) {
+        long currTime_gyro = millis();
+        float zDps = gyro_z * GYRO_CONVERSION_FACTOR;
+        float yDps = gyro_y * GYRO_CONVERSION_FACTOR;
+        float xDps = gyro_x * GYRO_CONVERSION_FACTOR;
+        float time = ((currTime_gyro - lastTime_gyro) / 1000.0f);
+        zAngle_gyro += zDps * time;
+        yAngle_gyro += yDps * time;
+        xAngle_gyro += xDps * time;
+        lastTime_gyro = currTime_gyro;
+    }
+    else {
+        lastTime_gyro = millis();
+    }
+}
+
+void IMUManager::readAccel()
+{
+    accel.readAcc();
+
+    accel_x = accel.a.x;
+    accel_y = accel.a.y;
+    accel_z = accel.a.z;
+}
+
+void IMUManager::readMag()
+{
+    accel.readMag();
+
+    mag_x = accel.m.x;
+    mag_y = accel.m.y;
+    mag_z = accel.m.z;
+}
+
+/*
+ * ======== calibrateGyro ========
+ * Calculate and offset error for each of the gyro's axes
+ *
+ * Sums the gyro readings over a period of time and averages 
+ * them to identify a stationary offset to be subtracted 
+ * from the raw gyro values. Assumes that there is no angular
+ * rotation during the calibration period.
+ */
+void IMUManager::calibrateGyro(int numSeconds)
+{
+    int sumX = 0;
+    int sumY = 0;
+    int sumZ = 0;
+    int count = 0;
+    unsigned long startTime = millis();
+
+    do {
+        gyro.read();
+        sumX += ((int) gyro.g.x);
+        sumY += ((int) gyro.g.y);
+        sumZ += ((int) gyro.g.z);
+        count++;
+        delay(2);
+    } while (((millis() - startTime) / 1000.0) < numSeconds);
+
+    Serial.print("Over a period of ");
+    Serial.print(numSeconds);
+    Serial.print(" seconds, gyro drifted ");
+    Serial.print(sumX);
+    Serial.print(" around the x-axis and ");
+    Serial.print(sumY);
+    Serial.print(" around the y-axis and ");
+    Serial.print(sumZ);
+    Serial.println(" around the z-axis");
+
+    gyro_x_offset = (int)(sumX / (float)(count) + 0.5f);
+    gyro_y_offset = (int)(sumY / (float)(count) + 0.5f);
+    gyro_z_offset = (int)(sumZ / (float)(count) + 0.5f);
+
+    Serial.print("X Zero offset: ");
+    Serial.println(gyro_x_offset);
+    Serial.print("Y Zero offset: ");
+    Serial.println(gyro_y_offset);
+    Serial.print("Z Zero offset: ");
+    Serial.println(gyro_z_offset);
+}
+
+/*
+ * ======== calibrateAccelerometer ========
+ * Calculate and offset error for each of the accelerometer's axes
+ *
+ * Sums the accelerometer readings over a period of time and averages 
+ * them to identify a stationary offset to be subtracted from the 
+ * raw accelerometer values. Assumes that the X and Y accelerations 
+ * are zero and the Z acceleration is 1 g during the calibration
+ * period.
+ */
+void IMUManager::calibrateAccelerometer(int numSeconds)
+{
+    finishedCalibration_accel = false;
+    float sumZReadings = 0;
+    float sumYReadings = 0;
+    float sumXReadings = 0;
+    float newZVal = 0;
+    float newYVal = 0;
+    float newXVal = 0;
+    int count = 0;
+
+    if (initCalibrationTime_accel == 0) {
+        initCalibrationTime_accel = millis();
+    }
+    else {
+        while (!finishedCalibration_accel) {
+            accel.read();
+            finishedCalibration_accel = (((millis() - initCalibrationTime_accel)
+                    / 1000.0) >= numSeconds);
+            newZVal = ((int) accel.a.z * ACCEL_CONVERSION_FACTOR);
+            newYVal = ((int) accel.a.y * ACCEL_CONVERSION_FACTOR);
+            newXVal = ((int) accel.a.x * ACCEL_CONVERSION_FACTOR);
+            sumZReadings += newZVal;
+            sumYReadings += newYVal;
+            sumXReadings += newXVal;
+            count++;
+            if (finishedCalibration_accel) {
+                accel_y_offset = -((float) (sumYReadings / (float) (count)));
+                accel_x_offset = -((float) (sumXReadings / (float) (count)));
+                accel_z_offset = -(((float) (sumZReadings) / (float) (count))
+                        - 1.0f);
+                Serial.print("Accelerometer X offset: ");
+                Serial.print(accel_x_offset);
+                Serial.print(" g's, Y offset: ");
+                Serial.print(accel_y_offset);
+                Serial.print(" g's, Z offset: ");
+                Serial.print(accel_z_offset);
+            }
+        }
+        initCalibrationTime_accel = 0;
+    }
+}
+
+/*
+ * ======== calibrateMagnetometer ========
+ * Track magnetometer data to adjust for inherent bias/error
+ *
+ * Finds the max and min values for each the magnetometer's axis
+ * readings in order to normalize the raw measurements from -1.0 
+ * to 1.0. Run this method while rotating the zumo in all three
+ * rotational degrees of freedom (roll, pitch yaw).
+ *
+ * Runs until a minimum of numSamples are obtained and a minimum
+ * difference between min and max values is detected
+ */
+void IMUManager::calibrateMagnetometer(int numSamples)
+{
+    unsigned int i = 0;
+    int currentSumDifferences = 0; /* once this values exceeds a certain amount, calibration is done */
+
+    while ((i < numSamples) || (currentSumDifferences < MAG_CALIBRATION_THRESHOLD))
+    {
+        accel.readMag();
+
+        /* if new value is lower than min, update the min */
+        mag_min.x = util.min(mag_min.x, accel.m.x);
+        mag_min.y = util.min(mag_min.y, accel.m.y);
+        mag_min.z = util.min(mag_min.z, accel.m.z);
+
+        /* if new value is higher than max, update the max */
+        mag_max.x = util.max(mag_max.x, accel.m.x);
+        mag_max.y = util.max(mag_max.y, accel.m.y);
+        mag_max.z = util.max(mag_max.z, accel.m.z);
+
+        currentSumDifferences = (mag_max.x-mag_min.x)+(mag_max.y-mag_min.y)+(mag_max.z-mag_min.z);
+
+        i++;
+        delay(50);
+    }
+
+    if (mag_max.x == mag_min.x) mag_max.x = mag_min.x + 1;
+    Serial.print("min.x   ");
+    Serial.print(mag_min.x);
+    Serial.println();
+    Serial.print("max.x   ");
+    Serial.print(mag_max.x);
+    Serial.println();
+    
+    if (mag_max.y == mag_min.y) mag_max.y = mag_min.y + 1;
+    Serial.print("min.y   ");
+    Serial.print(mag_min.y);
+    Serial.println();
+    Serial.print("max.y   ");
+    Serial.print(mag_max.y);
+    Serial.println();
+
+    if (mag_max.z == mag_min.z) mag_max.z = mag_min.z + 1;
+    Serial.print("min.z   ");
+    Serial.print(mag_min.z);
+    Serial.println();
+    Serial.print("max.z   ");
+    Serial.print(mag_max.z);
+    Serial.println();
+
+    /* Set calibrated values to accel.m_max and accel.m_min */
+    accel.m_max.x = mag_max.x;
+    accel.m_max.y = mag_max.y;
+    accel.m_max.z = mag_max.z;
+
+    accel.m_min.x = mag_min.x;
+    accel.m_min.y = mag_min.y;
+    accel.m_min.z = mag_min.z;
+}
+
+/* return the angular velocity measured along the X, Y, and Z axis in deg/s */
+float IMUManager::getGyroX()
+{
+    return gyro_x * GYRO_CONVERSION_FACTOR;
+}
+
+float IMUManager::getGyroY()
+{
+    return gyro_y * GYRO_CONVERSION_FACTOR;
+}
+
+float IMUManager::getGyroZ()
+{
+    return gyro_z * GYRO_CONVERSION_FACTOR;
+}
+
+/* return the roll pitch and yaw angles in degrees */
+float IMUManager::getGyroRoll()
+{
+    return util.wrapAngle(xAngle_gyro - xReferenceAngle_gyro);
+}
+
+float IMUManager::getGyroPitch()
+{
+    return util.wrapAngle(yAngle_gyro - yReferenceAngle_gyro);
+}
+
+float IMUManager::getGyroYaw()
+{
+    return util.wrapAngle(zAngle_gyro - zReferenceAngle_gyro);
+}
+
+/* return the gyro's abolute heading angle at the current angle 
+   along the X, Y and Z axis */
+void IMUManager::zeroGyroXAxis()
+{
+    xReferenceAngle_gyro = xAngle_gyro;
+}
+
+void IMUManager::zeroGyroYAxis()
+{
+    yReferenceAngle_gyro = yAngle_gyro;
+}
+
+void IMUManager::zeroGyroZAxis()
+{
+    zReferenceAngle_gyro = zAngle_gyro;
+}
+
+/* return the acceleration measured along the X, Y, and Z axis */
+float IMUManager::getAccelX()
+{
+    return ((accel.a.x - accel_x_offset) * ACCEL_CONVERSION_FACTOR);
+}
+
+float IMUManager::getAccelY()
+{
+    return ((accel.a.y - accel_y_offset) * ACCEL_CONVERSION_FACTOR);
+}
+
+float IMUManager::getAccelZ()
+{
+    return ((accel.a.z - accel_z_offset) * ACCEL_CONVERSION_FACTOR);
+}
+
+/* return the normalized magnetic field reading measured along the X, Y, and Z axis */
+float IMUManager::getMagX()
+{
+    return 2.0*(float)(accel.m.x - accel.m_min.x) / ( accel.m_max.x - accel.m_min.x) - 1.0;
+}
+
+float IMUManager::getMagY()
+{
+    return 2.0*(float)(accel.m.y - accel.m_min.y) / ( accel.m_max.y - accel.m_min.y) - 1.0;
+}
+
+float IMUManager::getMagZ()
+{
+    return 2.0*(float)(accel.m.z - accel.m_min.z) / ( accel.m_max.z - accel.m_min.z) - 1.0;
+}
+
+/*
+ * ======== getTiltAngle ========
+ * Returns the angle tilted with respect to vertical
+ *
+ * Calculated by taking the inverse tangent of the z-axis
+ * acceleration (gravity) over the x-axis acceleration
+ */
+float IMUManager::getTiltAngle()
+{
+    float xAcc = getAccelX();
+    float zAcc = getAccelZ();
+    float angInRads = (float) atan2(zAcc, xAcc);
+
+    return util.wrapAngle(((angInRads * 180.0) / M_PI)-180);
+}
+
+/*
+ * ======== getFilteredTiltAngle ========
+ * Returns the angle tilted with respect to vertical
+ *
+ * Calculated by combining the gyro angular velocity data
+ * and the accelerometer's tilt angle
+ */
+float IMUManager::getFilteredTiltAngle()
+{
+    filteredAngle_gyro = (COMPLEMENTARY_FILTER_WEIGHT * (filteredAngle_gyro + getGyroY()*0.0050)) 
+                         + ((1-COMPLEMENTARY_FILTER_WEIGHT) * getTiltAngle());
+    return filteredAngle_gyro;
+}
+
+
+
+
+
+
diff --git a/src/Energia/libraries/ZumoCC3200/utility/IMUManager.h b/src/Energia/libraries/ZumoCC3200/utility/IMUManager.h
new file mode 100644
index 0000000..9a4395a
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/IMUManager.h
@@ -0,0 +1,149 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef IMUManager_h
+#define IMUManager_h
+
+#include 
+#include "LSM303.h"
+#include "L3G.h"
+#include "Utilities.h"
+
+class IMUManager {
+public:
+    IMUManager();
+
+    static bool initGyro();
+    static bool initAccel();
+    static bool initMag();
+
+    static void enableGyroDefault();
+    static void enableAccelDefault();
+    static void enableMagDefault();
+
+    static void readGyro();
+    static void readAccel();
+    static void readMag();
+
+    static void calibrateGyro(int numSeconds);
+    static void calibrateAccelerometer(int numSeconds);
+    static void calibrateMagnetometer(int numSamples);
+
+    static float getGyroX();
+    static float getGyroY();
+    static float getGyroZ();
+
+    static float getGyroYaw();
+    static float getGyroPitch();
+    static float getGyroRoll();
+
+    static float getAccelX();
+    static float getAccelY();
+    static float getAccelZ();
+
+    static float getMagX();
+    static float getMagY();
+    static float getMagZ();
+    
+    static void zeroGyroXAxis();
+    static void zeroGyroYAxis();
+    static void zeroGyroZAxis();
+
+    static float getTiltAngle();
+    static float getFilteredTiltAngle(); 
+    
+    /* raw acceleration values as measured by the accelerometer */
+    static int accel_x;
+    static int accel_y;
+    static int accel_z;
+
+    /* raw angular velocity values as measured by the gyro */
+    static int gyro_x;
+    static int gyro_y;
+    static int gyro_z;
+
+    /* raw magnetic field values as measured by the magnetometer */
+    static int mag_x;
+    static int mag_y;
+    static int mag_z;
+private:
+    static LSM303 accel;
+    static L3G gyro;
+
+    static Utilities util;
+
+    /* gyro variables */
+    static float zAngle_gyro;
+    static float yAngle_gyro;
+    static float xAngle_gyro;
+
+    static float zReferenceAngle_gyro;
+    static float yReferenceAngle_gyro;
+    static float xReferenceAngle_gyro;
+
+    static int gyro_x_offset;
+    static int gyro_y_offset;
+    static int gyro_z_offset;
+    
+    static float avg_gyro_z_zero_offset;
+    static float avg_gyro_y_zero_offset;
+    static float avg_gyro_x_zero_offset;
+
+    static float tuningFactor_gyro;
+    static float filteredAngle_gyro;
+    
+    /* accelerometer variables */
+    static float accel_x_offset;
+    static float accel_y_offset;
+    static float accel_z_offset;
+
+    static bool finishedCalibration_accel;
+    static long initCalibrationTime_accel;
+
+    /* magnetometer variables */
+    static LSM303::vector mag_min;
+    static LSM303::vector mag_max;
+
+    /* constants: based on the default setup */
+    const static float GYRO_CONVERSION_FACTOR = (8.75f / 1000.0f);   /* deg/s */
+    const static float ACCEL_CONVERSION_FACTOR = (0.061f / 1000.0f); /* g */
+    const static float MAG_CONVERSION_FACTOR = (0.160f / 1000.0f);   /* gauss */
+
+    // const static float GYRO_SAMPLING_RATE 
+    // const static float ACCEL_SAMPLING_RATE 
+    // const static float MAG_SAMPLING_RATE 
+    
+    const static float COMPLEMENTARY_FILTER_WEIGHT = 0.95f;
+    const static int MAG_CALIBRATION_THRESHOLD = 9000;
+
+};
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/L3G.cpp b/src/Energia/libraries/ZumoCC3200/utility/L3G.cpp
new file mode 100644
index 0000000..cd65e26
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/L3G.cpp
@@ -0,0 +1,288 @@
+/*
+ *  ======== L3G.cpp ========
+ */
+
+#include 
+#include 
+
+#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 380 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);
+
+    // 0xAF = 0b10101111
+    //     DR = 10 (380 Hz ODR);
+    //     BW = 10 (50 Hz bandwidth);
+    //     PD = 1 (normal mode);
+    //     Zen = Yen = Xen = 1 (all axes enabled)
+    writeReg(CTRL_REG1, 0xAF);
+}
+
+/*
+ *  ======== 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 *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/libraries/ZumoCC3200/utility/L3G.h b/src/Energia/libraries/ZumoCC3200/utility/L3G.h
new file mode 100644
index 0000000..c80ec5b
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/L3G.h
@@ -0,0 +1,195 @@
+/*
+ *  ======== L3G ========
+ *  
+ *  Library used to communicate with and use the Pololu 
+ *  L3G 3-axis gyro. Code originally from https://github.com/pololu/l3g-arduino,
+ *  modified for additional features of the ZumoBot
+ */
+
+#ifndef L3G_h
+#define L3G_h
+
+#include  // for byte data type
+
+class L3G {
+public:
+    /*
+     *  ======== vector ========
+     */
+    template 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 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
+    static void vector_cross(const vector *a, const vector *b,
+            vector *out);
+
+    /*
+     *  ======== vector_dot ========
+     */
+    template
+    static float vector_dot(const vector *a, const vector *b);
+
+    /*
+     *  ======== vector_normalize ========
+     */
+    static void vector_normalize(vector *a);
+
+private:
+    deviceType _device; // chip type (D20H, D20, or 4200D)
+    byte address;
+    unsigned int io_timeout;
+    bool did_timeout;
+};
+
+/*
+ *  ======== vector_cross ========
+ */
+template
+void L3G::vector_cross(const vector *a, const vector *b,
+        vector *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
+float L3G::vector_dot(const vector *a, const vector *b)
+{
+    return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
+}
+
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/LSM303.cpp b/src/Energia/libraries/ZumoCC3200/utility/LSM303.cpp
new file mode 100644
index 0000000..ee72228
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/LSM303.cpp
@@ -0,0 +1,479 @@
+#include 
+#include 
+
+#include "LSM303.h"
+
+#include 
+
+// 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 ) { -32767, -32767, -32767 };
+    m_max = (LSM303::vector ) { +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) {
+        vector north = {1, 0, 0};
+        return heading(north);
+    }
+    else {
+        vector north = { 0, -1, 0 };
+        return heading(north);
+    }
+}
+
+void LSM303::vector_normalize(vector *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/libraries/ZumoCC3200/utility/LSM303.h b/src/Energia/libraries/ZumoCC3200/utility/LSM303.h
new file mode 100644
index 0000000..899f73b
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/LSM303.h
@@ -0,0 +1,356 @@
+/* 
+ *  ======== LSM303 ========
+ *
+ *  Library used to communicate with and use data from 
+ *  Pololu LSM303 accelerometer/magnetometer. Code originally from
+ *  https://github.com/pololu/lsm303-arduino, modified for some specific
+ *  usages in the context of controlling the ZumoBot.
+ */
+
+#ifndef LSM303_h
+#define LSM303_h
+
+#include  // for byte data type
+
+class LSM303
+{
+  public:
+    template  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 a;     // accelerometer readings
+    vector m;     // magnetometer readings
+    vector m_max; // maximum magnetometer values, used for calibration
+    vector 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  float heading(vector from);
+
+    /*
+     *  ======== vector_cross ========
+     */
+    template 
+        static void vector_cross(
+            const vector *a, const vector *b, vector *out);
+
+    /*
+     *  ======== vector_dot ========
+     */
+    template 
+        static float vector_dot(const vector *a, const vector *b);
+
+    /*
+     *  ======== vector_normalize ========
+     */
+    static void vector_normalize(vector *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  float LSM303::heading(vector from)
+{
+    vector 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 E;
+    vector 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 / 3.14159265358979323846;
+    if (heading < 0) {
+        heading += 360;
+    }
+    return heading;
+}
+
+/*
+ *  ======== vector_cross ========
+ *  To = Ta x Tb
+ */
+template 
+    void LSM303::vector_cross(
+        const vector *a, const vector *b, vector *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 
+    float LSM303::vector_dot(const vector *a, const vector *b)
+{
+    return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
+}
+
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/PIDController.cpp b/src/Energia/libraries/ZumoCC3200/utility/PIDController.cpp
new file mode 100644
index 0000000..da6ecdd
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/PIDController.cpp
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "PIDController.h"
+
+PIDController::PIDController(float p, float i, float d)
+{
+    m_P = p;
+    m_I = i;
+    m_D = d;
+
+    sumError = 0;
+    lastError = 0;
+    lastTime = 0;
+        
+    proportionalTerm = 0;
+    integralTerm = 0;
+    derivativeTerm = 0;
+}
+
+float PIDController::calculate(float error)
+{
+    float output;
+
+    /* compute and add the proportional term */
+    proportionalTerm = m_P * error;
+    output = proportionalTerm;
+
+    /* compute and add the derivative term */
+    unsigned long currTime = micros();
+    float dt = (currTime - lastTime) / 1000.0f;
+    derivativeTerm = m_D * ((error - lastError) / dt);
+    output += derivativeTerm;
+        
+    /* compute and add the integral term */
+    if ((error > 0 && lastError < 0) || (error < 0 && lastError > 0)) {
+        sumError = 0; /* auto-zero past integration sum */
+    }
+    sumError += error * dt;
+    integralTerm = sumError * m_I;
+    output += integralTerm;
+
+    /* update the previous values */
+    lastError = error;
+    lastTime = currTime;
+        
+    return (output);
+}
+
+float PIDController::getPContribution()
+{
+    return proportionalTerm; 
+}
+
+float PIDController::getIContribution()
+{
+    return integralTerm;
+}
+
+float PIDController::getDContribution()
+{
+    return derivativeTerm;
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/PIDController.h b/src/Energia/libraries/ZumoCC3200/utility/PIDController.h
new file mode 100644
index 0000000..2b87b04
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/PIDController.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PIDController_h
+#define PIDController_h
+
+#include 
+
+class PIDController
+{
+public:
+
+    PIDController(float p = 0, float i = 0, float d = 0);
+
+    float calculate(float error);
+
+    inline void setP(float newP) { m_P = newP;};
+    inline void setI(float newI) { m_I = newI;};
+    inline void setD(float newD) { m_D = newD;};
+
+    inline float getP() { return m_P;};
+    inline float getI() { return m_I;};
+    inline float getD() { return m_D;};
+
+    inline void clearIAccumulation() {sumError = 0;};
+    float getPContribution();
+    float getIContribution();
+    float getDContribution();
+
+private:
+    float m_P;
+    float m_I;
+    float m_D;
+    float lastError;
+    float sumError;
+    unsigned long lastTime;
+    float proportionalTerm;
+    float integralTerm;
+    float derivativeTerm;
+};
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/Pushbutton.cpp b/src/Energia/libraries/ZumoCC3200/utility/Pushbutton.cpp
new file mode 100644
index 0000000..18c2cd8
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/Pushbutton.cpp
@@ -0,0 +1,173 @@
+#include "Pushbutton.h"
+
+// constructor; takes arguments specifying whether to enable internal pull-up
+// and the default state of the pin that the button is connected to
+Pushbutton::Pushbutton(unsigned char pin, unsigned char pullUp, unsigned char defaultState)
+{
+  _pin = pin;
+  _pullUp = pullUp;
+  _defaultState = defaultState;
+  gsdpState = 0;
+  gsdrState = 0;
+  gsdpPrevTimeMillis = 0;
+  gsdrPrevTimeMillis = 0;
+  initialized = false;
+}
+
+// wait for button to be pressed
+void Pushbutton::waitForPress()
+{
+  init(); // initialize if necessary
+
+  do
+  {
+    while (!_isPressed()); // wait for button to be pressed
+    delay(10);            // debounce the button press
+  }
+  while (!_isPressed());   // if button isn't still pressed, loop
+}
+
+// wait for button to be released
+void Pushbutton::waitForRelease()
+{
+  init(); // initialize if necessary
+
+  do
+  {
+    while (_isPressed()); // wait for button to be released
+    delay(10);           // debounce the button release
+  }
+  while (_isPressed());   // if button isn't still released, loop
+}
+
+// wait for button to be pressed, then released
+void Pushbutton::waitForButton()
+{
+  waitForPress();
+  waitForRelease();
+}
+
+// indicates whether button is pressed
+boolean Pushbutton::isPressed()
+{
+  init(); // initialize if necessary
+
+  return _isPressed();
+}
+
+// Uses a finite state machine to detect a single button press and returns
+// true to indicate the press (false otherwise).  It requires the button to be
+// released for at least 15 ms and then pressed for at least 15 ms before
+// reporting the press.  This function handles all necessary debouncing and
+// should be called repeatedly in a loop.
+boolean Pushbutton::getSingleDebouncedPress()
+{
+  unsigned long timeMillis = millis();
+
+  init(); // initialize if necessary
+
+  switch (gsdpState)
+  {
+    case 0:
+      if (!_isPressed())                  // if button is released
+      {
+        gsdpPrevTimeMillis = timeMillis;
+        gsdpState = 1;                    // proceed to next state
+      }
+      break;
+
+    case 1:
+      if ((timeMillis - gsdpPrevTimeMillis >= 15) && !_isPressed()) // if 15 ms or longer has elapsed and button is still released
+        gsdpState = 2;                    // proceed to next state
+      else if (_isPressed())
+        gsdpState = 0;                    // button is pressed or bouncing, so go back to previous (initial) state
+      break;
+
+    case 2:
+      if (_isPressed())                   // if button is now pressed
+      {
+        gsdpPrevTimeMillis = timeMillis;
+        gsdpState = 3;                    // proceed to next state
+      }
+      break;
+
+    case 3:
+      if ((timeMillis - gsdpPrevTimeMillis >= 15) && _isPressed())  // if 15 ms or longer has elapsed and button is still pressed
+      {
+        gsdpState = 0;                    // next state becomes initial state
+        return true;                      // report button press
+      }
+      else if (!_isPressed())
+        gsdpState = 2;                    // button is released or bouncing, so go back to previous state
+      break;
+  }
+
+  return false;
+}
+
+// Uses a finite state machine to detect a single button release and returns
+// true to indicate the release (false otherwise).  It requires the button to be
+// pressed for at least 15 ms and then released for at least 15 ms before
+// reporting the release.  This function handles all necessary debouncing and
+// should be called repeatedly in a loop.
+boolean Pushbutton::getSingleDebouncedRelease()
+{
+  unsigned int timeMillis = millis();
+
+  init(); // initialize if necessary
+
+  switch (gsdrState)
+  {
+    case 0:
+      if (_isPressed())                   // if button is pressed
+      {
+        gsdrPrevTimeMillis = timeMillis;
+        gsdrState = 1;                    // proceed to next state
+      }
+      break;
+
+    case 1:
+      if ((timeMillis - gsdrPrevTimeMillis >= 15) && _isPressed())  // if 15 ms or longer has elapsed and button is still pressed
+        gsdrState = 2;                    // proceed to next state
+      else if (!_isPressed())
+        gsdrState = 0;                    // button is released or bouncing, so go back to previous (initial) state
+      break;
+
+    case 2:
+      if (!_isPressed())                  // if button is now released
+      {
+        gsdrPrevTimeMillis = timeMillis;
+        gsdrState = 3;                    // proceed to next state
+      }
+      break;
+
+    case 3:
+      if ((timeMillis - gsdrPrevTimeMillis >= 15) && !_isPressed()) // if 15 ms or longer has elapsed and button is still released
+      {
+        gsdrState = 0;                    // next state becomes initial state
+        return true;                      // report button release
+      }
+      else if (_isPressed())
+        gsdrState = 2;                    // button is pressed or bouncing, so go back to previous state
+      break;
+  }
+
+  return false;
+}
+
+// initializes I/O pin for use as button inputs
+void Pushbutton::init2()
+{
+  if (_pullUp == PULL_UP_ENABLED)
+    pinMode(_pin, INPUT_PULLUP);
+  else
+    pinMode(_pin, INPUT); // high impedance
+
+  delayMicroseconds(5); // give pull-up time to stabilize
+}
+
+// button is pressed if pin state differs from default state
+inline boolean Pushbutton::_isPressed()
+{
+  return (digitalRead(_pin) == LOW) ^ (_defaultState == DEFAULT_STATE_LOW);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/Pushbutton.h b/src/Energia/libraries/ZumoCC3200/utility/Pushbutton.h
new file mode 100644
index 0000000..9490f99
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/Pushbutton.h
@@ -0,0 +1,70 @@
+/*! \file Pushbutton.h
+ *
+ * See the Pushbutton class reference for more information about this library.
+ *
+ * \class Pushbutton Pushbutton.h
+ * \brief Read button presses and releases with debouncing
+ * 
+ */
+
+#ifndef Pushbutton_h
+#define Pushbutton_h
+
+#include 
+
+#define ZUMO_BUTTON 12
+
+#define PULL_UP_DISABLED    0
+#define PULL_UP_ENABLED     1
+
+#define DEFAULT_STATE_LOW   0
+#define DEFAULT_STATE_HIGH  1
+
+class Pushbutton
+{
+  public:
+
+    // constructor; takes arguments specifying whether to enable internal pull-up
+    // and the default state of the pin that the button is connected to
+    Pushbutton(unsigned char pin, unsigned char pullUp = PULL_UP_ENABLED, unsigned char defaultState = DEFAULT_STATE_HIGH);
+
+    // wait for button to be pressed, released, or pressed and released
+    void waitForPress();
+    void waitForRelease();
+    void waitForButton();
+
+    // indicates whether button is currently pressed
+    boolean isPressed();
+
+    // more complex functions that return true once for each button transition
+    // from released to pressed or pressed to released
+    boolean getSingleDebouncedPress();
+    boolean getSingleDebouncedRelease();
+
+  private:
+
+    unsigned char _pin;
+    unsigned char _pullUp;
+    unsigned char _defaultState;
+    unsigned char gsdpState;
+    unsigned char gsdrState;
+    unsigned int gsdpPrevTimeMillis;
+    unsigned int gsdrPrevTimeMillis;
+    boolean initialized;
+
+    inline void init()
+    {
+      if (!initialized)
+      {
+        initialized = true;
+        init2();
+      }
+    }
+
+    // initializes I/O pin for use as button input
+    void init2();
+
+    boolean _isPressed();
+};
+
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/TurnAngleCommand.cpp b/src/Energia/libraries/ZumoCC3200/utility/TurnAngleCommand.cpp
new file mode 100644
index 0000000..604b550
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/TurnAngleCommand.cpp
@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "TurnAngleCommand.h"
+#include "ZumoMotors.h"
+#include "IMUManager.h"
+
+TurnAngleCommand::TurnAngleCommand(float angle, float timeout)
+{
+    _angle = angle;
+    spinController = PIDController((float) 6.5, (float) 0.001, (float) 0.0);
+    numSeconds = timeout;
+    initialized = false;
+    targetAngleSet = false;
+    atAngle = false;
+}
+
+void TurnAngleCommand::run(Utilities::MotorInfo &info)
+{
+    float error;
+    float power;
+    if (!targetAngleSet) {
+        float currHeading = IMUManager::getGyroYaw();
+        targetAngle = Utilities::wrapAngle(currHeading + _angle);
+        targetAngleSet = true;
+        atAngle = false;
+    }
+    else {
+        error = Utilities::wrapAngle(IMUManager::getGyroYaw() - targetAngle);
+        power = spinController.calculate(fabs(error));
+        bool done = Utilities::inRange(fabs(error), (float) -10.0,
+                (float) 10.0);
+        if (done) {
+            atAngle = true;
+        }
+        if (atAngle) {
+            power = 0;
+        }
+        if (fabs(power) < 10) {
+            power = 0;
+        }
+        power = Utilities::clip(power);
+        /* Note: this is used because powers higher than 250 result
+         * in a buildup of error in the integrated gyro angle
+         */
+        if (power > 250) {
+            power = 250;
+        }
+        if (error > 0) {
+            ZumoMotors::setLeftSpeed(power);
+            ZumoMotors::setRightSpeed(-power);
+        }
+        else {
+            ZumoMotors::setLeftSpeed(-power);
+            ZumoMotors::setRightSpeed(power);
+        }
+        info.error = error;
+        info.leftSpeed = power;
+        info.rightSpeed = -power;
+        info.time = (float) millis();
+    }
+    timedOut = ((millis() - initTime) > ((int) (numSeconds * (float) 1000.0)));
+}
+
+void TurnAngleCommand::initialize()
+{
+    Utilities::reInitErrorVals();
+    initTime = millis();
+    initialized = true;
+    timedOut = false;
+    atAngle = false;
+    targetAngleSet = false;
+}
+
+void TurnAngleCommand::end()
+{
+    ZumoMotors::setLeftSpeed((int) 0);
+    ZumoMotors::setRightSpeed(0);
+    timedOut = true;
+    initialized = false;
+}
+
+bool TurnAngleCommand::isFinished()
+{
+    return timedOut || atAngle;
+}
+
+bool TurnAngleCommand::hasBeenInitialized()
+{
+    return initialized;
+}
+
+bool TurnAngleCommand::isTimedOut()
+{
+    return timedOut;
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/TurnAngleCommand.h b/src/Energia/libraries/ZumoCC3200/utility/TurnAngleCommand.h
new file mode 100644
index 0000000..486c7ec
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/TurnAngleCommand.h
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TurnAngleCommand_H
+#define TurnAngleCommand_H
+
+#include "PIDController.h"
+#include "Utilities.h"
+#include "Command.h"
+
+class TurnAngleCommand: public Command {
+
+public:
+    TurnAngleCommand(float angle = 0, float timeout = 0);
+
+    void run(Utilities::MotorInfo &info);
+    void initialize(void);
+    void end(void);
+    bool isFinished(void);
+    bool hasBeenInitialized(void);
+    bool isTimedOut(void);
+
+private:
+    float _angle;
+    float targetAngle;
+    float numSeconds;
+    long initTime;
+    bool timedOut;
+    bool initialized;
+    bool targetAngleSet;
+    bool atAngle;
+    PIDController spinController;
+};
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/Utilities.cpp b/src/Energia/libraries/ZumoCC3200/utility/Utilities.cpp
new file mode 100644
index 0000000..e839701
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/Utilities.cpp
@@ -0,0 +1,148 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "Utilities.h"
+#include 
+
+#ifndef M_PI
+#define M_PI    3.14159265358979323846
+#endif
+
+#define NUM_ERRORVALS 10
+
+static float errorVals[NUM_ERRORVALS] = {0};
+
+Utilities::Utilities()
+{
+    reInitErrorVals();
+}
+
+float Utilities::wrapAngle(float angle)
+{
+    angle = fmod(angle + 180, 360);
+    if (angle < 0) {
+        angle += 360;
+    }
+    return (angle - 180);
+}
+
+float Utilities::wrapAngle360(float angle)
+{
+    angle = wrapAngle(angle);
+    if (angle < 0) {
+        angle += 360;
+    }
+    return angle;
+}
+
+float Utilities::saturate(float value, float min, float max)
+{
+    if (value < min) {
+        value = min;
+    }
+    else if (value > max) {
+        value = max;
+    }
+    return value;
+}
+
+bool Utilities::inRange(float value, float lowerBound, float upperBound)
+{
+#if 0
+    float sum = 0;
+    for (int i = NUM_ERRORVALS - 1; i > 0; i--) {
+        errorVals[i] = errorVals[i - 1];
+        sum += errorVals[i];
+    }
+    sum += value;
+    errorVals[0] = value;
+    float avg = sum / NUM_ERRORVALS;
+    bool settled = (avg > lowerBound) && (avg < upperBound);
+    return (settled);
+#else
+    return ((value > lowerBound) && (value < upperBound));
+#endif
+}
+
+int Utilities::clip(int a)
+{
+    if (a > 400) {
+        a = 400;
+    }
+    if (a < -400) {
+        a = -400;
+    }
+    return a;
+}
+
+float Utilities::clip(float a)
+{
+    if (a > 400) {
+        a = 400;
+    }
+    if (a < -400) {
+        a = -400;
+    }
+    return a;
+}
+
+void Utilities::reInitErrorVals()
+{
+    for (int i = 0; i < (sizeof(errorVals) / sizeof(float)); i++) {
+        errorVals[i] = 0;
+    }
+}
+
+int Utilities::min(int a, int b)
+{
+    if (a < b) {
+        return a;
+    }
+    else {
+        return b;
+    }
+}
+
+int Utilities::max(int a, int b)
+{
+    if (a > b) {
+        return a;
+    }
+    else {
+        return b;
+    }
+}
+
+float Utilities::toDegrees(float angle)
+{
+    return float ((angle * 180.0f) / M_PI);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/Utilities.h b/src/Energia/libraries/ZumoCC3200/utility/Utilities.h
new file mode 100644
index 0000000..0196aa8
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/Utilities.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*  Utilities.h
+ *	
+ *  A class that includes a variety of useful "number - related"
+ *  operations.
+ */
+
+#ifndef Utilities_h
+#define Utilities_h
+
+class Utilities {
+public:
+    Utilities();
+
+    static float wrapAngle(float angle);
+    static float wrapAngle360(float angle);
+    static float saturate(float value, float min, float max);
+    static bool inRange(float value, float lowerBound, float upperBound);
+    static int clip(int input);
+    static float clip(float input);
+    static void reInitErrorVals();
+    static int min(int a, int b);
+    static int max(int a, int b);
+    static float toDegrees(float angle);
+
+    struct MotorInfo {
+        float error;
+        float leftSpeed;
+        float rightSpeed;
+        float time;
+    };
+private:
+};
+#endif
+
diff --git a/src/Energia/libraries/ZumoCC3200/utility/WaitCommand.cpp b/src/Energia/libraries/ZumoCC3200/utility/WaitCommand.cpp
new file mode 100644
index 0000000..494b0ff
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/WaitCommand.cpp
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include 
+#include "WaitCommand.h"
+
+WaitCommand::WaitCommand(float numSeconds)
+{
+    seconds = numSeconds;
+    timedOut = false;
+    initialized = false;
+}
+void WaitCommand::initialize()
+{
+    initTime = millis();
+    while (initTime < 0) {
+        initTime = millis();
+    }
+    initialized = true;
+}
+void WaitCommand::run(Utilities::MotorInfo &info)
+{
+    info.error = 0;
+    info.leftSpeed = 0;
+    info.rightSpeed = 0;
+    info.time = millis();
+}
+bool WaitCommand::isFinished()
+{
+    return isTimedOut();
+}
+bool WaitCommand::hasBeenInitialized()
+{
+    return initialized;
+}
+bool WaitCommand::isTimedOut()
+{
+    return ((millis() - initTime) > ((long) (seconds * 1000.0f)));
+}
+void WaitCommand::end()
+{
+    initialized = false;
+}
+
diff --git a/src/Energia/libraries/ZumoCC3200/utility/WaitCommand.h b/src/Energia/libraries/ZumoCC3200/utility/WaitCommand.h
new file mode 100644
index 0000000..a5556fd
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/WaitCommand.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2014, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef WaitCommand_h
+#define WaitCommand_h
+
+#include "Command.h"
+#include "Utilities.h"
+
+class WaitCommand: public Command {
+public:
+    WaitCommand(float numSeconds);
+    void run(Utilities::MotorInfo &info);
+    void initialize();
+    void end();
+    bool isFinished();
+    bool hasBeenInitialized();
+    bool isTimedOut();
+private:
+    long initTime;
+    float seconds;
+    bool timedOut;
+    bool initialized;
+};
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/ZumoMotors.cpp b/src/Energia/libraries/ZumoCC3200/utility/ZumoMotors.cpp
new file mode 100644
index 0000000..e6fc1a5
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/ZumoMotors.cpp
@@ -0,0 +1,138 @@
+#include 
+
+#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);
+}
+
+void ZumoMotors::setNormalizedSpeeds(float leftSpeed, float rightSpeed)
+{
+  setLeftSpeed((int)leftSpeed * 400);
+  setRightSpeed((int)rightSpeed * 400);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/ZumoMotors.h b/src/Energia/libraries/ZumoCC3200/utility/ZumoMotors.h
new file mode 100644
index 0000000..37ca761
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/ZumoMotors.h
@@ -0,0 +1,43 @@
+/*! \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  // required for boolean
+
+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);
+    static void setNormalizedSpeeds(float leftSpeed, float 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/libraries/ZumoCC3200/utility/spline.h b/src/Energia/libraries/ZumoCC3200/utility/spline.h
new file mode 100644
index 0000000..dbd65bf
--- /dev/null
+++ b/src/Energia/libraries/ZumoCC3200/utility/spline.h
@@ -0,0 +1,404 @@
+/*
+ * spline.h
+ *
+ * simple cubic spline interpolation library without external
+ * dependencies
+ *
+ * ---------------------------------------------------------------------
+ * Copyright (C) 2011, 2014 Tino Kluge (ttk448 at gmail.com)
+ *
+ *  This program is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License
+ *  as published by the Free Software Foundation; either version 2
+ *  of the License, or (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see .
+ * ---------------------------------------------------------------------
+ *
+ */
+
+#ifndef TK_SPLINE_H
+#define TK_SPLINE_H
+
+#include 
+#include 
+#include 
+#include 
+
+
+// unnamed namespace only because the implementation is in this
+// header file and we don't want to export symbols to the obj files
+namespace
+{
+
+namespace tk
+{
+
+// band matrix solver
+class band_matrix
+{
+private:
+    std::vector< std::vector > m_upper;  // upper band
+    std::vector< std::vector > m_lower;  // lower band
+public:
+    band_matrix() {};                             // constructor
+    band_matrix(int dim, int n_u, int n_l);       // constructor
+    ~band_matrix() {};                            // destructor
+    void resize(int dim, int n_u, int n_l);      // init with dim,n_u,n_l
+    int dim() const;                             // matrix dimension
+    int num_upper() const
+    {
+        return m_upper.size()-1;
+    }
+    int num_lower() const
+    {
+        return m_lower.size()-1;
+    }
+    // access operator
+    double & operator () (int i, int j);            // write
+    double   operator () (int i, int j) const;      // read
+    // we can store an additional diogonal (in m_lower)
+    double& saved_diag(int i);
+    double  saved_diag(int i) const;
+    void lu_decompose();
+    std::vector r_solve(const std::vector& b) const;
+    std::vector l_solve(const std::vector& b) const;
+    std::vector lu_solve(const std::vector& b,
+                                 bool is_lu_decomposed=false);
+
+};
+
+
+// spline interpolation
+class spline
+{
+public:
+    enum bd_type {
+        first_deriv = 1,
+        second_deriv = 2
+    };
+
+private:
+    std::vector m_x,m_y;            // x,y coordinates of points
+    // interpolation parameters
+    // f(x) = a*(x-x_i)^3 + b*(x-x_i)^2 + c*(x-x_i) + y_i
+    std::vector m_a,m_b,m_c;        // spline coefficients
+    double  m_b0, m_c0;                     // for left extrapol
+    bd_type m_left, m_right;
+    double  m_left_value, m_right_value;
+    bool    m_force_linear_extrapolation;
+
+public:
+    // set default boundary condition to be zero curvature at both ends
+    spline(): m_left(second_deriv), m_right(second_deriv),
+        m_left_value(0.0), m_right_value(0.0),
+        m_force_linear_extrapolation(false)
+    {
+        ;
+    }
+
+    // optional, but if called it has to come be before set_points()
+    void set_boundary(bd_type left, double left_value,
+                      bd_type right, double right_value,
+                      bool force_linear_extrapolation=false);
+    void set_points(const std::vector& x,
+                    const std::vector& y, bool cubic_spline=true);
+    double operator() (double x) const;
+};
+
+
+
+// ---------------------------------------------------------------------
+// implementation part, which could be separated into a cpp file
+// ---------------------------------------------------------------------
+
+
+// band_matrix implementation
+// -------------------------
+
+band_matrix::band_matrix(int dim, int n_u, int n_l)
+{
+    resize(dim, n_u, n_l);
+}
+void band_matrix::resize(int dim, int n_u, int n_l)
+{
+    assert(dim>0);
+    assert(n_u>=0);
+    assert(n_l>=0);
+    m_upper.resize(n_u+1);
+    m_lower.resize(n_l+1);
+    for(size_t i=0; i0) {
+        return m_upper[0].size();
+    } else {
+        return 0;
+    }
+}
+
+
+// defines the new operator (), so that we can access the elements
+// by A(i,j), index going from i=0,...,dim()-1
+double & band_matrix::operator () (int i, int j)
+{
+    int k=j-i;       // what band is the entry
+    assert( (i>=0) && (i=0) && (j diogonal, k<0 lower left part, k>0 upper right part
+    if(k>=0)   return m_upper[k][i];
+    else	    return m_lower[-k][i];
+}
+double band_matrix::operator () (int i, int j) const
+{
+    int k=j-i;       // what band is the entry
+    assert( (i>=0) && (i=0) && (j diogonal, k<0 lower left part, k>0 upper right part
+    if(k>=0)   return m_upper[k][i];
+    else	    return m_lower[-k][i];
+}
+// second diag (used in LU decomposition), saved in m_lower
+double band_matrix::saved_diag(int i) const
+{
+    assert( (i>=0) && (i=0) && (idim(); i++) {
+        assert(this->operator()(i,i)!=0.0);
+        this->saved_diag(i)=1.0/this->operator()(i,i);
+        j_min=std::max(0,i-this->num_lower());
+        j_max=std::min(this->dim()-1,i+this->num_upper());
+        for(int j=j_min; j<=j_max; j++) {
+            this->operator()(i,j) *= this->saved_diag(i);
+        }
+        this->operator()(i,i)=1.0;          // prevents rounding errors
+    }
+
+    // Gauss LR-Decomposition
+    for(int k=0; kdim(); k++) {
+        i_max=std::min(this->dim()-1,k+this->num_lower());  // num_lower not a mistake!
+        for(int i=k+1; i<=i_max; i++) {
+            assert(this->operator()(k,k)!=0.0);
+            x=-this->operator()(i,k)/this->operator()(k,k);
+            this->operator()(i,k)=-x;                         // assembly part of L
+            j_max=std::min(this->dim()-1,k+this->num_upper());
+            for(int j=k+1; j<=j_max; j++) {
+                // assembly part of R
+                this->operator()(i,j)=this->operator()(i,j)+x*this->operator()(k,j);
+            }
+        }
+    }
+}
+// solves Ly=b
+std::vector band_matrix::l_solve(const std::vector& b) const
+{
+    assert( this->dim()==(int)b.size() );
+    std::vector x(this->dim());
+    int j_start;
+    double sum;
+    for(int i=0; idim(); i++) {
+        sum=0;
+        j_start=std::max(0,i-this->num_lower());
+        for(int j=j_start; joperator()(i,j)*x[j];
+        x[i]=(b[i]*this->saved_diag(i)) - sum;
+    }
+    return x;
+}
+// solves Rx=y
+std::vector band_matrix::r_solve(const std::vector& b) const
+{
+    assert( this->dim()==(int)b.size() );
+    std::vector x(this->dim());
+    int j_stop;
+    double sum;
+    for(int i=this->dim()-1; i>=0; i--) {
+        sum=0;
+        j_stop=std::min(this->dim()-1,i+this->num_upper());
+        for(int j=i+1; j<=j_stop; j++) sum += this->operator()(i,j)*x[j];
+        x[i]=( b[i] - sum ) / this->operator()(i,i);
+    }
+    return x;
+}
+
+std::vector band_matrix::lu_solve(const std::vector& b,
+        bool is_lu_decomposed)
+{
+    assert( this->dim()==(int)b.size() );
+    std::vector  x,y;
+    if(is_lu_decomposed==false) {
+        this->lu_decompose();
+    }
+    y=this->l_solve(b);
+    x=this->r_solve(y);
+    return x;
+}
+
+
+
+
+// spline implementation
+// -----------------------
+
+void spline::set_boundary(spline::bd_type left, double left_value,
+                          spline::bd_type right, double right_value,
+                          bool force_linear_extrapolation)
+{
+    assert(m_x.size()==0);          // set_points() must not have happened yet
+    m_left=left;
+    m_right=right;
+    m_left_value=left_value;
+    m_right_value=right_value;
+    m_force_linear_extrapolation=force_linear_extrapolation;
+}
+
+
+void spline::set_points(const std::vector& x,
+                        const std::vector& y, bool cubic_spline)
+{
+    assert(x.size()==y.size());
+    assert(x.size()>2);
+    m_x=x;
+    m_y=y;
+    int   n=x.size();
+    // TODO: maybe sort x and y, rather than returning an error
+    for(int i=0; i  rhs(n);
+        for(int i=1; i::const_iterator it;
+    it=std::lower_bound(m_x.begin(),m_x.end(),x);
+    int idx=std::max( int(it-m_x.begin())-1, 0);
+
+    double h=x-m_x[idx];
+    double interpol;
+    if(xm_x[n-1]) {
+        // extrapolation to the right
+        interpol=(m_b[n-1]*h + m_c[n-1])*h + m_y[n-1];
+    } else {
+        // interpolation
+        interpol=((m_a[idx]*h + m_b[idx])*h + m_c[idx])*h + m_y[idx];
+    }
+    return interpol;
+}
+
+
+} // namespace tk
+
+
+} // namespace
+
+#endif /* TK_SPLINE_H */
+
diff --git a/src/Processing/AssistedDrive/AssistedDrive.pde b/src/Processing/AssistedDrive/AssistedDrive.pde
new file mode 100644
index 0000000..7f32535
--- /dev/null
+++ b/src/Processing/AssistedDrive/AssistedDrive.pde
@@ -0,0 +1,704 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * RedBear Zumo Bot interactive control with real-time acceleration display
+ * 
+ * Before running this program, start the zumo bot and connect to its
+ * WiFi network.
+ *
+ * Download the required G4P library at http://www.lagers.org.uk/g4p/
+ */
+
+import processing.net.Client;
+
+import g4p_controls.GButton;
+import g4p_controls.GWindow;
+import g4p_controls.GCScheme;
+import g4p_controls.GEvent;
+import g4p_controls.GDropList;
+import g4p_controls.GEditableTextControl;
+
+GButton forward, back, left, right, stop, driveStraight, turnAngle,
+        accelerometerData, gyroData, magData, gyroAngleData, errorData,
+        motorData, continuousGraphingToggle, logToggle, zeroGyro;
+GWindow controlWindow, graphSelector;
+
+Client client; /* data socket connected to zumo IMU data server */
+
+/* IMU data from zumo */
+char[] imuBuffer = new char[120];
+
+int MAX_FPS = 30;    /* max frames/sec, i.e., rate of calls to draw() */
+int MAX_XVALS = 200;  /* max # of samples in the x-axis */
+int MAX_PENDING = 6; /* max # of outstanding commands to send */
+
+String LOG_NAME = "zumo_log.txt";
+char command = ' ';
+char lastCommand = ' ';
+
+boolean buttonPressed = false;
+boolean mainWindowLocationSet = false;
+volatile boolean textEntered = false;
+byte[] driveInfoToSend;
+byte[] angleToSend;
+
+/* IMU data graph object */
+Graph graph1 = new Graph(430, 230, 475, 275, color(200, 20, 20));
+
+/* IMU data values to graph */
+float[] time = {
+    0
+};
+float[] ax = {
+    0
+};
+float[] ay = {
+    0
+};
+float[] az = {
+    0
+};
+
+char curGraph = 'A';
+int curGraphOffset = 1;
+boolean scaleData = true;
+long startMillis = 0;
+float curTime = 0;
+byte[] driveCmd;
+byte[] angleCmd;
+boolean logRunning = false;
+PrintWriter log = null; /* optional data log */
+int packetNum = 0;
+
+/*
+ *  ======== setup ========
+ */
+void setup()
+{
+    /* create minimal text output window for acc data */
+    size(1100, 600);
+
+    /* Make controls window and buttons */
+    forward = new GButton(this, 120, 200, 80, 80, "Forward");
+    back = new GButton(this, 120, 300, 80, 80, "Backward");
+    left = new GButton(this, 25, 300, 80, 80, "Left");
+    right = new GButton(this, 215, 300, 80, 80, "Right");
+    stop = new GButton(this, 25, 390, 270, 50, "STOP");
+    stop.setLocalColorScheme(GCScheme.RED_SCHEME);
+
+    driveStraight = new GButton(this, 25, 460, 125, 80,
+            "Enable Assisted Drive");
+    driveStraight.setLocalColorScheme(GCScheme.GREEN_SCHEME);
+
+    turnAngle = new GButton(this, 170, 460, 125, 80, "Turn 90 degrees");
+    turnAngle.setLocalColorScheme(GCScheme.CYAN_SCHEME);
+
+    accelerometerData = new GButton(this, 300, 50, 83, 83,
+            "Accelerometer Data");
+    accelerometerData.setLocalColorScheme(GCScheme.CYAN_SCHEME);
+    gyroData = new GButton(this, 400, 50, 83, 83, "Gyro Raw Data");
+    magData = new GButton(this, 500, 50, 83, 83, "Magnetometer Data");
+    magData.setLocalColorScheme(GCScheme.GREEN_SCHEME);
+    gyroAngleData = new GButton(this, 600, 50, 83, 83,
+            "Integrated Gyro Angle Data");
+    gyroAngleData.setLocalColorScheme(GCScheme.BLUE_SCHEME);
+    errorData = new GButton(this, 700, 50, 83, 83, "PID Error Data");
+    errorData.setLocalColorScheme(GCScheme.YELLOW_SCHEME);
+    motorData = new GButton(this, 800, 50, 83, 83, "Motor Power Data");
+    motorData.setLocalColorScheme(GCScheme.RED_SCHEME);
+
+    continuousGraphingToggle = new GButton(this, 900, 50, 83, 83,
+            "Continous Data");
+    continuousGraphingToggle.setLocalColorScheme(GCScheme.GREEN_SCHEME);
+
+    logToggle = new GButton(this, 85, 40, 100, 100, "Logger turned off");
+    logToggle.setLocalColorScheme(GCScheme.RED_SCHEME);
+    zeroGyro = new GButton(this, 1000, 50, 83, 83, "Zero Gyro");
+    zeroGyro.setLocalColorScheme(GCScheme.BLUE_SCHEME);
+
+    graph1.xLabel = "Time (s)";
+    graph1.yLabel = "Acceleration (m/s^2)";
+    graph1.Title = "Acceleration: (x, y, z) vs. t";
+    graph1.yMin = 0;
+    graph1.yMax = 0;
+
+    /* slow the draw() rate down to MAX_FPS frames/sec */
+    frameRate(MAX_FPS);
+    
+    angleToSend = new byte[3];
+    driveInfoToSend = new byte[2];
+    driveCmd = new byte[4];
+    angleCmd = new byte[5];
+
+    /* Connect to zumo's command server IP address and port */
+    client = new Client(this, "192.168.1.1", 8080);
+}
+
+/*
+ *  ======== draw ========
+ */
+boolean cont = false; /* continuously send previous command to get IMU data */
+String pcmd = " \n";  /* previous command */
+int pending = 0;      /* number of outstanding commands sent to zumo bot */
+
+void draw()
+{
+    if (!mainWindowLocationSet) {
+        mainWindowLocationSet = true;
+        frame.setLocation(600, 300);
+    }
+
+    /* send server commands based on keyboard and/or button input */
+    if (keyPressed || cont || buttonPressed) {
+        if (startMillis == 0) {
+            /* capture initial start time */
+            startMillis = java.lang.System.currentTimeMillis();
+        }
+        String cmd = pcmd;
+
+        if (keyPressed) {
+            /* handle keyboard input by converting to a button command  */
+            char tmp;
+            if ((tmp = key2Command()) != '\0') {
+                command = tmp;
+            }
+        }
+
+        if (command == ' ' || command == '.'
+                || command == 'w' || command == 's' || command == 'd'
+                || command == 'a' || command == 'g' || command == 't'
+                || command == 'z') {
+            buttonPressed = false;
+            
+            if (command == 'g') {
+                driveCmd[0] = 'g';
+                driveCmd[1] = driveInfoToSend[0];
+                driveCmd[2] = driveInfoToSend[1];
+                driveCmd[3] = '\n';
+            }
+            else if (command == 't') {
+                angleCmd[0] = 't';
+                angleCmd[1] = angleToSend[0];
+                angleCmd[2] = angleToSend[1];
+                angleCmd[3] = angleToSend[2];
+                angleCmd[4] = '\n';
+            }
+            else {
+                cmd = command + "\n";
+            }
+        }
+        else if (command == 'L') {
+            if (log == null) {
+                log = createWriter(LOG_NAME);
+                println("logging started ...");
+            }
+        }
+        else if (command == 'l') {
+            if (log != null) {
+                log.flush();
+                log.close();
+                log = null;
+                println("logging stopped.");
+            }
+        }
+        else if (command == 'G') {
+            if (curGraph != 'G') {
+                curGraph = 'G';
+                curGraphOffset = 5;
+                graph1.yLabel = "Rotational Speed (r/s)";
+                graph1.Title = " Gyro: (x, y, z) vs. t ";
+                graph1.yMin = 0;
+                graph1.yMax = 0;
+                ax = new float[] { 0 };
+                ay = new float[] { 0 };
+                az = new float[] { 0 };
+                time = new float[] { curTime };
+            }
+        }
+        else if (command == 'A') {
+            if (curGraph != 'A') {
+                curGraph = 'A';
+                curGraphOffset = 1;
+                graph1.yLabel = "Acceleration (m/s^2)";
+                graph1.Title = " Acceleration: (x, y, z) vs. t ";
+                graph1.yMin = 0;
+                graph1.yMax = 0;
+                ax = new float[] { 0 };
+                ay = new float[] { 0 };
+                az = new float[] { 0 };
+                time = new float[] { curTime };
+            }
+        }
+        else if (command == 'M') {
+            if (curGraph != 'M') {
+                curGraph = 'M';
+                curGraphOffset = 9;
+                graph1.yLabel = "Magnetometer Data (normalized)";
+                graph1.Title = " Magnetometer: (x, y, z) vs. t ";
+                graph1.yMin = -1.0;
+                graph1.yMax = 1.0;
+                ax = new float[] { 0 };
+                ay = new float[] { 0 };
+                az = new float[] { 0 };
+                time = new float[] { curTime };
+            }
+        }
+        else if (command == 'N') {
+            if (curGraph != 'N') {
+                curGraph = 'N';
+                curGraphOffset = 13;
+                graph1.yLabel = "Gyro Angle (degrees)";
+                graph1.Title = "Gyro Angle";
+                graph1.yMin = -180;
+                graph1.yMax = 180;
+                ax = new float[] { 0 };
+                ay = new float[] { 0 };
+                az = new float[] { 0 };
+                time = new float[] { curTime };
+            }
+        }
+        else if (command == 'e') {
+            if (curGraph != 'e') {
+                curGraph = 'e';
+                curGraphOffset = 15;
+                graph1.yLabel = "Error (degrees)";
+                graph1.Title = "Error";
+                graph1.yMin = 0;
+                graph1.yMax = 0;
+                ax = new float[] { 0 };
+                ay = new float[] { 0 };
+                az = new float[] { 0 };
+                time = new float[] { curTime };
+            }
+        }
+        else if (command == 'm') {
+            if (curGraph != 'm') {
+                curGraph = 'm';
+                curGraphOffset = 16;
+                graph1.yLabel = "Motor power";
+                graph1.Title = "Motor power (red = left, green = right)";
+                graph1.yMin = -400;
+                graph1.yMax = 400;
+                ax = new float[] { 0 };
+                ay = new float[] { 0 };
+                az = new float[] { 0 };
+                time = new float[] { curTime };
+            }
+        }
+        buttonPressed = false;
+
+        /* stop sending commands if too many commands are outstanding */
+        if (pending < MAX_PENDING) {
+            pending++;
+            if (command == 'g') {
+                client.write(driveCmd);
+                command = '.';
+            }
+            else if (command == 't') {
+                client.write(angleCmd);
+                command = '.';
+            }
+            else if (command == 'z') {
+                client.write(cmd);
+                command = '.';
+            }
+            else {
+                client.write(cmd);
+                pcmd = cmd;
+            }
+        }
+    }
+
+    /* Read IMU data from server and display it */
+    if (client.available() >= imuBuffer.length) {
+
+        /* read IMU data */
+        packetNum++;
+        for (int i = 0; i < imuBuffer.length; i++) {
+            imuBuffer[i] = client.readChar();
+        }
+
+        /* decrement pending count so we can send more commands */
+        if (--pending < 0) {
+            // println("Yikes! pending = " + pending);
+            pending = 0;
+        }
+
+        /* parse IMU data and display it */
+        String input = new String(imuBuffer);
+        String [] tokens = splitTokens(input, " ");
+        if (packetNum % 100 == 0) {
+            for (int i = 0; i < tokens.length; i++) {
+                print(tokens[i] + " ");
+            }
+            println("");
+            //println(input);
+        }
+        if (curGraph != 'N') {
+            if (tokens.length > (2 + curGraphOffset)) {
+                newPoint(tokens[curGraphOffset], /* x value */
+                    tokens[1 + curGraphOffset],  /* y value */
+                    tokens[2 + curGraphOffset],  /* z value */
+                    tokens[18]);                 /* time */
+            }
+        }
+        else if (tokens.length > curGraphOffset) {
+            newPoint(Float.toString(Float.parseFloat(tokens[curGraphOffset])),
+                    "0",         /* y value */
+                    "0",         /* z value */
+                    tokens[18]); /* time */
+        }
+    }
+}
+
+/*
+ *  ======== key2Command ========
+ *  convert key press to an appropriate button command
+ */
+char key2Command()
+{
+    if (key == ' ' || key == 'w' || key == 's' || key == 'd' || key == 'a') {
+        return (key);
+    }
+    else if (key == CODED) { /* handle arrow keys */
+        switch (keyCode) {
+            case UP:
+                return ('w');
+            case DOWN:
+                return ('s');
+            case LEFT:
+                return ('a');
+            case RIGHT:
+                return ('d');
+        }
+    }
+    return ('\0');
+}
+
+/*
+ *  ======== newPoint ========
+ *  add new point to data arrays and update graph
+ */
+void newPoint(String x, String y, String z, String timestamp)
+{
+    /* get next values */
+    curTime = (java.lang.System.currentTimeMillis() - startMillis) / 1000.0;
+    if (log != null) {
+        log.println("A: " + x + " " + y + " " + z);
+    }
+    if (curGraph == 'e') {
+        time = pushv(time, curTime);
+        ax = pushv(ax, scale(curGraph, x));
+        ay = pushv(ay, (float) 0);
+        az = pushv(az, (float) 0);
+    }
+    else if (curGraph == 'm') {
+        time = pushv(time, curTime);
+        ax = pushv(ax, scale(curGraph, x));
+        ay = pushv(ay, scale(curGraph, y));
+        az = pushv(az, 0.0);
+    }
+    else {
+        /* update all data arrays */
+        time = pushv(time, curTime);
+        ax = pushv(ax, scale(curGraph, x));
+        ay = pushv(ay, scale(curGraph, y));
+        az = pushv(az, scale(curGraph, z));
+    }
+
+    /* update display */
+    updatePlots();
+}
+
+/*
+ *  ======== pushv ========
+ *  append new data sample to arr[] and return new array
+ */
+float[] pushv(float [] arr, float val)
+{
+    float [] tmp = arr;
+
+    if (arr.length < MAX_XVALS) {
+        tmp = append(arr, val);
+    }
+    else {
+        /* shift data within arr to make room for new value */
+        int i;
+        for (i = 1; i < MAX_XVALS; i++) {
+            arr[i - 1] = arr[i];
+        }
+        arr[MAX_XVALS - 1] = val;
+    }
+
+    return (tmp);
+}
+
+/*
+ *  ======== updatePlots ========
+ *  redraw IMU graph
+ */
+void updatePlots()
+{
+    if (curGraph == 'e') {
+        //only graph that matters is x (the error)
+        background(200);
+
+        graph1.xMax = max(time);
+        graph1.xMin = min(time);
+
+        graph1.DrawAxis();
+        graph1.GraphColor = color(200, 40, 40);
+        graph1.LineGraph(time, ax);
+    }
+    else if (curGraph == 'm') {
+        //data that matters is x and y (left and right motor speeds)
+        background(200);
+
+        graph1.xMax = max(time);
+        graph1.xMin = min(time);
+
+        graph1.DrawAxis();
+
+        graph1.GraphColor = color(200, 40, 40);
+        graph1.LineGraph(time, ax);
+
+        graph1.GraphColor = color(40, 200, 40);
+        graph1.LineGraph(time, ay);
+    }
+
+    background(200);
+
+    graph1.xMax = max(time);
+    graph1.xMin = min(time);
+    graph1.yMax = max(max(max(az), max(ax), max(ay)), graph1.yMax);
+    graph1.yMin = min(min(min(az), min(ax), min(ay)), graph1.yMin);
+
+    graph1.DrawAxis();
+
+    graph1.GraphColor = color(200, 40, 40);
+    graph1.LineGraph(time, ax);
+
+    graph1.GraphColor = color(40, 200, 40);
+    graph1.LineGraph(time, ay);
+
+    graph1.GraphColor = color(40, 40, 200);
+    graph1.LineGraph(time, az);
+}
+
+/*
+ *  ======== scale =========
+ */
+float scale(char type, String value)
+{
+    if (scaleData) {
+        switch (type) {
+            case 'A':
+                return (float(value) * (2 * 9.80665) / float(32768)); /* acc data is +-2G */
+
+            case 'G':
+                return (float(value) * 250 / float(32768));           /* gyro data is +-250 deg/sec */
+            
+        }
+    }
+    return (float(value) / float(100));
+}
+
+/*
+ *  ======== handleButtonEvents ========
+ */
+boolean imuDrive = false;
+public void handleButtonEvents(GButton button, GEvent event) 
+{
+    buttonPressed = true;
+
+    if (button == forward) {
+        if (imuDrive) {
+            lastCommand = command;
+            processDriveTimeString("50", true);
+            command = 'g';
+        }
+        else {
+            command = 'w';
+        }
+    }
+    else if (button == back) {
+        if (imuDrive) {
+            lastCommand = command;
+            processDriveTimeString("50", false);
+            command = 'g';
+        }
+        else {
+            command = 's';
+        }
+    }
+    else if (button == right) {
+        command = 'd';
+    }
+    else if (button == left) {
+        command = 'a';
+    }
+    else if (button == stop) {
+        command = ' ';
+    }
+    else if (button == zeroGyro) {
+        command = 'z';
+    }
+    else if (button == driveStraight) {
+        if (!imuDrive) {
+            driveStraight.setText("Disable Assisted Drive");
+            driveStraight.setLocalColorScheme(GCScheme.RED_SCHEME);
+        }
+        else {
+            driveStraight.setText("Enable Assisted Drive");
+            driveStraight.setLocalColorScheme(GCScheme.GREEN_SCHEME);
+        }
+        imuDrive = !imuDrive;
+    }
+    else if (button == turnAngle) {
+        lastCommand = command;
+        command = 't';
+        processAngleString("90", true);
+    }
+    else if (button == accelerometerData) {
+        command = 'A';
+    }
+    else if (button == gyroData) {
+        command ='G';
+    }
+    else if (button == magData) {
+        command = 'M';
+    }
+    else if (button == gyroAngleData) {
+        command = 'N';
+    }
+    else if (button == errorData) {
+        command = 'e';
+    }
+    else if (button == motorData) {
+        command = 'm';
+    }
+    else if (button == continuousGraphingToggle) {
+        if (!cont) {
+            continuousGraphingToggle.setText("Non-continous Data");
+            continuousGraphingToggle.setLocalColorScheme(GCScheme.RED_SCHEME);
+        }
+        else {
+            continuousGraphingToggle.setText("Continuous Data");
+            continuousGraphingToggle.setLocalColorScheme(GCScheme.GREEN_SCHEME);
+        }
+        cont = !cont;
+    }
+    else if (button == logToggle) {
+        if (!logRunning) {
+            logToggle.setText("Logger started");
+            logToggle.setLocalColorScheme(GCScheme.GREEN_SCHEME);
+            logRunning = true;
+            command = 'l';
+        }
+        else {
+            logToggle.setText("Logger not running");
+            logToggle.setLocalColorScheme(GCScheme.RED_SCHEME);
+            logRunning = false;
+            command = 'L';
+        }
+    }
+}
+
+/*
+ *  ======== processDriveTimeString ========
+ *  Pack driveInfoToSend array with the data associated with the
+ *  "drive straight" button.
+ *      driveInfoToSend[0] - 0-127 forward, 128-255 reverse
+ *      driveInfoToSend[1] - duration in seconds (between 0 and 255)
+ */
+public void processDriveTimeString(String input, boolean forward) 
+{
+    double time = Double.parseDouble(input); //parse the input as a double
+    if (time < 0.0) { //can't be less than a second
+        time = 0.00;
+    }
+    else if (time > 255) {
+      time = 255;
+    }
+    
+    int tmp = (int)time;
+    driveInfoToSend[1] = (byte)tmp;
+    driveInfoToSend[0] = (byte)(forward ? 64 : -64);
+}
+
+/*
+ *  ======== processAngleString ========
+ *  Pack angleToSend array with the data associated with the
+ *  "turn" button.
+ *      angleToSend[0]   - 1 right, 0 left
+ *      angleToSend[1:2] - low-byte, high byte of angle
+ */
+public void processAngleString(String input, boolean right) 
+{
+    double angle = Double.parseDouble(input);
+    if (angle > 360.0) {
+        angle = 360.0;
+    }
+
+    int ang = (int)angle;
+    byte low = (byte)(ang & 0xFF);
+    byte high = (byte)((ang >>8) & 0xFF);
+    angleToSend[0] = (byte)(right ? 64 : -64);
+    angleToSend[1] = low;
+    angleToSend[2] = high;
+}
+
+/*
+ *  ======== controlWindowHandler ========
+ */
+void controlWindowHandler(GWindow window)
+{
+    command = ' '; // ensure the tank doesn't continue driving without control
+}
+
+/*
+ *  ======== handleDropListEvents ========
+ */
+public void handleDropListEvents(GDropList list, GEvent event)
+{
+}
+
+/*
+ *  ======== handleTextEvents ========
+ */
+public void handleTextEvents(GEditableTextControl textcontrol, GEvent event)
+{
+    if (event == GEvent.ENTERED) {
+        textEntered = true;
+    }
+}
diff --git a/src/Processing/AssistedDrive/GraphClass.pde b/src/Processing/AssistedDrive/GraphClass.pde
new file mode 100644
index 0000000..39f9b7c
--- /dev/null
+++ b/src/Processing/AssistedDrive/GraphClass.pde
@@ -0,0 +1,371 @@
+/*
+ * from: https://github.com/sebnil/RealtimePlotter
+ *
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2013 Sebatian Nilsson
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/*   =================================================================================       
+     The Graph class contains functions and variables that have been created to draw 
+     graphs. Here is a quick list of functions within the graph class:
+          
+     Graph(int x, int y, int w, int h,color k)
+     DrawAxis()
+     Bar([])
+     smoothLine([][])
+     DotGraph([][])
+     LineGraph([][]) 
+     
+     =================================================================================*/   
+
+    
+class Graph 
+{
+      
+    boolean Dot=true;            // Draw dots at each data point if true
+    boolean RightAxis;            // Draw the next graph using the right axis if true
+    boolean ErrorFlag=false;      // If the time array isn't in ascending order, make true  
+    boolean ShowMouseLines=true;  // Draw lines and give values of the mouse position
+    
+    int     xDiv=10,yDiv=5;            // Number of sub divisions
+    int     xPos,yPos;            // location of the top left corner of the graph  
+    int     Width,Height;         // Width and height of the graph
+    
+
+    color   GraphColor;
+    color   BackgroundColor=color(255);  
+    color   StrokeColor=color(180);     
+      
+    String  Title="Title";          // Default titles
+    String  xLabel="x - Label";
+    String  yLabel="y - Label";
+
+    float   yMax=1024, yMin=0;      // Default axis dimensions
+    float   xMax=10, xMin=0;
+    float   yMaxRight=1024,yMinRight=0;
+  
+    PFont   Font;                   // Selected font used for text 
+      
+    //    int Peakcounter=0,nPeakcounter=0;
+     
+    Graph(int x, int y, int w, int h, color k) {  // The main declaration function
+        xPos = x;
+        yPos = y;
+        Width = w;
+        Height = h;
+        GraphColor = k;
+    }
+     
+    void DrawAxis(){
+       
+        /*  =========================================================================================
+            Main axes Lines, Graph Labels, Graph Background
+            ==========================================================================================  */
+    
+        fill(BackgroundColor); color(0);stroke(StrokeColor);strokeWeight(1);
+        int t=60;
+        
+        rect(xPos-t*1.6,yPos-t,Width+t*2.5,Height+t*2);            // outline
+        textAlign(CENTER);textSize(18);
+        float c=textWidth(Title);
+        fill(BackgroundColor); color(0);stroke(0);strokeWeight(1);
+        rect(xPos+Width/2-c/2,yPos-35,c,0);                         // Heading Rectangle  
+        
+        fill(0);
+        text(Title,xPos+Width/2,yPos-37);                            // Heading Title
+        textAlign(CENTER);textSize(14);
+        text(xLabel,xPos+Width/2,yPos+Height+t/1.5);                     // x-axis Label 
+        
+        rotate(-PI/2);                                               // rotate -90 degrees
+        text(yLabel,-yPos-Height/2,xPos-t*1.6+20);                   // y-axis Label  
+        rotate(PI/2);                                                // rotate back
+        
+        textSize(10); noFill(); stroke(0); smooth();strokeWeight(1);
+        //Edges
+        line(xPos-3,yPos+Height,xPos-3,yPos);                        // y-axis line 
+        line(xPos-3,yPos+Height,xPos+Width+5,yPos+Height);           // x-axis line 
+          
+        stroke(200);
+        if(yMin<0){
+            line(xPos-7,                                       // zero line 
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height,   // 
+                 xPos+Width,
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height
+                 );
+        }
+          
+        if(RightAxis){                                       // Right-axis line   
+            stroke(0);
+            line(xPos+Width+3,yPos+Height,xPos+Width+3,yPos);
+        }
+            
+        /*  =========================================================================================
+            Sub-devisions for both axes, left and right
+            ==========================================================================================  */
+            
+        stroke(0);
+            
+        for(int x=0; x<=xDiv; x++){
+       
+            /*  =========================================================================================
+                x-axis
+                ==========================================================================================  */
+             
+            line(float(x)/xDiv*Width+xPos-3,yPos+Height,       //  x-axis Sub devisions    
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+5);     
+                 
+            textSize(10);                                      // x-axis Labels
+            String xAxis=str(xMin+float(x)/xDiv*(xMax-xMin));  // the only way to get a specific number of decimals 
+            String[] xAxisMS=split(xAxis,'.');                 // is to split the float into strings 
+            text(xAxisMS[0]+"."+xAxisMS[1].charAt(0),          // ...
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+15);   // x-axis Labels
+        }
+          
+          
+        /*  =========================================================================================
+            left y-axis
+            ==========================================================================================  */
+          
+        for(int y=0; y<=yDiv; y++){
+            line(xPos-3,float(y)/yDiv*Height+yPos,                // ...
+                 xPos-7,float(y)/yDiv*Height+yPos);              // y-axis lines 
+            
+            textAlign(RIGHT);fill(20);
+            
+            String yAxis=str(yMin+float(y)/yDiv*(yMax-yMin));     // Make y Label a string
+            String[] yAxisMS=split(yAxis,'.');                    // Split string
+           
+            text(yAxisMS[0]+"."+yAxisMS[1].charAt(0),             // ... 
+                 xPos-15,float(yDiv-y)/yDiv*Height+yPos+3);       // y-axis Labels 
+                        
+                        
+            /*  =========================================================================================
+                right y-axis
+                ==========================================================================================  */
+            
+            if(RightAxis){
+             
+                color(GraphColor); stroke(GraphColor);fill(20);
+            
+                line(xPos+Width+3,float(y)/yDiv*Height+yPos,             // ...
+                     xPos+Width+7,float(y)/yDiv*Height+yPos);            // Right Y axis sub devisions
+                   
+                textAlign(LEFT); 
+            
+                String yAxisRight=str(yMinRight+float(y)/                // ...
+                                      yDiv*(yMaxRight-yMinRight));           // convert axis values into string
+                String[] yAxisRightMS=split(yAxisRight,'.');             // 
+           
+                text(yAxisRightMS[0]+"."+yAxisRightMS[1].charAt(0),     // Right Y axis text
+                     xPos+Width+15,float(yDiv-y)/yDiv*Height+yPos+3);   // it's x,y location
+            
+                noFill();
+            }stroke(0);
+        }
+    }
+      
+    /*  =========================================================================================
+        Bar graph
+        ==========================================================================================  */   
+      
+    void Bar(float[] a, int from, int to) {
+        
+        stroke(GraphColor);
+        fill(GraphColor);
+          
+        if(from<0){                                      // If the From or To value is out of bounds 
+            for (int x=0; x Make sure time array doesn't decrease (go back in time) 
+               ===========================================================================*/
+            if(ix[i+1]){
+                   
+                    ErrorFlag=true;
+                
+                }
+            }
+         
+            /* =================================================================================       
+               First and last bits can't be part of the curve, no points before first bit, 
+               none after last bit. So a streight line is drawn instead   
+               ================================================================================= */ 
+
+            if(i==0 || i==x.length-2)line(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                                          xPos+(x[i+1]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i+1]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                                            
+            /* =================================================================================       
+               For the rest of the array a curve (spline curve) can be created making the graph 
+               smooth.     
+               ================================================================================= */ 
+                            
+            curveVertex( xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                         yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                           
+            /* =================================================================================       
+               If the Dot option is true, Place a dot at each data point.  
+               ================================================================================= */    
+           
+            if(Dot)ellipse(
+                xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                2,2
+                           );
+                             
+            /* =================================================================================       
+               Highlights points closest to Mouse X position   
+               =================================================================================*/ 
+                          
+            if( abs(mouseX-(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width))<5 ){
+                
+                 
+                float yLinePosition = yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height;
+                float xLinePosition = xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width;
+                strokeWeight(1);stroke(240);
+                // line(xPos,yLinePosition,xPos+Width,yLinePosition);
+                strokeWeight(2);stroke(GraphColor);
+                  
+                ellipse(xLinePosition,yLinePosition,4,4);
+            }
+        }  
+       
+        endShape(); 
+        yMax=tempyMax; yMin=tempyMin;
+        float xAxisTitleWidth=textWidth(str(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1])));
+           
+        if((mouseX>xPos&mouseX<(xPos+Width))&(mouseY>yPos&mouseY<(yPos+Height))){   
+            if(ShowMouseLines){
+                // if(mouseXxPos+Width)xlocation=xPos+Width;
+                else xlocation=mouseX;
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+                // Rectangle and x position
+                line(xlocation,yPos,xlocation,yPos+Height);
+                rect(xlocation-xAxisTitleWidth/2-10,yPos+Height-16,xAxisTitleWidth+20,12);
+            
+                textAlign(CENTER); fill(160);
+                text(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1]),xlocation,yPos+Height-6);
+            
+                // if(mouseYyPos+Height)ylocation=yPos+Height;
+                else ylocation=mouseY;
+          
+                // Rectangle and y position
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+            
+                line(xPos,ylocation,xPos+Width,ylocation);
+                int yAxisTitleWidth=int(textWidth(str(map(ylocation,yPos,yPos+Height,y[0],y[y.length-1]))) );
+                rect(xPos-15+3,ylocation-6, -60 ,12);
+            
+                textAlign(RIGHT); fill(GraphColor);//StrokeColor
+                //    text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos+Width+3,yPos+Height+4);
+                text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos -15,ylocation+4);
+                if(RightAxis){ 
+                          
+                    stroke(200); strokeWeight(0.5);fill(255);color(50);
+                           
+                    rect(xPos+Width+15-3,ylocation-6, 60 ,12);  
+                    textAlign(LEFT); fill(160);
+                    text(map(ylocation,yPos+Height,yPos,yMinRight,yMaxRight),xPos+Width+15,ylocation+4);
+                }
+                noStroke();noFill();
+            }
+        }
+    }
+
+    void smoothLine(float[] x ,float[] y, float[] z, float[] a ) {
+        GraphColor=color(188,53,53);
+        smoothLine(x ,y);
+        GraphColor=color(193-100,216-100,16);
+        smoothLine(z ,a);
+    }
+}
+
diff --git a/src/Processing/AttitudeDisplay/AttitudeDisplay.pde b/src/Processing/AttitudeDisplay/AttitudeDisplay.pde
new file mode 100644
index 0000000..be505d3
--- /dev/null
+++ b/src/Processing/AttitudeDisplay/AttitudeDisplay.pde
@@ -0,0 +1,208 @@
+/**
+ * Attitude Display
+ * by Daniel Shiffman.  
+ * 
+ * Demonstration some basic vector math: subtraction, normalization, scaling
+ * Normalizing a vector sets its length to 1.
+ */
+
+import processing.net.Client;
+
+Client client;           /* data socket connected to zumo IMU data server */
+
+int    MAX_FPS = 30;     /* max frames/sec, i.e., rate of calls to draw() */
+int    MAX_XVALS = 256;  /* max # of samples in the x-axis */
+int    MAX_PENDING = 6;  /* max # of outstanding commands to send */
+
+float viewAngle[] = {25, -35};  /* intial viewing angle */
+int view = 1;                  /* which view to display */
+String[] viewStr = {"Global & Body Frame", "Body Frame in Global Coordinates", "Global Frame in Body's Coordinates"};
+
+/* IMU and DCM data from zumo */
+char[] imuBuffer = new char[138];
+
+/* Direction cosine matrix scaled by 100 */
+float dcm[][] = {{100, 0, 0}, {0, 100, 0}, {0, 0, 100}};
+/* acceleration vector scaled by 100 */
+float acc[] = {0, 0, 100};
+
+/*
+ *  ======== setup ========
+ */
+void setup() {
+  /* setup for 3D display */
+  size(750, 550, P3D);
+  smooth();
+
+  //ortho(); 
+
+  /* slow the draw() rate down to MAX_FPS frames/sec */
+  frameRate(MAX_FPS);
+
+  /* Connect to zumo's command server IP address and port */
+  client = new Client(this, "192.168.1.1", 8080);
+}
+
+/*
+ *  ======== draw ========
+ */
+boolean cont = false;  /* continuously send previous command to get IMU data */
+String  pcmd = " \n";  /* previous command */
+int     pending = 0;   /* number of outstanding commands sent to zumo bot */
+
+void draw() {
+  background(150);
+  lights();  
+  fill(0);
+
+    /* send server commands based on keyboard input */
+  if (keyPressed || cont) {
+    String cmd = pcmd;
+    if  (key == ' ' || key == 'w' || key == 'a' || key == 's' || key == 'd') {
+      cmd = key + "\n";
+    }
+    else if (key == 'x') cont = false;
+    else if (key == 'c') cont = true;
+        
+    /* stop sending commands if too many commands are outstanding */
+    if (pending < MAX_PENDING) {
+      client.write(cmd);
+      pcmd = cmd;
+      pending++;
+    }
+  }
+
+  /* Read IMU data from server and display it */
+  if (client.available() >= imuBuffer.length) {
+    /* decrement pending count so we can send more commands */
+    if (--pending < 0) {
+      println("Yikes! pending = " + pending);
+      pending = 0;
+    }
+
+    /* read IMU data */
+    for (int i = 0; i < imuBuffer.length; i++) {
+      imuBuffer[i] = client.readChar();
+    }
+
+    /* parse IMU data and display it */
+    String input = new String(imuBuffer);
+    String [] tokens = splitTokens(input, " ");
+
+    /* update the dcm variable */
+    dcm[0][0] = float(tokens[13]);
+    dcm[0][1] = float(tokens[14]);
+    dcm[0][2] = float(tokens[15]);
+    dcm[1][0] = float(tokens[16]);
+    dcm[1][1] = float(tokens[17]);
+    dcm[1][2] = float(tokens[18]);
+    dcm[2][0] = float(tokens[19]);
+    dcm[2][1] = float(tokens[20]);
+    dcm[2][2] = float(tokens[21]);
+    
+//    println(dcm[0]);
+//    println(dcm[1]);
+//    println(dcm[2]);
+    
+    /* update the acceleration vector */
+    acc[0] = 100 * (float(tokens[1]) * (2 * 9.80665) / float(32768));
+    acc[1] = 100 * (float(tokens[2]) * (2 * 9.80665) / float(32768));
+    acc[2] = 100 * (float(tokens[3]) * (2 * 9.80665) / float(32768));
+  }
+
+  if (mousePressed == true) {
+    viewAngle[0] =  (mouseX - width/2 );
+    viewAngle[1] = -(mouseY - height/2) ;
+  } 
+
+  text("Camera View Angle(click on center and drag): " + viewAngle[0] + " , " + viewAngle[1]+ 
+       "\n"+"View:"+viewStr[view]+" (press V to change)"+
+       "\n"+"Key: X - blue, Y - red, Z - green", 10, 20);  
+
+  translate(width/2, height/2, 0);
+
+  for (int i=0; i<100; i++) {
+    rotateX(viewAngle[1] / 100 * PI/180);    
+    rotateY(viewAngle[0] / 100 * PI/180);
+  }   
+
+  drawCoordinateSystem();
+
+  drawDCM();
+}
+
+void keyPressed() {
+  if (key == 'v' || key == 'V') view = (view+1) % 3;
+}
+
+void drawCoordinateSystem() {
+  float d = 5;
+  //draw axis
+  strokeWeight(3);
+  stroke(0x330000FF);    // X - blue (screen's -X axis)
+  line(-300, 0, 0, 300, 0, 0);  
+  fill(0x330000FF);  
+  beginShape(TRIANGLES);
+  vertex(-310, 0, 0); vertex(-300, d, d); vertex(-300, d, -d);
+  vertex(-310, 0, 0); vertex(-300, d, -d); vertex(-300, -d, -d);  
+  vertex(-310, 0, 0); vertex(-300, -d, -d); vertex(-300, -d, d);    
+  vertex(-310, 0, 0); vertex(-300, -d, d); vertex(-300, d, d);      
+  endShape();
+  stroke(0x33FF0000);    // Y - red (screen's Z axis)
+  line(-0, 0, -300, 0, 0, 300);  
+  fill(0x33FF0000);  
+  beginShape(TRIANGLES);
+  vertex(0, 0, 310); vertex(d, d, 300); vertex(d, -d, 300);
+  vertex(0, 0, 310); vertex(d, -d, 300); vertex(-d, -d, 300);  
+  vertex(0, 0, 310); vertex(-d, -d, 300); vertex(-d, d, 300);    
+  vertex(0, 0, 310); vertex(-d, d, 300); vertex(d, d, 300);      
+  endShape();  
+  stroke(0x3300FF00);    // Z - green (screen's -Y axis)
+  line(-0, -300, 0, 0, 300, 0);  
+  fill(0x3300FF00);  
+  beginShape(TRIANGLES);
+  vertex(0, -310, 0); vertex(d, -300, d); vertex(d, -300, -d); 
+  vertex(0, -310, 0); vertex(d, -300, -d); vertex(-d, -300, -d);   
+  vertex(0, -310, 0); vertex(-d, -300, -d); vertex(-d, -300, d);    
+  vertex(0, -310, 0); vertex(-d, -300, d); vertex(d, -300, d);      
+  endShape();
+}
+
+void drawDCM() {
+  //show inverse gravitation vector in Device coordinates
+  if (0==view || 2==view) {
+    strokeWeight(5); 
+    stroke(#000000);   
+    drawVector(acc[0], acc[1], acc[2]);
+
+
+    //show World in Device coordinates
+    strokeWeight(3); 
+    stroke(#0000FF);  
+    drawVector(dcm[0][0], dcm[0][1], dcm[0][2]);
+    stroke(#FF0000);    
+    drawVector(dcm[1][0], dcm[1][1], dcm[1][2]);
+    stroke(#00FF00);    
+    drawVector(dcm[2][0], dcm[2][1], dcm[2][2]);
+  }
+
+  if (0==view || 1==view) { 
+    //show Device in World coordinates (translated (tranposed) DCM matrix)
+    strokeWeight(6); 
+    stroke(0==view ? 0x330000FF : #0000FF);      // X - blue 
+    drawVector(dcm[0][0], dcm[1][0], dcm[2][0]);
+    stroke(0==view ? 0x33FF0000 : #FF0000);    // Y - red
+    drawVector(dcm[0][1], dcm[1][1], dcm[2][1]);
+    stroke(0==view ? 0x3300FF00 : #00FF00);      // Z - green   
+    drawVector(dcm[0][2], dcm[1][2], dcm[2][2]);
+  }
+}
+
+void drawVector(float x, float y, float z) {
+  //Convert from DCM to screen coordinate system:
+  //DCM(X) = - Screen(X)  
+  //DCM(Y) =   Screen(Z)
+  //DCM(Z) = - Screen(Y)
+  line(0, 0, 0, -x, -z, y);
+}
+
diff --git a/src/Processing/Balancing/Balancing.pde b/src/Processing/Balancing/Balancing.pde
new file mode 100644
index 0000000..7e4c972
--- /dev/null
+++ b/src/Processing/Balancing/Balancing.pde
@@ -0,0 +1,373 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * RedBear Zumo Bot interactive control with real-time acceleration display
+ * 
+ * Before running this program, start the zumo bot and connect to its
+ * WiFi network.
+ */
+
+import processing.net.Client;
+
+import controlP5.ControlP5;
+import controlP5.ControlEvent;
+import controlP5.Textfield;
+
+ControlP5 cp5;
+
+Client client;           /* data socket connected to zumo IMU data server */
+
+/* IMU data from zumo */
+char[] imuBuffer = new char[96];
+
+int    MAX_FPS = 30;     /* max frames/sec, i.e., rate of calls to draw() */
+int    MAX_XVALS = 80;   /* max # of samples in the x-axis */
+int    MAX_PENDING = 6;  /* max # of outstanding commands to send */
+
+String LOG_NAME  = "zumo_log.txt";
+
+String textValue = "";
+
+/* IMU data graph object */
+Graph graph1 = new Graph(150, 80, 400, 200, color (200, 20, 20));
+
+/* IMU data values to graph */
+float[] time = { 0 };       
+float[] ax   = { 0 }; 
+float[] ay   = { 0 };
+float[] az   = { 0 };
+
+char curGraph = 'A';
+int curGraphOffset = 1;
+
+long startMillis = 0;
+float curTime = 0;
+boolean scaleData = true;
+
+PrintWriter log = null;  /* optional data log */
+
+/*
+ *  ======== setup ========
+ */
+void setup() 
+{
+    /* create minimal text output window for acc data */
+    size(950, 350); 
+   
+    graph1.xLabel = " Time (s)";
+    graph1.yLabel = "Acceleration (m/s^2)";
+    graph1.Title  = "Acceleration: (x, y, z) vs. t";
+    graph1.yMin   = 0;
+    graph1.yMax   = 0;
+
+    /* slow the draw() rate down to MAX_FPS frames/sec */
+    frameRate(MAX_FPS);
+        
+    /* Connect to zumo's command server IP address and port */
+    client = new Client(this, "192.168.1.1", 8080);
+    
+    PFont font = createFont("arial", 20);
+    
+    cp5 = new ControlP5(this);
+    
+    /* create text fields for real-time PID tuning */
+    cp5.addTextfield("P")
+     .setPosition(700,50)
+     .setSize(200,40)
+     .setFont(font)
+     .setValue(27.6)
+     .setText("27.6")
+     .setAutoClear(false)
+     .getCaptionLabel().setFont(font).setSize(16).toUpperCase(false).setText("Proportional Gain")
+     ;
+     
+    cp5.addTextfield("I")
+     .setPosition(700,150)
+     .setSize(200,40)
+     .setFont(font)
+     .setValue(0.27)
+     .setText("0.27")
+     .setAutoClear(false)
+     .getCaptionLabel().setFont(font).setSize(16).toUpperCase(false).setText("Integral Gain")
+
+     ;
+     
+    cp5.addTextfield("D")
+     .setPosition(700,250)
+     .setSize(200,40)
+     .setFont(font)
+     .setValue(0.2)
+     .setText("0.2")
+     .setAutoClear(false)
+     .getCaptionLabel().setFont(font).setSize(16).toUpperCase(false).setText("Derivative Gain")
+
+     ; 
+     
+    textFont(font);
+
+    background(0);
+    graph1.DrawAxis();
+}
+
+/*
+ *  ======== draw ========
+ */
+boolean cont = false;  /* continuously send previous command to get IMU data */
+String  pcmd = " \n";  /* previous command */
+int     pending = 0;   /* number of outstanding commands sent to zumo bot */
+String  pidCmd = null;
+
+void draw() 
+{
+    background(0);
+    fill(255);
+    graph1.DrawAxis();
+
+    /* send server commands based on keyboard input */
+    if (keyPressed || cont) {
+        if (startMillis == 0) {
+            /* capture initial start time */
+            startMillis = java.lang.System.currentTimeMillis();
+        }
+
+        String cmd = pcmd;
+        if (key == ' '
+            || key == 'w' || key == 'a' || key == 's' || key == 'd') {
+            cmd = key + "\n";
+        }
+        else if (key == 'b') {
+            cmd = key + "\n";
+            if (curGraph != 'B') {
+                curGraph = 'B';
+                curGraphOffset = 11;
+                graph1.yLabel = "Tilt Angle (deg)";
+                graph1.Title = "Tilt Angle: vs. t";
+                graph1.yMin = 0;
+                graph1.yMax = 0;
+                ax = new float [] { 0 }; 
+                ay = new float [] { 0 };
+                az = new float [] { 0 };
+                time = new float [] { curTime }; 
+            }
+        }
+        else if (key == 'x') cont = false;
+        else if (key == 'c') cont = true;
+        else if (key == 'L') {
+            if (log == null) {
+                log = createWriter(LOG_NAME);
+                log.println("LOG STARTED");
+                println("logging started ...");
+            }
+        }
+        else if (key == 'l') {
+            if (log != null) {
+                log.println("LOG STOPPED");
+                log.flush();
+                log.close();
+                log = null;
+                println("logging stopped.");
+            }
+        }
+        else if (key == 'G') {
+            if (curGraph != 'G') {
+                curGraph = 'G';
+                curGraphOffset = 5;
+                graph1.yLabel = "Rotational Speed (deg/s)";
+                graph1.Title = "Gyro: (x, y, z) vs. t ";  
+                graph1.yMin = 0;
+                graph1.yMax = 0;
+                ax = new float [] { 0 }; 
+                ay = new float [] { 0 };
+                az = new float [] { 0 };
+                time = new float [] { curTime }; 
+            }
+        }
+        else if (key == 'A') {
+            if (curGraph != 'A') {
+                curGraph = 'A';
+                curGraphOffset = 1;
+                graph1.yLabel = "Acceleration (m/s^2)";
+                graph1.Title = "Acceleration: (x, y, z) vs. t";
+                graph1.yMin = 0;
+                graph1.yMax = 0;
+                ax = new float [] { 0 }; 
+                ay = new float [] { 0 };
+                az = new float [] { 0 };
+                time = new float [] { curTime }; 
+            }
+        }
+
+        /* stop sending commands if too many commands are outstanding */
+        if (pending < MAX_PENDING) {
+            if (pidCmd != null) {
+                client.write(pidCmd);
+                pidCmd = null;
+            }
+            else {
+                client.write(cmd);
+                pcmd = cmd;
+            }
+            pending++;
+        }
+    }
+
+    /* Read IMU data from server and display it */
+    if (client.available() >= imuBuffer.length) {
+        /* read IMU data */
+        for (int i = 0; i < imuBuffer.length; i++) {
+            imuBuffer[i] = client.readChar();
+        }
+
+        /* decrement pending count so we can send more commands */
+        if (--pending < 0) {
+            println("Yikes! pending = " + pending);
+            pending = 0;
+        }
+       
+        /* parse IMU data and display it */
+        String input = new String(imuBuffer);
+        String [] tokens = splitTokens(input, " ");
+        if (tokens.length > (2 + curGraphOffset)) {
+            if (curGraph != 'B') {
+                newPoint(tokens[0 + curGraphOffset],  /* x value */
+                         tokens[1 + curGraphOffset],  /* y value */
+                         tokens[2 + curGraphOffset]); /* z value */
+            }
+            else {
+                newPoint(tokens[2 + curGraphOffset],  /* tilt */
+                         tokens[3 + curGraphOffset],  /* motor power */
+                         tokens[4 + curGraphOffset]); /* angle error */
+            }
+        }
+    }
+}
+
+/*
+ *  ======== newPoint ========
+ *  add new point to data arrays and update graph
+ */
+void newPoint(String x, String y, String z)
+{
+    /* get next values */
+    curTime = (java.lang.System.currentTimeMillis() - startMillis) / 1000.0;
+    if (log != null) {
+        log.println("A: " + x + " " + y + " " + z);
+    }
+    
+    /* update all data arrays */
+    time = pushv(time, curTime);
+    ax = pushv(ax, scale(curGraph, x));
+    ay = pushv(ay, scale(curGraph, y));
+    az = pushv(az, scale(curGraph, z));
+
+    /* update display */
+    updatePlots();
+}
+
+/*
+ *  ======== pushv ========
+ *  append new data sample to arr[] and return new array
+ */
+float[] pushv(float [] arr, float val)
+{
+    float [] tmp = arr;
+    
+    if (arr.length < MAX_XVALS) {
+        tmp = append(arr, val);
+    }
+    else {
+        /* shift data within arr to make room for new value */
+        int i;
+        for (i = 1; i < MAX_XVALS; i++) {
+            arr[i - 1] = arr[i];
+        }
+        arr[MAX_XVALS - 1] = val;
+    }
+
+    return (tmp);
+}
+
+/*
+ *  ======== updatePlots ========
+ *  redraw IMU graph
+ */
+void updatePlots()
+{
+    graph1.xMax = max(time);
+    graph1.xMin = min(time);
+    graph1.yMax = max(max(max(az), max(ax), max(ay)), graph1.yMax);
+    graph1.yMin = min(min(min(az), min(ax), min(ay)), graph1.yMin);
+                                                  
+    graph1.DrawAxis();
+      
+    graph1.GraphColor = color(200, 40, 40);  
+    graph1.LineGraph(time, ax);      
+      
+    graph1.GraphColor = color(40, 200, 40);   
+    graph1.LineGraph(time, ay);
+
+    graph1.GraphColor = color(40, 40, 200);   
+    graph1.LineGraph(time, az);
+}
+
+/*
+ *  ======== scale ========
+ */
+float scale(char type, String value)
+{
+    if (scaleData) {
+        switch (type) {
+            case 'A':
+                /* acc data is +-2G */
+                return (float(value) * (2 * 9.80665) / float(32768));
+
+            case 'G':
+                /* gyro data is +-250 deg/sec */
+                return (float(value) * 250 / float(32768));
+            
+            case 'B':
+                return (float(value) / float(100));
+        }
+    }
+    return (float(value));
+}
+
+/*
+ *  ======== controlEvent ========
+ */
+void controlEvent(ControlEvent theEvent) 
+{
+    if (theEvent.isAssignableFrom(Textfield.class)) {
+        pidCmd = theEvent.getName() + theEvent.getStringValue() + "\n";
+    }
+}
diff --git a/src/Processing/Balancing/GraphClass.pde b/src/Processing/Balancing/GraphClass.pde
new file mode 100644
index 0000000..39f9b7c
--- /dev/null
+++ b/src/Processing/Balancing/GraphClass.pde
@@ -0,0 +1,371 @@
+/*
+ * from: https://github.com/sebnil/RealtimePlotter
+ *
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2013 Sebatian Nilsson
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/*   =================================================================================       
+     The Graph class contains functions and variables that have been created to draw 
+     graphs. Here is a quick list of functions within the graph class:
+          
+     Graph(int x, int y, int w, int h,color k)
+     DrawAxis()
+     Bar([])
+     smoothLine([][])
+     DotGraph([][])
+     LineGraph([][]) 
+     
+     =================================================================================*/   
+
+    
+class Graph 
+{
+      
+    boolean Dot=true;            // Draw dots at each data point if true
+    boolean RightAxis;            // Draw the next graph using the right axis if true
+    boolean ErrorFlag=false;      // If the time array isn't in ascending order, make true  
+    boolean ShowMouseLines=true;  // Draw lines and give values of the mouse position
+    
+    int     xDiv=10,yDiv=5;            // Number of sub divisions
+    int     xPos,yPos;            // location of the top left corner of the graph  
+    int     Width,Height;         // Width and height of the graph
+    
+
+    color   GraphColor;
+    color   BackgroundColor=color(255);  
+    color   StrokeColor=color(180);     
+      
+    String  Title="Title";          // Default titles
+    String  xLabel="x - Label";
+    String  yLabel="y - Label";
+
+    float   yMax=1024, yMin=0;      // Default axis dimensions
+    float   xMax=10, xMin=0;
+    float   yMaxRight=1024,yMinRight=0;
+  
+    PFont   Font;                   // Selected font used for text 
+      
+    //    int Peakcounter=0,nPeakcounter=0;
+     
+    Graph(int x, int y, int w, int h, color k) {  // The main declaration function
+        xPos = x;
+        yPos = y;
+        Width = w;
+        Height = h;
+        GraphColor = k;
+    }
+     
+    void DrawAxis(){
+       
+        /*  =========================================================================================
+            Main axes Lines, Graph Labels, Graph Background
+            ==========================================================================================  */
+    
+        fill(BackgroundColor); color(0);stroke(StrokeColor);strokeWeight(1);
+        int t=60;
+        
+        rect(xPos-t*1.6,yPos-t,Width+t*2.5,Height+t*2);            // outline
+        textAlign(CENTER);textSize(18);
+        float c=textWidth(Title);
+        fill(BackgroundColor); color(0);stroke(0);strokeWeight(1);
+        rect(xPos+Width/2-c/2,yPos-35,c,0);                         // Heading Rectangle  
+        
+        fill(0);
+        text(Title,xPos+Width/2,yPos-37);                            // Heading Title
+        textAlign(CENTER);textSize(14);
+        text(xLabel,xPos+Width/2,yPos+Height+t/1.5);                     // x-axis Label 
+        
+        rotate(-PI/2);                                               // rotate -90 degrees
+        text(yLabel,-yPos-Height/2,xPos-t*1.6+20);                   // y-axis Label  
+        rotate(PI/2);                                                // rotate back
+        
+        textSize(10); noFill(); stroke(0); smooth();strokeWeight(1);
+        //Edges
+        line(xPos-3,yPos+Height,xPos-3,yPos);                        // y-axis line 
+        line(xPos-3,yPos+Height,xPos+Width+5,yPos+Height);           // x-axis line 
+          
+        stroke(200);
+        if(yMin<0){
+            line(xPos-7,                                       // zero line 
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height,   // 
+                 xPos+Width,
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height
+                 );
+        }
+          
+        if(RightAxis){                                       // Right-axis line   
+            stroke(0);
+            line(xPos+Width+3,yPos+Height,xPos+Width+3,yPos);
+        }
+            
+        /*  =========================================================================================
+            Sub-devisions for both axes, left and right
+            ==========================================================================================  */
+            
+        stroke(0);
+            
+        for(int x=0; x<=xDiv; x++){
+       
+            /*  =========================================================================================
+                x-axis
+                ==========================================================================================  */
+             
+            line(float(x)/xDiv*Width+xPos-3,yPos+Height,       //  x-axis Sub devisions    
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+5);     
+                 
+            textSize(10);                                      // x-axis Labels
+            String xAxis=str(xMin+float(x)/xDiv*(xMax-xMin));  // the only way to get a specific number of decimals 
+            String[] xAxisMS=split(xAxis,'.');                 // is to split the float into strings 
+            text(xAxisMS[0]+"."+xAxisMS[1].charAt(0),          // ...
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+15);   // x-axis Labels
+        }
+          
+          
+        /*  =========================================================================================
+            left y-axis
+            ==========================================================================================  */
+          
+        for(int y=0; y<=yDiv; y++){
+            line(xPos-3,float(y)/yDiv*Height+yPos,                // ...
+                 xPos-7,float(y)/yDiv*Height+yPos);              // y-axis lines 
+            
+            textAlign(RIGHT);fill(20);
+            
+            String yAxis=str(yMin+float(y)/yDiv*(yMax-yMin));     // Make y Label a string
+            String[] yAxisMS=split(yAxis,'.');                    // Split string
+           
+            text(yAxisMS[0]+"."+yAxisMS[1].charAt(0),             // ... 
+                 xPos-15,float(yDiv-y)/yDiv*Height+yPos+3);       // y-axis Labels 
+                        
+                        
+            /*  =========================================================================================
+                right y-axis
+                ==========================================================================================  */
+            
+            if(RightAxis){
+             
+                color(GraphColor); stroke(GraphColor);fill(20);
+            
+                line(xPos+Width+3,float(y)/yDiv*Height+yPos,             // ...
+                     xPos+Width+7,float(y)/yDiv*Height+yPos);            // Right Y axis sub devisions
+                   
+                textAlign(LEFT); 
+            
+                String yAxisRight=str(yMinRight+float(y)/                // ...
+                                      yDiv*(yMaxRight-yMinRight));           // convert axis values into string
+                String[] yAxisRightMS=split(yAxisRight,'.');             // 
+           
+                text(yAxisRightMS[0]+"."+yAxisRightMS[1].charAt(0),     // Right Y axis text
+                     xPos+Width+15,float(yDiv-y)/yDiv*Height+yPos+3);   // it's x,y location
+            
+                noFill();
+            }stroke(0);
+        }
+    }
+      
+    /*  =========================================================================================
+        Bar graph
+        ==========================================================================================  */   
+      
+    void Bar(float[] a, int from, int to) {
+        
+        stroke(GraphColor);
+        fill(GraphColor);
+          
+        if(from<0){                                      // If the From or To value is out of bounds 
+            for (int x=0; x Make sure time array doesn't decrease (go back in time) 
+               ===========================================================================*/
+            if(ix[i+1]){
+                   
+                    ErrorFlag=true;
+                
+                }
+            }
+         
+            /* =================================================================================       
+               First and last bits can't be part of the curve, no points before first bit, 
+               none after last bit. So a streight line is drawn instead   
+               ================================================================================= */ 
+
+            if(i==0 || i==x.length-2)line(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                                          xPos+(x[i+1]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i+1]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                                            
+            /* =================================================================================       
+               For the rest of the array a curve (spline curve) can be created making the graph 
+               smooth.     
+               ================================================================================= */ 
+                            
+            curveVertex( xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                         yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                           
+            /* =================================================================================       
+               If the Dot option is true, Place a dot at each data point.  
+               ================================================================================= */    
+           
+            if(Dot)ellipse(
+                xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                2,2
+                           );
+                             
+            /* =================================================================================       
+               Highlights points closest to Mouse X position   
+               =================================================================================*/ 
+                          
+            if( abs(mouseX-(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width))<5 ){
+                
+                 
+                float yLinePosition = yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height;
+                float xLinePosition = xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width;
+                strokeWeight(1);stroke(240);
+                // line(xPos,yLinePosition,xPos+Width,yLinePosition);
+                strokeWeight(2);stroke(GraphColor);
+                  
+                ellipse(xLinePosition,yLinePosition,4,4);
+            }
+        }  
+       
+        endShape(); 
+        yMax=tempyMax; yMin=tempyMin;
+        float xAxisTitleWidth=textWidth(str(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1])));
+           
+        if((mouseX>xPos&mouseX<(xPos+Width))&(mouseY>yPos&mouseY<(yPos+Height))){   
+            if(ShowMouseLines){
+                // if(mouseXxPos+Width)xlocation=xPos+Width;
+                else xlocation=mouseX;
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+                // Rectangle and x position
+                line(xlocation,yPos,xlocation,yPos+Height);
+                rect(xlocation-xAxisTitleWidth/2-10,yPos+Height-16,xAxisTitleWidth+20,12);
+            
+                textAlign(CENTER); fill(160);
+                text(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1]),xlocation,yPos+Height-6);
+            
+                // if(mouseYyPos+Height)ylocation=yPos+Height;
+                else ylocation=mouseY;
+          
+                // Rectangle and y position
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+            
+                line(xPos,ylocation,xPos+Width,ylocation);
+                int yAxisTitleWidth=int(textWidth(str(map(ylocation,yPos,yPos+Height,y[0],y[y.length-1]))) );
+                rect(xPos-15+3,ylocation-6, -60 ,12);
+            
+                textAlign(RIGHT); fill(GraphColor);//StrokeColor
+                //    text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos+Width+3,yPos+Height+4);
+                text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos -15,ylocation+4);
+                if(RightAxis){ 
+                          
+                    stroke(200); strokeWeight(0.5);fill(255);color(50);
+                           
+                    rect(xPos+Width+15-3,ylocation-6, 60 ,12);  
+                    textAlign(LEFT); fill(160);
+                    text(map(ylocation,yPos+Height,yPos,yMinRight,yMaxRight),xPos+Width+15,ylocation+4);
+                }
+                noStroke();noFill();
+            }
+        }
+    }
+
+    void smoothLine(float[] x ,float[] y, float[] z, float[] a ) {
+        GraphColor=color(188,53,53);
+        smoothLine(x ,y);
+        GraphColor=color(193-100,216-100,16);
+        smoothLine(z ,a);
+    }
+}
+
diff --git a/src/Processing/ManualDrive/zecho/zecho.pde b/src/Processing/ManualDrive/zecho/zecho.pde
new file mode 100644
index 0000000..d3b2457
--- /dev/null
+++ b/src/Processing/ManualDrive/zecho/zecho.pde
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * Simple RedBear Zumo Bot interactive control
+ * 
+ * Before running this program, start the zumo bot and connect to its
+ * WiFi network.
+ */
+
+import processing.net.Client;
+
+Client client;           /* data socket connected to zumo IMU data server */
+
+/* IMU data from zumo */
+char[] imuBuffer = new char[72];
+
+int    MAX_FPS = 30;     /* max frames/sec, i.e., rate of calls to draw() */
+int    MAX_XVALS = 80;   /* max # of samples in the x-axis */
+int    MAX_PENDING = 6;  /* max # of outstanding commands to send */
+
+/*
+ *  ======== setup ========
+ */
+void setup() 
+{
+    /* create minimal text output window for acc data */
+    size(280, 120);
+    background(204);
+    stroke(0);
+    textSize(20);
+    fill(50);
+
+    /* slow down the draw() rate to MAX_FPS frames/sec */
+    frameRate(MAX_FPS);
+
+    /* Connect to zumo's command server IP address and port */
+    client = new Client(this, "192.168.1.1", 8080);
+}
+
+/*
+ *  ======== draw ========
+ */
+int pending = 0;
+boolean cont = false;
+
+void draw() 
+{
+    /* send server commands based on keyboard input */
+    if (keyPressed || cont) {
+        String cmd = " \n";
+        if  (key == 'w' || key == 'a' || key == 's' || key == 'd') {
+            cmd = key + "\n";
+        }
+        else if (key == 'x') cont = false;
+        else if (key == 'c') cont = true;
+
+        if (pending < MAX_PENDING) {
+            client.write(cmd);
+            pending++;
+        }
+    }
+
+    /* Read IMU data from server and display it */
+    if (client.available() >= imuBuffer.length) {
+
+        /* read IMU data and convert it to a string */
+        for (int i = 0; i < imuBuffer.length; i++) {
+            imuBuffer[i] = client.readChar();
+        }
+        String input = new String(imuBuffer);
+        pending--;
+
+        /* parse IMU data and display it */
+        String [] tokens = splitTokens(input, " ");
+        if (tokens.length > 3) {
+            background(204);
+            text(tokens[1] + ", " + tokens[2] + ", " + tokens[3], 10, 60);
+        }
+    }
+}
diff --git a/src/Processing/ManualDrive/zgraph/GraphClass.pde b/src/Processing/ManualDrive/zgraph/GraphClass.pde
new file mode 100644
index 0000000..39f9b7c
--- /dev/null
+++ b/src/Processing/ManualDrive/zgraph/GraphClass.pde
@@ -0,0 +1,371 @@
+/*
+ * from: https://github.com/sebnil/RealtimePlotter
+ *
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2013 Sebatian Nilsson
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/*   =================================================================================       
+     The Graph class contains functions and variables that have been created to draw 
+     graphs. Here is a quick list of functions within the graph class:
+          
+     Graph(int x, int y, int w, int h,color k)
+     DrawAxis()
+     Bar([])
+     smoothLine([][])
+     DotGraph([][])
+     LineGraph([][]) 
+     
+     =================================================================================*/   
+
+    
+class Graph 
+{
+      
+    boolean Dot=true;            // Draw dots at each data point if true
+    boolean RightAxis;            // Draw the next graph using the right axis if true
+    boolean ErrorFlag=false;      // If the time array isn't in ascending order, make true  
+    boolean ShowMouseLines=true;  // Draw lines and give values of the mouse position
+    
+    int     xDiv=10,yDiv=5;            // Number of sub divisions
+    int     xPos,yPos;            // location of the top left corner of the graph  
+    int     Width,Height;         // Width and height of the graph
+    
+
+    color   GraphColor;
+    color   BackgroundColor=color(255);  
+    color   StrokeColor=color(180);     
+      
+    String  Title="Title";          // Default titles
+    String  xLabel="x - Label";
+    String  yLabel="y - Label";
+
+    float   yMax=1024, yMin=0;      // Default axis dimensions
+    float   xMax=10, xMin=0;
+    float   yMaxRight=1024,yMinRight=0;
+  
+    PFont   Font;                   // Selected font used for text 
+      
+    //    int Peakcounter=0,nPeakcounter=0;
+     
+    Graph(int x, int y, int w, int h, color k) {  // The main declaration function
+        xPos = x;
+        yPos = y;
+        Width = w;
+        Height = h;
+        GraphColor = k;
+    }
+     
+    void DrawAxis(){
+       
+        /*  =========================================================================================
+            Main axes Lines, Graph Labels, Graph Background
+            ==========================================================================================  */
+    
+        fill(BackgroundColor); color(0);stroke(StrokeColor);strokeWeight(1);
+        int t=60;
+        
+        rect(xPos-t*1.6,yPos-t,Width+t*2.5,Height+t*2);            // outline
+        textAlign(CENTER);textSize(18);
+        float c=textWidth(Title);
+        fill(BackgroundColor); color(0);stroke(0);strokeWeight(1);
+        rect(xPos+Width/2-c/2,yPos-35,c,0);                         // Heading Rectangle  
+        
+        fill(0);
+        text(Title,xPos+Width/2,yPos-37);                            // Heading Title
+        textAlign(CENTER);textSize(14);
+        text(xLabel,xPos+Width/2,yPos+Height+t/1.5);                     // x-axis Label 
+        
+        rotate(-PI/2);                                               // rotate -90 degrees
+        text(yLabel,-yPos-Height/2,xPos-t*1.6+20);                   // y-axis Label  
+        rotate(PI/2);                                                // rotate back
+        
+        textSize(10); noFill(); stroke(0); smooth();strokeWeight(1);
+        //Edges
+        line(xPos-3,yPos+Height,xPos-3,yPos);                        // y-axis line 
+        line(xPos-3,yPos+Height,xPos+Width+5,yPos+Height);           // x-axis line 
+          
+        stroke(200);
+        if(yMin<0){
+            line(xPos-7,                                       // zero line 
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height,   // 
+                 xPos+Width,
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height
+                 );
+        }
+          
+        if(RightAxis){                                       // Right-axis line   
+            stroke(0);
+            line(xPos+Width+3,yPos+Height,xPos+Width+3,yPos);
+        }
+            
+        /*  =========================================================================================
+            Sub-devisions for both axes, left and right
+            ==========================================================================================  */
+            
+        stroke(0);
+            
+        for(int x=0; x<=xDiv; x++){
+       
+            /*  =========================================================================================
+                x-axis
+                ==========================================================================================  */
+             
+            line(float(x)/xDiv*Width+xPos-3,yPos+Height,       //  x-axis Sub devisions    
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+5);     
+                 
+            textSize(10);                                      // x-axis Labels
+            String xAxis=str(xMin+float(x)/xDiv*(xMax-xMin));  // the only way to get a specific number of decimals 
+            String[] xAxisMS=split(xAxis,'.');                 // is to split the float into strings 
+            text(xAxisMS[0]+"."+xAxisMS[1].charAt(0),          // ...
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+15);   // x-axis Labels
+        }
+          
+          
+        /*  =========================================================================================
+            left y-axis
+            ==========================================================================================  */
+          
+        for(int y=0; y<=yDiv; y++){
+            line(xPos-3,float(y)/yDiv*Height+yPos,                // ...
+                 xPos-7,float(y)/yDiv*Height+yPos);              // y-axis lines 
+            
+            textAlign(RIGHT);fill(20);
+            
+            String yAxis=str(yMin+float(y)/yDiv*(yMax-yMin));     // Make y Label a string
+            String[] yAxisMS=split(yAxis,'.');                    // Split string
+           
+            text(yAxisMS[0]+"."+yAxisMS[1].charAt(0),             // ... 
+                 xPos-15,float(yDiv-y)/yDiv*Height+yPos+3);       // y-axis Labels 
+                        
+                        
+            /*  =========================================================================================
+                right y-axis
+                ==========================================================================================  */
+            
+            if(RightAxis){
+             
+                color(GraphColor); stroke(GraphColor);fill(20);
+            
+                line(xPos+Width+3,float(y)/yDiv*Height+yPos,             // ...
+                     xPos+Width+7,float(y)/yDiv*Height+yPos);            // Right Y axis sub devisions
+                   
+                textAlign(LEFT); 
+            
+                String yAxisRight=str(yMinRight+float(y)/                // ...
+                                      yDiv*(yMaxRight-yMinRight));           // convert axis values into string
+                String[] yAxisRightMS=split(yAxisRight,'.');             // 
+           
+                text(yAxisRightMS[0]+"."+yAxisRightMS[1].charAt(0),     // Right Y axis text
+                     xPos+Width+15,float(yDiv-y)/yDiv*Height+yPos+3);   // it's x,y location
+            
+                noFill();
+            }stroke(0);
+        }
+    }
+      
+    /*  =========================================================================================
+        Bar graph
+        ==========================================================================================  */   
+      
+    void Bar(float[] a, int from, int to) {
+        
+        stroke(GraphColor);
+        fill(GraphColor);
+          
+        if(from<0){                                      // If the From or To value is out of bounds 
+            for (int x=0; x Make sure time array doesn't decrease (go back in time) 
+               ===========================================================================*/
+            if(ix[i+1]){
+                   
+                    ErrorFlag=true;
+                
+                }
+            }
+         
+            /* =================================================================================       
+               First and last bits can't be part of the curve, no points before first bit, 
+               none after last bit. So a streight line is drawn instead   
+               ================================================================================= */ 
+
+            if(i==0 || i==x.length-2)line(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                                          xPos+(x[i+1]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i+1]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                                            
+            /* =================================================================================       
+               For the rest of the array a curve (spline curve) can be created making the graph 
+               smooth.     
+               ================================================================================= */ 
+                            
+            curveVertex( xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                         yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                           
+            /* =================================================================================       
+               If the Dot option is true, Place a dot at each data point.  
+               ================================================================================= */    
+           
+            if(Dot)ellipse(
+                xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                2,2
+                           );
+                             
+            /* =================================================================================       
+               Highlights points closest to Mouse X position   
+               =================================================================================*/ 
+                          
+            if( abs(mouseX-(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width))<5 ){
+                
+                 
+                float yLinePosition = yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height;
+                float xLinePosition = xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width;
+                strokeWeight(1);stroke(240);
+                // line(xPos,yLinePosition,xPos+Width,yLinePosition);
+                strokeWeight(2);stroke(GraphColor);
+                  
+                ellipse(xLinePosition,yLinePosition,4,4);
+            }
+        }  
+       
+        endShape(); 
+        yMax=tempyMax; yMin=tempyMin;
+        float xAxisTitleWidth=textWidth(str(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1])));
+           
+        if((mouseX>xPos&mouseX<(xPos+Width))&(mouseY>yPos&mouseY<(yPos+Height))){   
+            if(ShowMouseLines){
+                // if(mouseXxPos+Width)xlocation=xPos+Width;
+                else xlocation=mouseX;
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+                // Rectangle and x position
+                line(xlocation,yPos,xlocation,yPos+Height);
+                rect(xlocation-xAxisTitleWidth/2-10,yPos+Height-16,xAxisTitleWidth+20,12);
+            
+                textAlign(CENTER); fill(160);
+                text(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1]),xlocation,yPos+Height-6);
+            
+                // if(mouseYyPos+Height)ylocation=yPos+Height;
+                else ylocation=mouseY;
+          
+                // Rectangle and y position
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+            
+                line(xPos,ylocation,xPos+Width,ylocation);
+                int yAxisTitleWidth=int(textWidth(str(map(ylocation,yPos,yPos+Height,y[0],y[y.length-1]))) );
+                rect(xPos-15+3,ylocation-6, -60 ,12);
+            
+                textAlign(RIGHT); fill(GraphColor);//StrokeColor
+                //    text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos+Width+3,yPos+Height+4);
+                text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos -15,ylocation+4);
+                if(RightAxis){ 
+                          
+                    stroke(200); strokeWeight(0.5);fill(255);color(50);
+                           
+                    rect(xPos+Width+15-3,ylocation-6, 60 ,12);  
+                    textAlign(LEFT); fill(160);
+                    text(map(ylocation,yPos+Height,yPos,yMinRight,yMaxRight),xPos+Width+15,ylocation+4);
+                }
+                noStroke();noFill();
+            }
+        }
+    }
+
+    void smoothLine(float[] x ,float[] y, float[] z, float[] a ) {
+        GraphColor=color(188,53,53);
+        smoothLine(x ,y);
+        GraphColor=color(193-100,216-100,16);
+        smoothLine(z ,a);
+    }
+}
+
diff --git a/src/Processing/ManualDrive/zgraph/zgraph.pde b/src/Processing/ManualDrive/zgraph/zgraph.pde
new file mode 100644
index 0000000..9084591
--- /dev/null
+++ b/src/Processing/ManualDrive/zgraph/zgraph.pde
@@ -0,0 +1,274 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * RedBear Zumo Bot interactive control with real-time acceleration display
+ * 
+ * Before running this program, start the zumo bot and connect to its
+ * WiFi network.
+ */
+
+import processing.net.Client;
+
+Client client;           /* data socket connected to zumo IMU data server */
+
+/* IMU data from zumo */
+char[] imuBuffer = new char[72];
+
+int    MAX_FPS = 30;     /* max frames/sec, i.e., rate of calls to draw() */
+int    MAX_XVALS = 256;  /* max # of samples in the x-axis */
+int    MAX_PENDING = 6;  /* max # of outstanding commands to send */
+
+String LOG_NAME  = "zumo_log.txt";
+
+/* IMU data graph object */
+Graph graph1 = new Graph(150, 80, 400, 200, color (200, 20, 20));
+
+/* IMU data values to graph */
+float[] time = { 0 };       
+float[] ax   = { 0 }; 
+float[] ay   = { 0 };
+float[] az   = { 0 };
+
+char curGraph = 'A';
+int curGraphOffset = 1;
+long startMillis = 0;
+float curTime = 0;
+boolean scaleData = true;
+
+PrintWriter log = null;  /* optional data log */
+
+/*
+ *  ======== setup ========
+ */
+void setup() 
+{
+    /* create minimal text output window for acc data */
+    size(650, 350); 
+   
+    graph1.xLabel = " Time (s)";
+    graph1.yLabel = "Acceleration (m/s^2)";
+    graph1.Title  = " Acceleration: (x, y, z) vs. t ";  
+    graph1.yMin   = 0;
+    graph1.yMax   = 0;
+
+    /* slow the draw() rate down to MAX_FPS frames/sec */
+    frameRate(MAX_FPS);
+        
+    /* Connect to zumo's command server IP address and port */
+    client = new Client(this, "192.168.1.1", 8080);
+
+    graph1.DrawAxis();
+}
+
+/*
+ *  ======== draw ========
+ */
+boolean cont = false;  /* continuously send previous command to get IMU data */
+String  pcmd = " \n";  /* previous command */
+int     pending = 0;   /* number of outstanding commands sent to zumo bot */
+
+void draw() 
+{
+    /* send server commands based on keyboard input */
+    if (keyPressed || cont) {
+        if (startMillis == 0) {
+            /* capture initial start time */
+            startMillis = java.lang.System.currentTimeMillis();
+        }
+        String cmd = pcmd;
+        if  (key == ' ' || key == 'w' || key == 'a' || key == 's' || key == 'd') {
+            cmd = key + "\n";
+        }
+        else if (key == 'x') cont = false;
+        else if (key == 'c') cont = true;
+        else if (key == 'L') {
+            if (log == null) {
+                log = createWriter(LOG_NAME);
+                println("logging started ...");
+            }
+        }
+        else if (key == 'l') {
+            if (log != null) {
+                log.flush();
+                log.close();
+                log = null;
+                println("logging stopped.");
+            }
+        }
+        else if (key == 'G') {
+            if (curGraph != 'G') {
+                curGraph = 'G';
+                curGraphOffset = 5;
+                graph1.yLabel = "Rotational Speed (deg/s)";
+                graph1.Title = " Gyro: (x, y, z) vs. t ";  
+                graph1.yMin = 0;
+                graph1.yMax = 0;
+                ax = new float [] { 0 }; 
+                ay = new float [] { 0 };
+                az = new float [] { 0 };
+                time = new float [] { curTime }; 
+            }
+        }
+        else if (key == 'A') {
+            if (curGraph != 'A') {
+                curGraph = 'A';
+                curGraphOffset = 1;
+                graph1.yLabel = "Acceleration (m/s^2)";
+                graph1.Title = " Acceleration: (x, y, z) vs. t ";  
+                graph1.yMin = 0;
+                graph1.yMax = 0;
+                ax = new float [] { 0 }; 
+                ay = new float [] { 0 };
+                az = new float [] { 0 };
+                time = new float [] { curTime }; 
+            }
+        }
+
+        /* stop sending commands if too many commands are outstanding */
+        if (pending < MAX_PENDING) {
+            client.write(cmd);
+            pcmd = cmd;
+            pending++;
+        }
+    }
+
+    /* Read IMU data from server and display it */
+    if (client.available() >= imuBuffer.length) {
+        /* read IMU data */
+        for (int i = 0; i < imuBuffer.length; i++) {
+            imuBuffer[i] = client.readChar();
+        }
+
+        /* decrement pending count so we can send more commands */
+        if (--pending < 0) {
+            println("Yikes! pending = " + pending);
+            pending = 0;
+        }
+       
+        /* parse IMU data and display it */
+        String input = new String(imuBuffer);
+        String [] tokens = splitTokens(input, " ");
+        if (tokens.length > (2 + curGraphOffset)) {
+            newPoint(tokens[0 + curGraphOffset],  /* x value */
+                     tokens[1 + curGraphOffset],  /* y value */
+                     tokens[2 + curGraphOffset]); /* z value */
+        }
+    }
+}
+
+/*
+ *  ======== newPoint ========
+ *  add new point to data arrays and update graph
+ */
+void newPoint(String x, String y, String z)
+{
+    /* get next values */
+    curTime = (java.lang.System.currentTimeMillis() - startMillis) / 1000.0;
+    if (log != null) {
+        log.println("A: " + x + " " + y + " " + z);
+    }
+    
+    /* update all data arrays */
+    time = pushv(time, curTime);
+    ax = pushv(ax, scale(curGraph, x));
+    ay = pushv(ay, scale(curGraph, y));
+    az = pushv(az, scale(curGraph, z));
+
+    /* update display */
+    updatePlots();
+}
+
+/*
+ *  ======== pushv ========
+ *  append new data sample to arr[] and return new array
+ */
+float[] pushv(float [] arr, float val)
+{
+    float [] tmp = arr;
+    
+    if (arr.length < MAX_XVALS) {
+        tmp = append(arr, val);
+    }
+    else {
+        /* shift data within arr to make room for new value */
+        int i;
+        for (i = 1; i < MAX_XVALS; i++) {
+            arr[i - 1] = arr[i];
+        }
+        arr[MAX_XVALS - 1] = val;
+    }
+
+    return (tmp);
+}
+
+/*
+ *  ======== updatePlots ========
+ *  redraw IMU graph
+ */
+void updatePlots()
+{
+    background(255);
+
+    graph1.xMax = max(time);
+    graph1.xMin = min(time);
+    graph1.yMax = max(max(max(az), max(ax), max(ay)), graph1.yMax);
+    graph1.yMin = min(min(min(az), min(ax), min(ay)), graph1.yMin);
+
+    graph1.DrawAxis();
+      
+    graph1.GraphColor = color(200, 40, 40);  
+    graph1.LineGraph(time, ax);      
+      
+    graph1.GraphColor = color(40, 200, 40);   
+    graph1.LineGraph(time, ay);
+
+    graph1.GraphColor = color(40, 40, 200);   
+    graph1.LineGraph(time, az);
+}
+
+/*
+ *  ======== scale =========
+ */
+float scale(char type, String value)
+{
+    if (scaleData) {
+        switch (type) {
+            case 'A':
+                return (float(value) * (2 * 9.80665) / float(32768)); /* acc data is +-2G */
+
+            case 'G':
+                return (float(value) * 250 / float(32768));           /* gyro data is +-250 deg/sec */
+        }
+    }
+    return (float(value));
+}
diff --git a/src/Processing/MasterSlave/GraphClass.pde b/src/Processing/MasterSlave/GraphClass.pde
new file mode 100644
index 0000000..39f9b7c
--- /dev/null
+++ b/src/Processing/MasterSlave/GraphClass.pde
@@ -0,0 +1,371 @@
+/*
+ * from: https://github.com/sebnil/RealtimePlotter
+ *
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2013 Sebatian Nilsson
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/*   =================================================================================       
+     The Graph class contains functions and variables that have been created to draw 
+     graphs. Here is a quick list of functions within the graph class:
+          
+     Graph(int x, int y, int w, int h,color k)
+     DrawAxis()
+     Bar([])
+     smoothLine([][])
+     DotGraph([][])
+     LineGraph([][]) 
+     
+     =================================================================================*/   
+
+    
+class Graph 
+{
+      
+    boolean Dot=true;            // Draw dots at each data point if true
+    boolean RightAxis;            // Draw the next graph using the right axis if true
+    boolean ErrorFlag=false;      // If the time array isn't in ascending order, make true  
+    boolean ShowMouseLines=true;  // Draw lines and give values of the mouse position
+    
+    int     xDiv=10,yDiv=5;            // Number of sub divisions
+    int     xPos,yPos;            // location of the top left corner of the graph  
+    int     Width,Height;         // Width and height of the graph
+    
+
+    color   GraphColor;
+    color   BackgroundColor=color(255);  
+    color   StrokeColor=color(180);     
+      
+    String  Title="Title";          // Default titles
+    String  xLabel="x - Label";
+    String  yLabel="y - Label";
+
+    float   yMax=1024, yMin=0;      // Default axis dimensions
+    float   xMax=10, xMin=0;
+    float   yMaxRight=1024,yMinRight=0;
+  
+    PFont   Font;                   // Selected font used for text 
+      
+    //    int Peakcounter=0,nPeakcounter=0;
+     
+    Graph(int x, int y, int w, int h, color k) {  // The main declaration function
+        xPos = x;
+        yPos = y;
+        Width = w;
+        Height = h;
+        GraphColor = k;
+    }
+     
+    void DrawAxis(){
+       
+        /*  =========================================================================================
+            Main axes Lines, Graph Labels, Graph Background
+            ==========================================================================================  */
+    
+        fill(BackgroundColor); color(0);stroke(StrokeColor);strokeWeight(1);
+        int t=60;
+        
+        rect(xPos-t*1.6,yPos-t,Width+t*2.5,Height+t*2);            // outline
+        textAlign(CENTER);textSize(18);
+        float c=textWidth(Title);
+        fill(BackgroundColor); color(0);stroke(0);strokeWeight(1);
+        rect(xPos+Width/2-c/2,yPos-35,c,0);                         // Heading Rectangle  
+        
+        fill(0);
+        text(Title,xPos+Width/2,yPos-37);                            // Heading Title
+        textAlign(CENTER);textSize(14);
+        text(xLabel,xPos+Width/2,yPos+Height+t/1.5);                     // x-axis Label 
+        
+        rotate(-PI/2);                                               // rotate -90 degrees
+        text(yLabel,-yPos-Height/2,xPos-t*1.6+20);                   // y-axis Label  
+        rotate(PI/2);                                                // rotate back
+        
+        textSize(10); noFill(); stroke(0); smooth();strokeWeight(1);
+        //Edges
+        line(xPos-3,yPos+Height,xPos-3,yPos);                        // y-axis line 
+        line(xPos-3,yPos+Height,xPos+Width+5,yPos+Height);           // x-axis line 
+          
+        stroke(200);
+        if(yMin<0){
+            line(xPos-7,                                       // zero line 
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height,   // 
+                 xPos+Width,
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height
+                 );
+        }
+          
+        if(RightAxis){                                       // Right-axis line   
+            stroke(0);
+            line(xPos+Width+3,yPos+Height,xPos+Width+3,yPos);
+        }
+            
+        /*  =========================================================================================
+            Sub-devisions for both axes, left and right
+            ==========================================================================================  */
+            
+        stroke(0);
+            
+        for(int x=0; x<=xDiv; x++){
+       
+            /*  =========================================================================================
+                x-axis
+                ==========================================================================================  */
+             
+            line(float(x)/xDiv*Width+xPos-3,yPos+Height,       //  x-axis Sub devisions    
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+5);     
+                 
+            textSize(10);                                      // x-axis Labels
+            String xAxis=str(xMin+float(x)/xDiv*(xMax-xMin));  // the only way to get a specific number of decimals 
+            String[] xAxisMS=split(xAxis,'.');                 // is to split the float into strings 
+            text(xAxisMS[0]+"."+xAxisMS[1].charAt(0),          // ...
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+15);   // x-axis Labels
+        }
+          
+          
+        /*  =========================================================================================
+            left y-axis
+            ==========================================================================================  */
+          
+        for(int y=0; y<=yDiv; y++){
+            line(xPos-3,float(y)/yDiv*Height+yPos,                // ...
+                 xPos-7,float(y)/yDiv*Height+yPos);              // y-axis lines 
+            
+            textAlign(RIGHT);fill(20);
+            
+            String yAxis=str(yMin+float(y)/yDiv*(yMax-yMin));     // Make y Label a string
+            String[] yAxisMS=split(yAxis,'.');                    // Split string
+           
+            text(yAxisMS[0]+"."+yAxisMS[1].charAt(0),             // ... 
+                 xPos-15,float(yDiv-y)/yDiv*Height+yPos+3);       // y-axis Labels 
+                        
+                        
+            /*  =========================================================================================
+                right y-axis
+                ==========================================================================================  */
+            
+            if(RightAxis){
+             
+                color(GraphColor); stroke(GraphColor);fill(20);
+            
+                line(xPos+Width+3,float(y)/yDiv*Height+yPos,             // ...
+                     xPos+Width+7,float(y)/yDiv*Height+yPos);            // Right Y axis sub devisions
+                   
+                textAlign(LEFT); 
+            
+                String yAxisRight=str(yMinRight+float(y)/                // ...
+                                      yDiv*(yMaxRight-yMinRight));           // convert axis values into string
+                String[] yAxisRightMS=split(yAxisRight,'.');             // 
+           
+                text(yAxisRightMS[0]+"."+yAxisRightMS[1].charAt(0),     // Right Y axis text
+                     xPos+Width+15,float(yDiv-y)/yDiv*Height+yPos+3);   // it's x,y location
+            
+                noFill();
+            }stroke(0);
+        }
+    }
+      
+    /*  =========================================================================================
+        Bar graph
+        ==========================================================================================  */   
+      
+    void Bar(float[] a, int from, int to) {
+        
+        stroke(GraphColor);
+        fill(GraphColor);
+          
+        if(from<0){                                      // If the From or To value is out of bounds 
+            for (int x=0; x Make sure time array doesn't decrease (go back in time) 
+               ===========================================================================*/
+            if(ix[i+1]){
+                   
+                    ErrorFlag=true;
+                
+                }
+            }
+         
+            /* =================================================================================       
+               First and last bits can't be part of the curve, no points before first bit, 
+               none after last bit. So a streight line is drawn instead   
+               ================================================================================= */ 
+
+            if(i==0 || i==x.length-2)line(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                                          xPos+(x[i+1]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i+1]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                                            
+            /* =================================================================================       
+               For the rest of the array a curve (spline curve) can be created making the graph 
+               smooth.     
+               ================================================================================= */ 
+                            
+            curveVertex( xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                         yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                           
+            /* =================================================================================       
+               If the Dot option is true, Place a dot at each data point.  
+               ================================================================================= */    
+           
+            if(Dot)ellipse(
+                xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                2,2
+                           );
+                             
+            /* =================================================================================       
+               Highlights points closest to Mouse X position   
+               =================================================================================*/ 
+                          
+            if( abs(mouseX-(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width))<5 ){
+                
+                 
+                float yLinePosition = yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height;
+                float xLinePosition = xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width;
+                strokeWeight(1);stroke(240);
+                // line(xPos,yLinePosition,xPos+Width,yLinePosition);
+                strokeWeight(2);stroke(GraphColor);
+                  
+                ellipse(xLinePosition,yLinePosition,4,4);
+            }
+        }  
+       
+        endShape(); 
+        yMax=tempyMax; yMin=tempyMin;
+        float xAxisTitleWidth=textWidth(str(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1])));
+           
+        if((mouseX>xPos&mouseX<(xPos+Width))&(mouseY>yPos&mouseY<(yPos+Height))){   
+            if(ShowMouseLines){
+                // if(mouseXxPos+Width)xlocation=xPos+Width;
+                else xlocation=mouseX;
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+                // Rectangle and x position
+                line(xlocation,yPos,xlocation,yPos+Height);
+                rect(xlocation-xAxisTitleWidth/2-10,yPos+Height-16,xAxisTitleWidth+20,12);
+            
+                textAlign(CENTER); fill(160);
+                text(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1]),xlocation,yPos+Height-6);
+            
+                // if(mouseYyPos+Height)ylocation=yPos+Height;
+                else ylocation=mouseY;
+          
+                // Rectangle and y position
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+            
+                line(xPos,ylocation,xPos+Width,ylocation);
+                int yAxisTitleWidth=int(textWidth(str(map(ylocation,yPos,yPos+Height,y[0],y[y.length-1]))) );
+                rect(xPos-15+3,ylocation-6, -60 ,12);
+            
+                textAlign(RIGHT); fill(GraphColor);//StrokeColor
+                //    text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos+Width+3,yPos+Height+4);
+                text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos -15,ylocation+4);
+                if(RightAxis){ 
+                          
+                    stroke(200); strokeWeight(0.5);fill(255);color(50);
+                           
+                    rect(xPos+Width+15-3,ylocation-6, 60 ,12);  
+                    textAlign(LEFT); fill(160);
+                    text(map(ylocation,yPos+Height,yPos,yMinRight,yMaxRight),xPos+Width+15,ylocation+4);
+                }
+                noStroke();noFill();
+            }
+        }
+    }
+
+    void smoothLine(float[] x ,float[] y, float[] z, float[] a ) {
+        GraphColor=color(188,53,53);
+        smoothLine(x ,y);
+        GraphColor=color(193-100,216-100,16);
+        smoothLine(z ,a);
+    }
+}
+
diff --git a/src/Processing/MasterSlave/MasterSlave.pde b/src/Processing/MasterSlave/MasterSlave.pde
new file mode 100644
index 0000000..5563cca
--- /dev/null
+++ b/src/Processing/MasterSlave/MasterSlave.pde
@@ -0,0 +1,280 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * RedBear Zumo Bot interactive control with real-time acceleration display
+ * 
+ * Before running this program, start the zumo bot and connect to its
+ * WiFi network.
+ */
+
+import processing.net.Client;
+import hypermedia.net.*;
+
+UDP udp;  /* define the UDP object */    
+
+/* IMU data from zumo */
+char[] imuBuffer = new char[72];
+
+String[] tokens;
+
+char  cmd = ' ';  /* current command */
+boolean cont = false;  /* continuously send previous command to get IMU data */
+
+/* zumo IP Address and destination port */
+String IP = "192.168.1.1";
+int PORT = 8080;
+
+int    MAX_FPS = 30;     /* max frames/sec, i.e., rate of calls to draw() */
+int    MAX_XVALS = 80;   /* max # of samples in the x-axis */
+int    MAX_PENDING = 6;  /* max # of outstanding commands to send */
+
+String LOG_NAME  = "zumo_log.txt";
+
+/* IMU data graph object */
+Graph graph1 = new Graph(150, 80, 400, 200, color (200, 20, 20));
+
+/* IMU data values to graph */
+float[] time = { 0 };       
+float[] ax   = { 0 }; 
+float[] ay   = { 0 };
+float[] az   = { 0 };
+
+char curGraph = 'A';
+int curGraphOffset = 1;
+float curTime = 0;
+boolean scaleData = true;
+
+PrintWriter log = null;  /* optional data log */
+
+/*
+ *  ======== setup ========
+ */
+void setup() 
+{
+    /* create minimal text output window for acc data */
+    size(650, 350); 
+   
+    graph1.xLabel = " Time (s)";
+    graph1.yLabel = "Acceleration (m/s^2)";
+    graph1.Title  = " Acceleration: (x, y, z) vs. t ";  
+    graph1.yMin   = 0;
+    graph1.yMax   = 0;
+
+    /* slow the draw() rate down to MAX_FPS frames/sec */
+    frameRate(MAX_FPS);
+        
+    /* create a new datagram connection on port 6000 */
+    udp = new UDP(this, 6000);  
+}
+
+/*
+ *  ======== draw ========
+ */
+int loopCount = 0;
+
+void draw() 
+{
+    /* send server commands based on keyboard input */
+    if (cont) {
+  
+        if(!keyPressed){
+          cmd = ' ';
+        }
+        /* send the current command every 4th iteration */
+        if (loopCount % 4 == 0) {
+            /* send the command to the master zumo at 192.168.1.1 */
+            udp.send(String.valueOf(cmd), IP, PORT);
+        }
+        
+        loopCount++;
+    }
+
+}
+
+void keyPressed()
+{
+    if  (key == 'w' || key == 'a' || key == 's' || key == 'd') {
+        cmd = key;
+    }
+    else if (key == 'x') cont = false;
+    else if (key == 'c') cont = true;
+    else if (key == 'L') {
+        if (log == null) {
+            log = createWriter(LOG_NAME);
+            println("logging started ...");
+        }
+    }
+    else if (key == 'l') {
+        if (log != null) {
+            log.flush();
+            log.close();
+            log = null;
+            println("logging stopped.");
+        }
+    }
+    else if (key == 'G') {
+        if (curGraph != 'G') {
+            curGraph = 'G';
+            curGraphOffset = 5;
+            graph1.yLabel = "Rotational Speed (deg/s)";
+            graph1.Title = " Gyro: (x, y, z) vs. t ";  
+            graph1.yMin = 0;
+            graph1.yMax = 0;
+            ax = new float [] { 0 }; 
+            ay = new float [] { 0 };
+            az = new float [] { 0 };
+            time = new float [] { curTime }; 
+        }
+    }
+    else if (key == 'A') {
+        if (curGraph != 'A') {
+            curGraph = 'A';
+            curGraphOffset = 1;
+            graph1.yLabel = "Acceleration (m/s^2)";
+            graph1.Title = " Acceleration: (x, y, z) vs. t ";  
+            graph1.yMin = 0;
+            graph1.yMax = 0;
+            ax = new float [] { 0 }; 
+            ay = new float [] { 0 };
+            az = new float [] { 0 };
+            time = new float [] { curTime }; 
+        }
+    }  
+}
+
+/*
+ *  ======== newPoint ========
+ *  add new point to data arrays and update graph
+ */
+void newPoint(String x, String y, String z)
+{
+    /* get next values */
+    curTime += 1;
+    if (log != null) {
+        log.println("A: " + x + " " + y + " " + z);
+    }
+    
+    /* update all data arrays */
+    time = pushv(time, curTime);
+    ax = pushv(ax, scale(curGraph, x));
+    ay = pushv(ay, scale(curGraph, y));
+    az = pushv(az, scale(curGraph, z));
+
+    /* update display */
+    updatePlots();
+}
+
+/*
+ *  ======== pushv ========
+ *  append new data sample to arr[] and return new array
+ */
+float[] pushv(float [] arr, float val)
+{
+    float [] tmp = arr;
+    
+    if (arr.length < MAX_XVALS) {
+        tmp = append(arr, val);
+    }
+    else {
+        /* shift data within arr to make room for new value */
+        int i;
+        for (i = 1; i < MAX_XVALS; i++) {
+            arr[i - 1] = arr[i];
+        }
+        arr[MAX_XVALS - 1] = val;
+    }
+
+    return (tmp);
+}
+
+/*
+ *  ======== updatePlots ========
+ *  redraw IMU graph
+ */
+void updatePlots()
+{
+    background(255);
+
+    graph1.xMax = max(time);
+    graph1.xMin = min(time);
+    graph1.yMax = max(max(max(az), max(ax), max(ay)), graph1.yMax);
+    graph1.yMin = min(min(min(az), min(ax), min(ay)), graph1.yMin);
+                                                  
+    graph1.DrawAxis();
+      
+    graph1.GraphColor = color(200, 40, 40);  
+    graph1.LineGraph(time, ax);      
+      
+    graph1.GraphColor = color(40, 200, 40);   
+    graph1.LineGraph(time, ay);
+
+    graph1.GraphColor = color(40, 40, 200);   
+    graph1.LineGraph(time, az);
+}
+
+float scale(char type, String value)
+{
+  if (scaleData) {
+    switch (type) {
+        case 'A':
+            return (float(value) * (2 * 9.80665) / float(32768)); /* acc data is +-2G */
+
+        case 'G':
+            return (float(value) * 250 / float(32768));           /* gyro data is +-250 deg/sec */
+    }
+  }
+    return (float(value));
+}
+
+void receive(byte[] data, String ip, int port) {   // <-- extended handler  
+  
+  for(int i=0; i < data.length; i++){
+    print(char(data[i]));    
+    imuBuffer[i] = char(data[i]);
+  }
+  println();   
+       
+  /* parse IMU data and display it */
+  String input = new String(imuBuffer);
+  tokens = splitTokens(input, " ");
+  
+  if (cont) {
+      if (tokens.length > (2 + curGraphOffset)) {
+          newPoint(tokens[0 + curGraphOffset],  /* x value */
+                   tokens[1 + curGraphOffset],  /* y value */
+                   tokens[2 + curGraphOffset]); /* z value */
+      }  
+     
+  }
+  
+}
diff --git a/src/Processing/SplineDrive/SplineDrive.pde b/src/Processing/SplineDrive/SplineDrive.pde
new file mode 100644
index 0000000..a21c1e3
--- /dev/null
+++ b/src/Processing/SplineDrive/SplineDrive.pde
@@ -0,0 +1,174 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * RedBear Zumo Bot interactive control with real-time acceleration display
+ * 
+ * Before running this program, start the zumo bot and connect to its
+ * WiFi network.
+ */
+
+import processing.net.Client;
+
+import java.awt.Point;
+import java.util.ArrayList;
+import java.util.List;
+
+Client client;           /* data socket connected to zumo IMU data server */
+
+int    MAX_FPS = 30;     /* max frames/sec, i.e., rate of calls to draw() */
+int    MAX_XVALS = 80;   /* max # of samples in the x-axis */
+int    MAX_PENDING = 6;  /* max # of outstanding commands to send */
+
+ArrayList pointList = new ArrayList();
+
+String LOG_NAME  = "zumo_log.txt";
+
+PrintWriter log = null;  /* optional data log */
+
+/*
+ *  ======== setup ========
+ */
+void setup() 
+{
+    /* create large drawing window for spline */
+    size(800, 800); 
+    
+    smooth();
+    background(153, 204, 255);
+    noLoop();
+    
+    fill(0);
+    text("Click anywhere to begin spline"
+         +"\n"+"Blue point indicates start of spline"
+         +"\n"+"Press 'x' to clear, 's' to send spline", 10, 20);  
+
+    /* slow the draw() rate down to MAX_FPS frames/sec */
+    frameRate(MAX_FPS);
+        
+    /* Connect to zumo's command server IP address and port */
+    client = new Client(this, "192.168.1.1", 8080);
+}
+
+/*
+ *  ======== draw ========
+ */
+boolean cont = false;  /* continuously send previous command to get IMU data */
+String  pcmd = " \n";  /* previous command */
+int     pending = 0;   /* number of outstanding commands sent to zumo bot */
+
+void draw() 
+{
+     background(153, 204, 255);
+     
+     fill(0);
+     text("Click anywhere to begin spline"
+         +"\n"+"Blue point indicates start of spline"
+         +"\n"+"Press 'x' to clear, 's' to send spline", 10, 20);  
+  
+     noFill();
+     stroke(0);
+     beginShape();
+     if (pointList.size() > 1) {
+       for (int i=0; i Make sure time array doesn't decrease (go back in time) 
+               ===========================================================================*/
+            if(ix[i+1]){
+                   
+                    ErrorFlag=true;
+                
+                }
+            }
+         
+            /* =================================================================================       
+               First and last bits can't be part of the curve, no points before first bit, 
+               none after last bit. So a streight line is drawn instead   
+               ================================================================================= */ 
+
+            if(i==0 || i==x.length-2)line(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                                          xPos+(x[i+1]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i+1]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                                            
+            /* =================================================================================       
+               For the rest of the array a curve (spline curve) can be created making the graph 
+               smooth.     
+               ================================================================================= */ 
+                            
+            curveVertex( xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                         yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                           
+            /* =================================================================================       
+               If the Dot option is true, Place a dot at each data point.  
+               ================================================================================= */    
+           
+            if(Dot)ellipse(
+                xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                2,2
+                           );
+                             
+            /* =================================================================================       
+               Highlights points closest to Mouse X position   
+               =================================================================================*/ 
+                          
+            if( abs(mouseX-(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width))<5 ){
+                
+                 
+                float yLinePosition = yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height;
+                float xLinePosition = xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width;
+                strokeWeight(1);stroke(240);
+                // line(xPos,yLinePosition,xPos+Width,yLinePosition);
+                strokeWeight(2);stroke(GraphColor);
+                  
+                ellipse(xLinePosition,yLinePosition,4,4);
+            }
+        }  
+       
+        endShape(); 
+        yMax=tempyMax; yMin=tempyMin;
+        float xAxisTitleWidth=textWidth(str(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1])));
+           
+        if((mouseX>xPos&mouseX<(xPos+Width))&(mouseY>yPos&mouseY<(yPos+Height))){   
+            if(ShowMouseLines){
+                // if(mouseXxPos+Width)xlocation=xPos+Width;
+                else xlocation=mouseX;
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+                // Rectangle and x position
+                line(xlocation,yPos,xlocation,yPos+Height);
+                rect(xlocation-xAxisTitleWidth/2-10,yPos+Height-16,xAxisTitleWidth+20,12);
+            
+                textAlign(CENTER); fill(160);
+                text(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1]),xlocation,yPos+Height-6);
+            
+                // if(mouseYyPos+Height)ylocation=yPos+Height;
+                else ylocation=mouseY;
+          
+                // Rectangle and y position
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+            
+                line(xPos,ylocation,xPos+Width,ylocation);
+                int yAxisTitleWidth=int(textWidth(str(map(ylocation,yPos,yPos+Height,y[0],y[y.length-1]))) );
+                rect(xPos-15+3,ylocation-6, -60 ,12);
+            
+                textAlign(RIGHT); fill(GraphColor);//StrokeColor
+                //    text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos+Width+3,yPos+Height+4);
+                text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos -15,ylocation+4);
+                if(RightAxis){ 
+                          
+                    stroke(200); strokeWeight(0.5);fill(255);color(50);
+                           
+                    rect(xPos+Width+15-3,ylocation-6, 60 ,12);  
+                    textAlign(LEFT); fill(160);
+                    text(map(ylocation,yPos+Height,yPos,yMinRight,yMaxRight),xPos+Width+15,ylocation+4);
+                }
+                noStroke();noFill();
+            }
+        }
+    }
+
+    void smoothLine(float[] x ,float[] y, float[] z, float[] a ) {
+        GraphColor=color(188,53,53);
+        smoothLine(x ,y);
+        GraphColor=color(193-100,216-100,16);
+        smoothLine(z ,a);
+    }
+}
+
diff --git a/src/Processing/UDPBroadcast/UDPBroadcast.pde b/src/Processing/UDPBroadcast/UDPBroadcast.pde
new file mode 100644
index 0000000..a146c61
--- /dev/null
+++ b/src/Processing/UDPBroadcast/UDPBroadcast.pde
@@ -0,0 +1,570 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * RedBear Zumo Bot interactive control with real-time acceleration display 
+ * of multiple bots. Supports data graphing for up to four zumos 
+ * 
+ * Before running this program, start the all zumo bots and connect to a shared
+ * network (i.e. all zumos and the PC are all on the same network)
+ */
+
+import processing.net.Client;
+import hypermedia.net.*;
+import java.util.ArrayList;
+
+UDP udp;  /* define the UDP object */    
+
+/* IMU data from zumo */
+char[] imuBuffer = new char[72];
+
+/* parsed IMU data to graph */
+String[] data1 = new String[12];
+String[] data2 = new String[12];
+String[] data3 = new String[12];
+String[] data4 = new String[12];
+
+/* list of IP addresses for Zumos that have sent IMU packets 
+   the zumo's index in this list corresponds to its graphs number */
+String[] zumoIPs = new String[4]; /* an empty string means no zumo connected */
+
+char  cmd = ' ';  /* current command */
+boolean cont = false;  /* continuously send previous command to get IMU data */
+
+/* broadcast IP */
+String IP = "192.168.1.255";
+int PORT = 8080;
+
+int    MAX_FPS = 30;     /* max frames/sec, i.e., rate of calls to draw() */
+int    MAX_XVALS = 80;   /* max # of samples in the x-axis */
+int    MAX_PENDING = 6;  /* max # of outstanding commands to send */
+
+String LOG_NAME  = "zumo_log.txt";
+
+/* IMU data graph objects */
+Graph graph1 = new Graph(150, 80, 300, 150, color (200, 20, 20));
+Graph graph2 = new Graph(600, 80, 300, 150, color (200, 20, 20));
+Graph graph3 = new Graph(150, 340, 300, 150, color (200, 20, 20));
+Graph graph4 = new Graph(600, 340, 300, 150, color (200, 20, 20));
+
+/* IMU data values to graph */
+float[] time1 = { 0 };  
+float[] time2 = { 0 };
+float[] time3 = { 0 };
+float[] time4 = { 0 };
+
+float[] x1   = { 0 }; 
+float[] y1   = { 0 };
+float[] z1   = { 0 };
+
+float[] x2   = { 0 }; 
+float[] y2   = { 0 };
+float[] z2   = { 0 };
+
+float[] x3   = { 0 }; 
+float[] y3   = { 0 };
+float[] z3   = { 0 };
+
+float[] x4   = { 0 }; 
+float[] y4   = { 0 };
+float[] z4   = { 0 };
+
+char curGraph = 'A';
+int curGraphOffset = 1;
+
+long startMillis = 0;
+float curTime1 = 0;
+float curTime2 = 0;
+float curTime3 = 0;
+float curTime4 = 0;
+
+boolean scaleData = true;
+
+PrintWriter log = null;  /* optional data log */
+
+/*
+ *  ======== setup ========
+ */
+void setup() 
+{
+    /* text output window for four graphs */
+    size(1000, 550); 
+    
+    /* initialize all graphs */
+    graph1.xLabel = " Time (s)";
+    graph1.yLabel = "Acceleration (m/s^2)";
+    graph1.Title  = " Zumo1";  
+    graph1.yMin   = 0;
+    graph1.yMax   = 0;
+    
+    graph2.xLabel = " Time (s)";
+    graph2.yLabel = "Acceleration (m/s^2)";
+    graph2.Title  = " Zumo2";  
+    graph2.yMin   = 0;
+    graph2.yMax   = 0;
+    
+    graph3.xLabel = " Time (s)";
+    graph3.yLabel = "Acceleration (m/s^2)";
+    graph3.Title  = " Zumo3";  
+    graph3.yMin   = 0;
+    graph3.yMax   = 0;
+    
+    graph4.xLabel = " Time (s)";
+    graph4.yLabel = "Acceleration (m/s^2)";
+    graph4.Title  = " Zumo4";  
+    graph4.yMin   = 0;
+    graph4.yMax   = 0;
+
+    /* slow the draw() rate down to MAX_FPS frames/sec */
+    frameRate(MAX_FPS);
+        
+    /* create a new datagram connection on port 6000 */
+    udp = new UDP(this, 6000);  
+    udp.broadcast(true);
+    udp.listen(true);
+    
+    println(udp.address());
+}
+
+/*
+ *  ======== draw ========
+ */
+int loopCount = 0;
+
+void draw() 
+{
+    /* send server commands based on keyboard input */
+    if (cont) {
+  
+        if(!keyPressed){
+          cmd = ' ';
+        }
+        
+        if(loopCount % 2 == 0){
+          /* broadcast the command every other iteration of draw */
+          udp.send(String.valueOf(cmd), IP, PORT);
+        }
+         
+        /* graph the incoming IMU data */
+        if (data1.length > (2 + curGraphOffset) && zumoIPs[0] != null) {
+          println("graph 1");
+          newPoint(data1[0 + curGraphOffset],  /* x value */
+                   data1[1 + curGraphOffset],  /* y value */
+                   data1[2 + curGraphOffset],  /* z value */
+                   1); 
+        }
+        
+        if (data2.length > (2 + curGraphOffset) && zumoIPs[1] != null) {
+          println("graph 2");
+          newPoint(data2[0 + curGraphOffset],  /* x value */
+                   data2[1 + curGraphOffset],  /* y value */
+                   data2[2 + curGraphOffset],  /* z value */
+                   2); 
+        }
+        
+        if (data3.length > (2 + curGraphOffset) && zumoIPs[2] != null) {
+          println("graph 3");
+          newPoint(data3[0 + curGraphOffset],  /* x value */
+                   data3[1 + curGraphOffset],  /* y value */
+                   data3[2 + curGraphOffset],  /* z value */
+                   3); 
+        }
+        
+        if (data4.length > (2 + curGraphOffset) && zumoIPs[3] != null) {
+          println("graph 4");
+            newPoint(data4[0 + curGraphOffset],  /* x value */
+                     data4[1 + curGraphOffset],  /* y value */
+                     data4[2 + curGraphOffset],  /* z value */
+                     4); 
+        }
+        
+        loopCount++;
+        
+        /* update display */
+        updatePlots();
+    }
+
+}
+
+void keyPressed()
+{
+    if (startMillis == 0) {
+        /* capture initial start time */
+        startMillis = java.lang.System.currentTimeMillis();
+    }
+
+    if  (key == 'w' || key == 'a' || key == 's' || key == 'd') {
+        cmd = key;
+    }
+    else if (key == 'x') cont = false;
+    else if (key == 'c') cont = true;
+    else if (key == 'L') {
+        if (log == null) {
+            log = createWriter(LOG_NAME);
+            println("logging started ...");
+        }
+    }
+    else if (key == 'l') {
+        if (log != null) {
+            log.flush();
+            log.close();
+            log = null;
+            println("logging stopped.");
+        }
+    }
+    else if (key == 'G') {
+        if (curGraph != 'G') {
+            curGraph = 'G';
+            curGraphOffset = 5;
+
+            graph1.yLabel = "Rotational Speed (deg/s)";
+            graph1.yMin = 0;
+            graph1.yMax = 0;
+            
+            graph2.yLabel = "Rotational Speed (deg/s)";
+            graph2.yMin = 0;
+            graph2.yMax = 0;
+            
+            graph3.yLabel = "Rotational Speed (deg/s)";
+            graph3.yMin = 0;
+            graph3.yMax = 0;
+            
+            graph4.yLabel = "Rotational Speed (deg/s)";
+            graph4.yMin = 0;
+            graph4.yMax = 0;
+           
+            x1   = new float [] { 0 }; 
+            y1   = new float [] { 0 };
+            z1   = new float [] { 0 };
+            
+            x2   = new float [] { 0 }; 
+            y2   = new float [] { 0 };
+            z2   = new float [] { 0 };
+            
+            x3   = new float [] { 0 }; 
+            y3   = new float [] { 0 };
+            z3   = new float [] { 0 };
+            
+            x4   = new float [] { 0 }; 
+            y4   = new float [] { 0 };
+            z4   = new float [] { 0 };
+            
+            time1 = new float [] { curTime1 }; 
+            time2 = new float [] { curTime2 }; 
+            time3 = new float [] { curTime3 }; 
+            time4 = new float [] { curTime4 }; 
+        }
+    }
+    else if (key == 'A') {
+        if (curGraph != 'A') {
+            curGraph = 'A';
+            curGraphOffset = 1;
+            
+            graph1.yLabel = "Acceleration (m/s^2)";
+            graph1.yMin = 0;
+            graph1.yMax = 0;
+            
+            graph2.yLabel = "Acceleration (m/s^2)";
+            graph2.yMin = 0;
+            graph2.yMax = 0;
+            
+            graph3.yLabel = "Acceleration (m/s^2)";
+            graph3.yMin = 0;
+            graph3.yMax = 0;
+            
+            graph4.yLabel = "Acceleration (m/s^2)";
+            graph4.yMin = 0;
+            graph4.yMax = 0;            
+           
+            x1   = new float [] { 0 }; 
+            y1   = new float [] { 0 };
+            z1   = new float [] { 0 };
+            
+            x2   = new float [] { 0 }; 
+            y2   = new float [] { 0 };
+            z2   = new float [] { 0 };
+            
+            x3   = new float [] { 0 }; 
+            y3   = new float [] { 0 };
+            z3   = new float [] { 0 };
+            
+            x4   = new float [] { 0 }; 
+            y4   = new float [] { 0 };
+            z4   = new float [] { 0 };
+            
+            time1 = new float [] { curTime1 }; 
+            time2 = new float [] { curTime2 }; 
+            time3 = new float [] { curTime3 }; 
+            time4 = new float [] { curTime4 };             
+        }
+    }  
+}
+
+/*
+ *  ======== newPoint ========
+ *  add new point to data arrays and update graph
+ */
+void newPoint(String x, String y, String z, int graphNum)
+{
+    /* log data */
+    if (log != null) {
+        log.println("A: " + x + " " + y + " " + z);
+    }
+    
+    if(x == null){
+      x = "0";
+    }
+    if(y == null){
+      y = "0";
+    }
+    if(z == null){
+      z = "0";
+    }
+    
+    println(x + " " + y + " " + z);
+    
+    /* update all data arrays */
+    switch(graphNum){
+      case 1:
+        curTime1 = (java.lang.System.currentTimeMillis() - startMillis)/1000.0;
+        time1 = pushv(time1, curTime1);
+        x1 = pushv(x1, scale(curGraph, x));
+        y1 = pushv(y1, scale(curGraph, y));
+        z1 = pushv(z1, scale(curGraph, z));        
+        break;
+      case 2:
+        curTime2 = (java.lang.System.currentTimeMillis() - startMillis)/1000.0;
+        time2 = pushv(time2, curTime2);
+        x2 = pushv(x2, scale(curGraph, x));
+        y2 = pushv(y2, scale(curGraph, y));
+        z2 = pushv(z2, scale(curGraph, z));
+        break; 
+      case 3:
+        curTime3 = (java.lang.System.currentTimeMillis() - startMillis)/1000.0;
+        time3 = pushv(time3, curTime3);
+        x3 = pushv(x3, scale(curGraph, x));
+        y3 = pushv(y3, scale(curGraph, y));
+        z3 = pushv(z3, scale(curGraph, z));      
+        break;
+      case 4:
+        curTime4 = (java.lang.System.currentTimeMillis() - startMillis)/1000.0;
+        time4 = pushv(time4, curTime4);
+        x4 = pushv(x4, scale(curGraph, x));
+        y4 = pushv(y4, scale(curGraph, y));
+        z4 = pushv(z4, scale(curGraph, z));      
+        break;     
+    }
+}
+
+/*
+ *  ======== pushv ========
+ *  append new data sample to arr[] and return new array
+ */
+float[] pushv(float [] arr, float val)
+{
+    float [] tmp = arr;
+    
+    if (arr.length < MAX_XVALS) {
+        tmp = append(arr, val);
+    }
+    else {
+        /* shift data within arr to make room for new value */
+        int i;
+        for (i = 1; i < MAX_XVALS; i++) {
+            arr[i - 1] = arr[i];
+        }
+        arr[MAX_XVALS - 1] = val;
+    }
+
+    return (tmp);
+}
+
+/*
+ *  ======== updatePlots ========
+ *  redraw IMU graph
+ */
+void updatePlots()
+{
+    background(255);
+    
+    /* update graph1 */
+    graph1.xMax = max(time1);
+    graph1.xMin = min(time1);
+    graph1.yMax = max(max(max(z1), max(x1), max(y1)), graph1.yMax);
+    graph1.yMin = min(min(min(z1), min(x1), min(y1)), graph1.yMin);
+                                                  
+    graph1.DrawAxis();
+      
+    graph1.GraphColor = color(200, 40, 40);  
+    graph1.LineGraph(time1, x1);      
+      
+    graph1.GraphColor = color(40, 200, 40);   
+    graph1.LineGraph(time1, y1);
+
+    graph1.GraphColor = color(40, 40, 200);   
+    graph1.LineGraph(time1, z1);
+    
+    /* update graph2 */
+    graph2.xMax = max(time2);
+    graph2.xMin = min(time2);
+    graph2.yMax = max(max(max(z2), max(x2), max(y2)), graph2.yMax);
+    graph2.yMin = min(min(min(z2), min(x2), min(y2)), graph2.yMin);
+                                                  
+    graph2.DrawAxis();
+      
+    graph2.GraphColor = color(200, 40, 40);  
+    graph2.LineGraph(time2, x2);      
+      
+    graph2.GraphColor = color(40, 200, 40);   
+    graph2.LineGraph(time2, y2);
+
+    graph2.GraphColor = color(40, 40, 200);   
+    graph2.LineGraph(time2, z2);
+    
+    /* update graph3 */
+    graph3.xMax = max(time3);
+    graph3.xMin = min(time3);
+    graph3.yMax = max(max(max(z3), max(x3), max(y3)), graph3.yMax);
+    graph3.yMin = min(min(min(z3), min(x3), min(y3)), graph3.yMin);
+                                                  
+    graph3.DrawAxis();
+      
+    graph3.GraphColor = color(200, 40, 40);  
+    graph3.LineGraph(time3, x3);      
+      
+    graph3.GraphColor = color(40, 200, 40);   
+    graph3.LineGraph(time3, y3);
+
+    graph3.GraphColor = color(40, 40, 200);   
+    graph3.LineGraph(time3, z3);    
+    
+    /* update graph4 */
+    graph4.xMax = max(time4);
+    graph4.xMin = min(time4);
+    graph4.yMax = max(max(max(z4), max(x4), max(y4)), graph4.yMax);
+    graph4.yMin = min(min(min(z4), min(x4), min(y4)), graph4.yMin);
+                                                  
+    graph4.DrawAxis();
+      
+    graph4.GraphColor = color(200, 40, 40);  
+    graph4.LineGraph(time4, x4);      
+      
+    graph4.GraphColor = color(40, 200, 40);   
+    graph4.LineGraph(time4, y4);
+
+    graph4.GraphColor = color(40, 40, 200);   
+    graph4.LineGraph(time4, z4);
+}
+
+float scale(char type, String value)
+{
+  if(value != null){
+    if (scaleData) {
+      switch (type) {
+          case 'A':
+              return (float(value) * (2 * 9.80665) / float(32768)); /* acc data is +-2G */
+  
+          case 'G':
+              return (float(value) * 250 / float(32768));           /* gyro data is +-250 deg/sec */
+      }
+    }
+    return (float(value));
+  }
+  else{
+    return 0;
+  }
+}
+
+
+/*
+ *  ======== receive ========
+ *  handle UDP receive events
+ */
+ int[] receiveCount = {0, 0, 0, 0}; /* each active zumo had a receiveCount, if that receiveCount reaches zero
+                                       that zumo is deemed inactive is removed from the zumoIPs list */
+ 
+void receive(byte[] data, String ip, int port) {       
+  
+  //println("received IMU data from Zumo at " + ip + " and " + port);
+  
+  /* parse IMU data and display it */
+  String input = new String(data);
+  //println(input);
+
+  for(int i = 0; i < 4; i++){
+    /* don't let the receiveCounts go below 0 */
+    if(receiveCount[i] > 0){
+      receiveCount[i]--;
+    }
+    /* if receiveCount reaches zero, boot 'em out */
+    if(receiveCount[i] == 0){
+      if(zumoIPs[i] != null){
+        println("zumo "+zumoIPs[i]+" inactive");
+        zumoIPs[i] = null;
+      }
+     
+    }
+  }
+  
+  for(int i = 0; i < 4; i++){
+    if(!ip.equals(zumoIPs[i])){
+      /* check to see if that ip is empty */
+      if(zumoIPs[i] == null){
+        zumoIPs[i] = ip;
+        receiveCount[i] += 10;
+        return;
+      }
+    }
+    else{
+      /* update that zumo's data array */
+      switch(i){
+        case 0:
+          data1 = splitTokens(input, " ");
+          break;
+        case 1:  
+          data2 = splitTokens(input, " ");
+          break;
+        case 2:
+          data3 = splitTokens(input, " ");
+          break;
+        case 3:
+          data4 = splitTokens(input, " ");
+          break;
+      }  
+      /* increment that zumo's receiveCount */
+      receiveCount[i] += 10;
+      return; /* exit the method */
+    }
+  }
+  
+  
+}
diff --git a/src/Processing/UDPExample/GraphClass.pde b/src/Processing/UDPExample/GraphClass.pde
new file mode 100644
index 0000000..39f9b7c
--- /dev/null
+++ b/src/Processing/UDPExample/GraphClass.pde
@@ -0,0 +1,371 @@
+/*
+ * from: https://github.com/sebnil/RealtimePlotter
+ *
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2013 Sebatian Nilsson
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/*   =================================================================================       
+     The Graph class contains functions and variables that have been created to draw 
+     graphs. Here is a quick list of functions within the graph class:
+          
+     Graph(int x, int y, int w, int h,color k)
+     DrawAxis()
+     Bar([])
+     smoothLine([][])
+     DotGraph([][])
+     LineGraph([][]) 
+     
+     =================================================================================*/   
+
+    
+class Graph 
+{
+      
+    boolean Dot=true;            // Draw dots at each data point if true
+    boolean RightAxis;            // Draw the next graph using the right axis if true
+    boolean ErrorFlag=false;      // If the time array isn't in ascending order, make true  
+    boolean ShowMouseLines=true;  // Draw lines and give values of the mouse position
+    
+    int     xDiv=10,yDiv=5;            // Number of sub divisions
+    int     xPos,yPos;            // location of the top left corner of the graph  
+    int     Width,Height;         // Width and height of the graph
+    
+
+    color   GraphColor;
+    color   BackgroundColor=color(255);  
+    color   StrokeColor=color(180);     
+      
+    String  Title="Title";          // Default titles
+    String  xLabel="x - Label";
+    String  yLabel="y - Label";
+
+    float   yMax=1024, yMin=0;      // Default axis dimensions
+    float   xMax=10, xMin=0;
+    float   yMaxRight=1024,yMinRight=0;
+  
+    PFont   Font;                   // Selected font used for text 
+      
+    //    int Peakcounter=0,nPeakcounter=0;
+     
+    Graph(int x, int y, int w, int h, color k) {  // The main declaration function
+        xPos = x;
+        yPos = y;
+        Width = w;
+        Height = h;
+        GraphColor = k;
+    }
+     
+    void DrawAxis(){
+       
+        /*  =========================================================================================
+            Main axes Lines, Graph Labels, Graph Background
+            ==========================================================================================  */
+    
+        fill(BackgroundColor); color(0);stroke(StrokeColor);strokeWeight(1);
+        int t=60;
+        
+        rect(xPos-t*1.6,yPos-t,Width+t*2.5,Height+t*2);            // outline
+        textAlign(CENTER);textSize(18);
+        float c=textWidth(Title);
+        fill(BackgroundColor); color(0);stroke(0);strokeWeight(1);
+        rect(xPos+Width/2-c/2,yPos-35,c,0);                         // Heading Rectangle  
+        
+        fill(0);
+        text(Title,xPos+Width/2,yPos-37);                            // Heading Title
+        textAlign(CENTER);textSize(14);
+        text(xLabel,xPos+Width/2,yPos+Height+t/1.5);                     // x-axis Label 
+        
+        rotate(-PI/2);                                               // rotate -90 degrees
+        text(yLabel,-yPos-Height/2,xPos-t*1.6+20);                   // y-axis Label  
+        rotate(PI/2);                                                // rotate back
+        
+        textSize(10); noFill(); stroke(0); smooth();strokeWeight(1);
+        //Edges
+        line(xPos-3,yPos+Height,xPos-3,yPos);                        // y-axis line 
+        line(xPos-3,yPos+Height,xPos+Width+5,yPos+Height);           // x-axis line 
+          
+        stroke(200);
+        if(yMin<0){
+            line(xPos-7,                                       // zero line 
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height,   // 
+                 xPos+Width,
+                 yPos+Height-(abs(yMin)/(yMax-yMin))*Height
+                 );
+        }
+          
+        if(RightAxis){                                       // Right-axis line   
+            stroke(0);
+            line(xPos+Width+3,yPos+Height,xPos+Width+3,yPos);
+        }
+            
+        /*  =========================================================================================
+            Sub-devisions for both axes, left and right
+            ==========================================================================================  */
+            
+        stroke(0);
+            
+        for(int x=0; x<=xDiv; x++){
+       
+            /*  =========================================================================================
+                x-axis
+                ==========================================================================================  */
+             
+            line(float(x)/xDiv*Width+xPos-3,yPos+Height,       //  x-axis Sub devisions    
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+5);     
+                 
+            textSize(10);                                      // x-axis Labels
+            String xAxis=str(xMin+float(x)/xDiv*(xMax-xMin));  // the only way to get a specific number of decimals 
+            String[] xAxisMS=split(xAxis,'.');                 // is to split the float into strings 
+            text(xAxisMS[0]+"."+xAxisMS[1].charAt(0),          // ...
+                 float(x)/xDiv*Width+xPos-3,yPos+Height+15);   // x-axis Labels
+        }
+          
+          
+        /*  =========================================================================================
+            left y-axis
+            ==========================================================================================  */
+          
+        for(int y=0; y<=yDiv; y++){
+            line(xPos-3,float(y)/yDiv*Height+yPos,                // ...
+                 xPos-7,float(y)/yDiv*Height+yPos);              // y-axis lines 
+            
+            textAlign(RIGHT);fill(20);
+            
+            String yAxis=str(yMin+float(y)/yDiv*(yMax-yMin));     // Make y Label a string
+            String[] yAxisMS=split(yAxis,'.');                    // Split string
+           
+            text(yAxisMS[0]+"."+yAxisMS[1].charAt(0),             // ... 
+                 xPos-15,float(yDiv-y)/yDiv*Height+yPos+3);       // y-axis Labels 
+                        
+                        
+            /*  =========================================================================================
+                right y-axis
+                ==========================================================================================  */
+            
+            if(RightAxis){
+             
+                color(GraphColor); stroke(GraphColor);fill(20);
+            
+                line(xPos+Width+3,float(y)/yDiv*Height+yPos,             // ...
+                     xPos+Width+7,float(y)/yDiv*Height+yPos);            // Right Y axis sub devisions
+                   
+                textAlign(LEFT); 
+            
+                String yAxisRight=str(yMinRight+float(y)/                // ...
+                                      yDiv*(yMaxRight-yMinRight));           // convert axis values into string
+                String[] yAxisRightMS=split(yAxisRight,'.');             // 
+           
+                text(yAxisRightMS[0]+"."+yAxisRightMS[1].charAt(0),     // Right Y axis text
+                     xPos+Width+15,float(yDiv-y)/yDiv*Height+yPos+3);   // it's x,y location
+            
+                noFill();
+            }stroke(0);
+        }
+    }
+      
+    /*  =========================================================================================
+        Bar graph
+        ==========================================================================================  */   
+      
+    void Bar(float[] a, int from, int to) {
+        
+        stroke(GraphColor);
+        fill(GraphColor);
+          
+        if(from<0){                                      // If the From or To value is out of bounds 
+            for (int x=0; x Make sure time array doesn't decrease (go back in time) 
+               ===========================================================================*/
+            if(ix[i+1]){
+                   
+                    ErrorFlag=true;
+                
+                }
+            }
+         
+            /* =================================================================================       
+               First and last bits can't be part of the curve, no points before first bit, 
+               none after last bit. So a streight line is drawn instead   
+               ================================================================================= */ 
+
+            if(i==0 || i==x.length-2)line(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                                          xPos+(x[i+1]-x[0])/(x[x.length-1]-x[0])*Width,
+                                          yPos+Height-(y[i+1]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                                            
+            /* =================================================================================       
+               For the rest of the array a curve (spline curve) can be created making the graph 
+               smooth.     
+               ================================================================================= */ 
+                            
+            curveVertex( xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                         yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height);
+                           
+            /* =================================================================================       
+               If the Dot option is true, Place a dot at each data point.  
+               ================================================================================= */    
+           
+            if(Dot)ellipse(
+                xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width,
+                yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height,
+                2,2
+                           );
+                             
+            /* =================================================================================       
+               Highlights points closest to Mouse X position   
+               =================================================================================*/ 
+                          
+            if( abs(mouseX-(xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width))<5 ){
+                
+                 
+                float yLinePosition = yPos+Height-(y[i]/(yMax-yMin)*Height)+(yMin)/(yMax-yMin)*Height;
+                float xLinePosition = xPos+(x[i]-x[0])/(x[x.length-1]-x[0])*Width;
+                strokeWeight(1);stroke(240);
+                // line(xPos,yLinePosition,xPos+Width,yLinePosition);
+                strokeWeight(2);stroke(GraphColor);
+                  
+                ellipse(xLinePosition,yLinePosition,4,4);
+            }
+        }  
+       
+        endShape(); 
+        yMax=tempyMax; yMin=tempyMin;
+        float xAxisTitleWidth=textWidth(str(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1])));
+           
+        if((mouseX>xPos&mouseX<(xPos+Width))&(mouseY>yPos&mouseY<(yPos+Height))){   
+            if(ShowMouseLines){
+                // if(mouseXxPos+Width)xlocation=xPos+Width;
+                else xlocation=mouseX;
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+                // Rectangle and x position
+                line(xlocation,yPos,xlocation,yPos+Height);
+                rect(xlocation-xAxisTitleWidth/2-10,yPos+Height-16,xAxisTitleWidth+20,12);
+            
+                textAlign(CENTER); fill(160);
+                text(map(xlocation,xPos,xPos+Width,x[0],x[x.length-1]),xlocation,yPos+Height-6);
+            
+                // if(mouseYyPos+Height)ylocation=yPos+Height;
+                else ylocation=mouseY;
+          
+                // Rectangle and y position
+                stroke(200); strokeWeight(0.5);fill(255);color(50);
+            
+                line(xPos,ylocation,xPos+Width,ylocation);
+                int yAxisTitleWidth=int(textWidth(str(map(ylocation,yPos,yPos+Height,y[0],y[y.length-1]))) );
+                rect(xPos-15+3,ylocation-6, -60 ,12);
+            
+                textAlign(RIGHT); fill(GraphColor);//StrokeColor
+                //    text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos+Width+3,yPos+Height+4);
+                text(map(ylocation,yPos+Height,yPos,yMin,yMax),xPos -15,ylocation+4);
+                if(RightAxis){ 
+                          
+                    stroke(200); strokeWeight(0.5);fill(255);color(50);
+                           
+                    rect(xPos+Width+15-3,ylocation-6, 60 ,12);  
+                    textAlign(LEFT); fill(160);
+                    text(map(ylocation,yPos+Height,yPos,yMinRight,yMaxRight),xPos+Width+15,ylocation+4);
+                }
+                noStroke();noFill();
+            }
+        }
+    }
+
+    void smoothLine(float[] x ,float[] y, float[] z, float[] a ) {
+        GraphColor=color(188,53,53);
+        smoothLine(x ,y);
+        GraphColor=color(193-100,216-100,16);
+        smoothLine(z ,a);
+    }
+}
+
diff --git a/src/Processing/UDPExample/UDPExample.pde b/src/Processing/UDPExample/UDPExample.pde
new file mode 100644
index 0000000..7c58d1d
--- /dev/null
+++ b/src/Processing/UDPExample/UDPExample.pde
@@ -0,0 +1,277 @@
+/*
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * *  Neither the name of Texas Instruments Incorporated nor the names of
+ *    its contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * RedBear Zumo Bot interactive control with real-time acceleration display
+ * 
+ * Before running this program, start the zumo bot and connect to its
+ * WiFi network.
+ */
+
+import processing.net.Client;
+import hypermedia.net.*;
+
+UDP udp;  /* define the UDP object */    
+
+/* IMU data from zumo */
+char[] imuBuffer = new char[72];
+
+String[] tokens = new String[12];
+
+char  cmd = ' ';  /* current command */
+boolean cont = false;  /* continuously send previous command to get IMU data */
+
+/* zumo IP Address and destination port */
+String IP = "192.168.1.1";
+int PORT = 8080;
+
+int    MAX_FPS = 30;     /* max frames/sec, i.e., rate of calls to draw() */
+int    MAX_XVALS = 80;   /* max # of samples in the x-axis */
+int    MAX_PENDING = 6;  /* max # of outstanding commands to send */
+
+String LOG_NAME  = "zumo_log.txt";
+
+/* IMU data graph object */
+Graph graph1 = new Graph(150, 80, 400, 200, color (200, 20, 20));
+
+/* IMU data values to graph */
+float[] time = { 0 };       
+float[] ax   = { 0 }; 
+float[] ay   = { 0 };
+float[] az   = { 0 };
+
+char curGraph = 'A';
+int curGraphOffset = 1;
+float curTime = 0;
+boolean scaleData = true;
+
+PrintWriter log = null;  /* optional data log */
+
+/*
+ *  ======== setup ========
+ */
+void setup() 
+{
+    /* create minimal text output window for acc data */
+    size(650, 350); 
+   
+    graph1.xLabel = " Time (s)";
+    graph1.yLabel = "Acceleration (m/s^2)";
+    graph1.Title  = " Acceleration: (x, y, z) vs. t ";  
+    graph1.yMin   = 0;
+    graph1.yMax   = 0;
+
+    /* slow the draw() rate down to MAX_FPS frames/sec */
+    frameRate(MAX_FPS);
+        
+    /* create a new datagram connection on port 8080 */
+    udp = new UDP(this, 8080);  
+    udp.listen(true);
+    
+    println(udp.address());
+}
+
+/*
+ *  ======== draw ========
+ */
+int loopCount = 0;
+
+void draw() 
+{
+    /* send server commands based on keyboard input */
+    if (cont) {
+        
+        /* if no key is currently pressed, stop the zumo */
+        if(!keyPressed){
+          cmd = ' ';
+        }
+        
+        /* send the current command every 4th iteration */
+        if (loopCount % 4 == 0) {
+            /* send the command to the zumo at 192.168.1.1 */
+            udp.send(String.valueOf(cmd), IP, PORT);
+        }
+        
+        /* graph the incoming IMU data */
+        if (tokens.length > (2 + curGraphOffset)) {
+            newPoint(tokens[0 + curGraphOffset],  /* x value */
+                     tokens[1 + curGraphOffset],  /* y value */
+                     tokens[2 + curGraphOffset]); /* z value */
+        }  
+
+        loopCount++;
+    }
+
+}
+
+void keyPressed()
+{
+    if  (key == 'w' || key == 'a' || key == 's' || key == 'd') {
+        cmd = key;
+    }
+    else if (key == 'x') cont = false;
+    else if (key == 'c') cont = true;
+    else if (key == 'L') {
+        if (log == null) {
+            log = createWriter(LOG_NAME);
+            println("logging started ...");
+        }
+    }
+    else if (key == 'l') {
+        if (log != null) {
+            log.flush();
+            log.close();
+            log = null;
+            println("logging stopped.");
+        }
+    }
+    else if (key == 'G') {
+        if (curGraph != 'G') {
+            curGraph = 'G';
+            curGraphOffset = 5;
+            graph1.yLabel = "Rotational Speed (deg/s)";
+            graph1.Title = " Gyro: (x, y, z) vs. t ";  
+            graph1.yMin = 0;
+            graph1.yMax = 0;
+            ax = new float [] { 0 }; 
+            ay = new float [] { 0 };
+            az = new float [] { 0 };
+            time = new float [] { curTime }; 
+        }
+    }
+    else if (key == 'A') {
+        if (curGraph != 'A') {
+            curGraph = 'A';
+            curGraphOffset = 1;
+            graph1.yLabel = "Acceleration (m/s^2)";
+            graph1.Title = " Acceleration: (x, y, z) vs. t ";  
+            graph1.yMin = 0;
+            graph1.yMax = 0;
+            ax = new float [] { 0 }; 
+            ay = new float [] { 0 };
+            az = new float [] { 0 };
+            time = new float [] { curTime }; 
+        }
+    }  
+}
+
+/*
+ *  ======== newPoint ========
+ *  add new point to data arrays and update graph
+ */
+void newPoint(String x, String y, String z)
+{
+    /* get next values */
+    curTime += 1;
+    if (log != null) {
+        log.println("A: " + x + " " + y + " " + z);
+    }
+    
+    /* update all data arrays */
+    time = pushv(time, curTime);
+    ax = pushv(ax, scale(curGraph, x));
+    ay = pushv(ay, scale(curGraph, y));
+    az = pushv(az, scale(curGraph, z));
+
+    /* update display */
+    updatePlots();
+}
+
+/*
+ *  ======== pushv ========
+ *  append new data sample to arr[] and return new array
+ */
+float[] pushv(float [] arr, float val)
+{
+    float [] tmp = arr;
+    
+    if (arr.length < MAX_XVALS) {
+        tmp = append(arr, val);
+    }
+    else {
+        /* shift data within arr to make room for new value */
+        int i;
+        for (i = 1; i < MAX_XVALS; i++) {
+            arr[i - 1] = arr[i];
+        }
+        arr[MAX_XVALS - 1] = val;
+    }
+
+    return (tmp);
+}
+
+/*
+ *  ======== updatePlots ========
+ *  redraw IMU graph
+ */
+void updatePlots()
+{
+    background(255);
+
+    graph1.xMax = max(time);
+    graph1.xMin = min(time);
+    graph1.yMax = max(max(max(az), max(ax), max(ay)), graph1.yMax);
+    graph1.yMin = min(min(min(az), min(ax), min(ay)), graph1.yMin);
+                                                  
+    graph1.DrawAxis();
+      
+    graph1.GraphColor = color(200, 40, 40);  
+    graph1.LineGraph(time, ax);      
+      
+    graph1.GraphColor = color(40, 200, 40);   
+    graph1.LineGraph(time, ay);
+
+    graph1.GraphColor = color(40, 40, 200);   
+    graph1.LineGraph(time, az);
+}
+
+float scale(char type, String value)
+{
+  if (scaleData) {
+    switch (type) {
+        case 'A':
+            return (float(value) * (2 * 9.80665) / float(32768)); /* acc data is +-2G */
+
+        case 'G':
+            return (float(value) * 250 / float(32768));           /* gyro data is +-250 deg/sec */
+    }
+  }
+    return (float(value));
+}
+
+void receive(byte[] data, String ip, int port) {  
+       
+  /* parse IMU data */
+  String input = new String(imuBuffer);
+  tokens = splitTokens(input, " ");
+  
+}
-- 
2.39.2