summary | shortlog | log | commit | commitdiff | tree
raw | patch | inline | side by side (parent: d8d507d)
raw | patch | inline | side by side (parent: d8d507d)
author | dave russo <d-russo@ti.com> | |
Mon, 28 Sep 2015 03:46:12 +0000 (20:46 -0700) | ||
committer | dave russo <d-russo@ti.com> | |
Mon, 28 Sep 2015 03:46:12 +0000 (20:46 -0700) |
96 files changed:
diff --git a/.gitignore b/.gitignore
--- /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
--- /dev/null
+++ b/README.html
@@ -0,0 +1,76 @@
+<!DOCTYPE html>
+<html xmlns="http://www.w3.org/1999/xhtml">
+ <head>
+ <title>Zumo CC3200 README.md</title>
+ <link rel="stylesheet" type="text/css" href="etc/github-markdown.css">
+ <style>
+ .markdown-body {
+ min-width: 200px;
+ max-width: 790px;
+ margin: 0 auto;
+ padding: 30px;
+ }
+ </style>
+ </head>
+ <body ><article class="markdown-body">
+<p><a href="https://git.ti.com/zumo-cc3200/zumo-cc3200/archive-tarball/master"><img src="etc/build.svg" alt="download" title="" /></a></p>
+
+<h1><a href="http://processors.wiki.ti.com/index.php/ZumoCC3200#The_Zumo_CC3200" title="ZumoCC3200 Home Page">Zumo CC3200</a></h1>
+
+<p>The Zumo CC3200 project was created to enable makers to easily create
+applications that leverage the power of a <a href="http://www.ti.com/product/cc3200">TI CC3200</a> connected to a
+motorized hardware platform equipped with Inertial Measurement (IMU)
+sensors.</p>
+
+<p>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.</p>
+
+<p>Each of the ZumoCC3200 examples consists of two programs: an Energia program
+that runs on the Zumo CC3200 robot and a <a href="https:/processing.org" title="Processing Home Page">Processing</a>
+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.</p>
+
+<h2><a href="https://gitorious.design.ti.com/sb/zumo/trees/master/src/Energia/libraries" title="ZumoCC3200 Energia Library Sources">The ZumoCC3200 Library</a></h2>
+
+<p>To use the ZumoCC3200 library, </p>
+
+<ul>
+<li><a href="https://git.ti.com/zumo-cc3200/zumo-cc3200/archive-tarball/master" title="ZumoCC3200 Repo Download">Download the ZumoCC3200 library</a>, </li>
+<li><a href="http://processors.wiki.ti.com/index.php/ZumoCC3200Demos#Install_the_Energia_ZumoCC3200_Library" title="ZumoCC3200 Energia Library Install Instructions">Install the ZumoCC3200 library</a> into your Energia
+sketchbook, and</li>
+<li><a href="http://processors.wiki.ti.com/index.php/ZumoCC3200Demos#Build_and_Upload_a_Zumo_CC3200_Example" title="ZumoCC3200 Energia Build Instructions">Build and run an example</a></li>
+</ul>
+
+<p>Like most Energia/Arduino libraries, the ZumoCC3200 library includes serveral
+examples that serve as simple starting points for your project.</p>
+
+<h2><a href="https://gitorious.design.ti.com/sb/zumo/trees/master/src/Processing" title="ZumoCC3200 Processing Sketch Sources">Processing Sketches</a></h2>
+
+<p>The Processing sketches that communicate with the ZumoCC3200 bot are also
+<a href="https://gitorious.design.ti.com/sb/zumo/trees/master/src/Processing" title="ZumoCC3200 Processing Sketch Sources">included in this repo</a>. Note that some of these sketches
+rely on freely available Contributed libraries that must be installed. </p>
+
+<p>Prerequisite libraries and installation instructions for these sketches is
+<a href="http://processors.wiki.ti.com/index.php/ZumoCC3200Demos#Install_Processing_Libraries" title="Processing Library Install Instructions">here</a>.</p>
+
+<h2><a href="license.html">License</a></h2>
+
+<p>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. </p>
+
+<p>Complete license details are provided <a href="license.html">here</a>.</p>
+
+<h2>Contributing</h2>
+
+<p><a href="http://processors.wiki.ti.com/index.php/ZumoCC3200#The_Zumo_CC3200" title="ZumoCC3200 Home Page">Zumo CC3200</a> is a collaborative project originally created as a part of the TI Santa Barbara 2015 Summer Intern program by <a href="http://processors.wiki.ti.com/index.php/ZumoCC3200#Credits" title="Interns">Adam Dai and Tony Oliverio</a>, and you are invited to help. </p>
+
+<p>In-depth details about contributing code, bug fixes, and documentation are
+forthcoming. </p>
+ </article></body>
+</html>
diff --git a/etc/Markdown.pl b/etc/Markdown.pl
--- /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
+# <http://daringfireball.net/projects/markdown/>
+#
+
+
+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 <a>
+# and <img> 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]*
+ <?(\S+?)>? # 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 <p>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.:
+ # <div>
+ # <div>
+ # tags for inner block must be indented.
+ # </div>
+ # </div>
+ #
+ # 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 `<div>` and stop at the first `</div>`.
+ $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
+ </\2> # 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<tag>` to `</tag>\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
+ .*</\2> # 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 <hr />. 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:
+ <!
+ (--.*?--\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<hr$g_empty_element_suffix\n}gmx;
+ $text =~ s{^[ ]{0,2}([ ]? -[ ]?){3,}[ \t]*$}{\n<hr$g_empty_element_suffix\n}gmx;
+ $text =~ s{^[ ]{0,2}([ ]? _[ ]?){3,}[ \t]*$}{\n<hr$g_empty_element_suffix\n}gmx;
+
+ $text = _DoLists($text);
+
+ $text = _DoCodeBlocks($text);
+
+ $text = _DoBlockQuotes($text);
+
+ # We already ran _HashHTMLBlocks() before, in Markdown(), but that
+ # was to escape raw HTML in the original Markdown source. This time,
+ # we're escaping the markup we've just created, so that we don't wrap
+ # <p> 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 `<http://example.com/>`
+ # Must come after _DoAnchors(), because you can use < and >
+ # delimiters in inline links like [this](<url>).
+ $text = _DoAutoLinks($text);
+
+ $text = _EncodeAmpsAndAngles($text);
+
+ $text = _DoItalicsAndBold($text);
+
+ # Do hard breaks:
+ $text =~ s/ {2,}\n/ <br$g_empty_element_suffix\n/g;
+
+ return $text;
+}
+
+
+sub _EscapeSpecialChars {
+ my $text = shift;
+ my $tokens ||= _TokenizeHTML($text);
+
+ $text = ''; # rebuild $text from the tokens
+# my $in_pre = 0; # Keep track of when we're inside <pre> or <code> 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 <a> 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 = "<a href=\"$url\"";
+ if ( defined $g_titles{$link_id} ) {
+ my $title = $g_titles{$link_id};
+ $title =~ s! \* !$g_escape_table{'*'}!gx;
+ $title =~ s! _ !$g_escape_table{'_'}!gx;
+ $result .= " title=\"$title\"";
+ }
+ $result .= ">$link_text</a>";
+ }
+ else {
+ $result = $whole_match;
+ }
+ $result;
+ }xsge;
+
+ #
+ # Next, inline-style links: [link text](url "optional title")
+ #
+ $text =~ s{
+ ( # wrap whole match in $1
+ \[
+ ($g_nested_brackets) # link text = $2
+ \]
+ \( # literal paren
+ [ \t]*
+ <?(.*?)>? # 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 = "<a href=\"$url\"";
+
+ if (defined $title) {
+ $title =~ s/"/"/g;
+ $title =~ s! \* !$g_escape_table{'*'}!gx;
+ $title =~ s! _ !$g_escape_table{'_'}!gx;
+ $result .= " title=\"$title\"";
+ }
+
+ $result .= ">$link_text</a>";
+
+ $result;
+ }xsge;
+
+ return $text;
+}
+
+
+sub _DoImages {
+#
+# Turn Markdown image shortcuts into <img> 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 = "<img src=\"$url\" alt=\"$alt_text\"";
+ if (defined $g_titles{$link_id}) {
+ my $title = $g_titles{$link_id};
+ $title =~ s! \* !$g_escape_table{'*'}!gx;
+ $title =~ s! _ !$g_escape_table{'_'}!gx;
+ $result .= " title=\"$title\"";
+ }
+ $result .= $g_empty_element_suffix;
+ }
+ else {
+ # If there's no such link ID, leave intact:
+ $result = $whole_match;
+ }
+
+ $result;
+ }xsge;
+
+ #
+ # Next, handle inline images: ![alt text](url "optional title")
+ # Don't forget: encode * and _
+
+ $text =~ s{
+ ( # wrap whole match in $1
+ !\[
+ (.*?) # alt text = $2
+ \]
+ \( # literal paren
+ [ \t]*
+ <?(\S+?)>? # 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 = "<img src=\"$url\" alt=\"$alt_text\"";
+ if (defined $title) {
+ $title =~ s! \* !$g_escape_table{'*'}!gx;
+ $title =~ s! _ !$g_escape_table{'_'}!gx;
+ $result .= " title=\"$title\"";
+ }
+ $result .= $g_empty_element_suffix;
+
+ $result;
+ }xsge;
+
+ return $text;
+}
+
+
+sub _DoHeaders {
+ my $text = shift;
+
+ # Setext-style headers:
+ # Header 1
+ # ========
+ #
+ # Header 2
+ # --------
+ #
+ $text =~ s{ ^(.+)[ \t]*\n=+[ \t]*\n+ }{
+ "<h1>" . _RunSpanGamut($1) . "</h1>\n\n";
+ }egmx;
+
+ $text =~ s{ ^(.+)[ \t]*\n-+[ \t]*\n+ }{
+ "<h2>" . _RunSpanGamut($1) . "</h2>\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);
+ "<h$h_level>" . _RunSpanGamut($2) . "</h$h_level>\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 . "</$list_type>\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 . "</$list_type>\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);
+ }
+
+ "<li>" . $item . "</li>\n";
+ }egmx;
+
+ $g_list_level--;
+ return $list_str;
+}
+
+
+
+sub _DoCodeBlocks {
+#
+# Process Markdown `<pre><code>` 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<pre><code>" . $codeblock . "\n</code></pre>\n\n";
+
+ $result;
+ }egmx;
+
+ return $text;
+}
+
+
+sub _DoCodeSpans {
+#
+# * Backtick quotes are used for <code></code> 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:
+#
+# <p>Just type <code>foo `bar` baz</code> at the prompt.</p>
+#
+# 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 <code>`bar`</code> ...
+#
+
+ my $text = shift;
+
+ $text =~ s@
+ (`+) # $1 = Opening run of `
+ (.+?) # $2 = The code block
+ (?<!`)
+ \1 # Matching closer
+ (?!`)
+ @
+ my $c = "$2";
+ $c =~ s/^[ \t]*//g; # leading whitespace
+ $c =~ s/[ \t]*$//g; # trailing whitespace
+ $c = _EncodeCode($c);
+ "<code>$c</code>";
+ @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;
+
+ # <strong> must go first:
+ $text =~ s{ (\*\*|__) (?=\S) (.+?[*_]*) (?<=\S) \1 }
+ {<strong>$2</strong>}gsx;
+
+ $text =~ s{ (\*|_) (?=\S) (.+?) (?<=\S) \1 }
+ {<em>$2</em>}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 <pre> content, so we need to fix that:
+ $bq =~ s{
+ (\s*<pre>.+?</pre>)
+ }{
+ my $pre = $1;
+ $pre =~ s/^ //mg;
+ $pre;
+ }egsx;
+
+ "<blockquote>\n$bq\n</blockquote>\n\n";
+ }egmx;
+
+
+ return $text;
+}
+
+
+sub _FormParagraphs {
+#
+# Params:
+# $text - string to process with html <p> tags
+#
+ my $text = shift;
+
+ # Strip leading and trailing lines:
+ $text =~ s/\A\n+//;
+ $text =~ s/\n+\z//;
+
+ my @grafs = split(/\n{2,}/, $text);
+
+ #
+ # Wrap <p> tags.
+ #
+ foreach (@grafs) {
+ unless (defined( $g_html_blocks{$_} )) {
+ $_ = _RunSpanGamut($_);
+ s/^([ \t]*)/<p>/;
+ $_ .= "</p>";
+ }
+ }
+
+ #
+ # 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]+)>}{<a href="$1">$1</a>}gi;
+
+ # Email addresses: <address@domain.foo>
+ $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.:
+#
+# <a href="mailto:foo@e
+# xample.com">foo
+# @example.com</a>
+#
+# Based on a filter by Matthew Wickline, posted to the BBEdit-Talk
+# mailing list: <http://tinyurl.com/yu7ue>
+#
+
+ 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{<a href="$addr">$addr</a>};
+ $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 <a href="<MTFoo>">, 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.
+# <http://www.bradchoate.com/past/mtregex.php>
+#
+
+ 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: <! ( -- .*? -- \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:
+# <http://www.nntp.perl.org/group/perl.macperl.anyperl/154>
+#
+ 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<Markdown>
+
+
+=head1 SYNOPSIS
+
+B<Markdown.pl> [ B<--html4tags> ] [ B<--version> ] [ B<-shortversion> ]
+ [ I<file> ... ]
+
+
+=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 <div> and <table> 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.:
+
+ <br>
+
+instead of Markdown's default XHTML style tags, e.g.:
+
+ <br />
+
+
+=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
+<http://daringfireball.net/>
+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
--- /dev/null
+++ b/etc/build.svg
@@ -0,0 +1,34 @@
+<svg contentScriptType="text/ecmascript" width="133"
+ xmlns:xlink="http://www.w3.org/1999/xlink" zoomAndPan="magnify"
+ contentStyleType="text/css" height="20" preserveAspectRatio="xMidYMid meet"
+ xmlns="http://www.w3.org/2000/svg" version="1.0">
+
+
+ <linearGradient xmlns:xlink="http://www.w3.org/1999/xlink" x2="0" y2="100%"
+ xlink:type="simple" xlink:actuate="onLoad" id="a"
+ xlink:show="other">
+ <stop stop-opacity=".1" stop-color="#bbb" offset="0"/>
+ <stop stop-opacity=".1" offset="1"/>
+ </linearGradient>
+
+ <rect rx="3" fill="#555" width="80" height="20" class="sWidth"/>
+ <rect rx="3" fill="url(#a)" width="80" height="20" class="sWidth"/>
+ <rect rx="3" fill="#4c1" width="56" x="72" height="20" class="vWidth tMove"/>
+ <rect fill="#4c1" width="13" x="72" height="20" class="tMove"/>
+
+ <g font-size="11" font-family="DejaVu Sans,Verdana,Geneva,sans-serif"
+ fill="#fff">
+ <text x="6" fill="#010101" fill-opacity=".3" y="15">
+ Download
+ </text>
+ <text x="6" id="tText" y="14">
+ Download
+ </text>
+ <text fill="#010101" x="78" fill-opacity=".3" y="15" class="tMove">
+ 1.0.0.0
+ </text>
+ <text x="78" id="vText" y="14" class="tMove">
+ 1.0.0.0
+ </text>
+ </g>
+</svg>
diff --git a/etc/github-markdown.css b/etc/github-markdown.css
--- /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
--- /dev/null
+++ b/etc/md2html.ksh
@@ -0,0 +1,32 @@
+#!/bin/ksh
+#
+# Convert a markdown file to an html file
+#
+# usage: ms2html <markdown_file>
+#
+# 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 '<!DOCTYPE html>'
+echo '<html xmlns="http://www.w3.org/1999/xhtml">'
+echo ' <head>'
+echo " <title>Zumo CC3200 `basename $1`</title>"
+echo ' <link rel="stylesheet" type="text/css" href="etc/github-markdown.css">'
+echo ' <style>'
+echo ' .markdown-body {'
+echo ' min-width: 200px;'
+echo ' max-width: 790px;'
+echo ' margin: 0 auto;'
+echo ' padding: 30px;'
+echo ' }'
+echo ' </style>'
+echo ' </head>'
+echo ' <body ><article class="markdown-body">'
+perl etc/Markdown.pl $1
+echo ' </article></body>'
+echo '</html>'
+
diff --git a/license.html b/license.html
--- /dev/null
+++ b/license.html
@@ -0,0 +1,473 @@
+<!--
+Texas Instruments Manifest Format 2.0
+-->
+
+<!DOCTYPE html PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html>
+
+<head>
+<meta http-equiv="Content-Type" content="text/html; charset=ISO-8859-1" />
+<!-- @Start Style -->
+<!-- Default style in case someone doesnt have Internet Access -->
+<style type="text/css" id="internalStyle">
+ body, div, p {
+ font-family: Lucida Grande, Verdana, Geneva, Arial, sans-serif;
+ font-size: 13px;
+ line-height: 1.3;
+ }
+ body {
+ margin: 20px;
+ }
+ h1 {
+ font-size: 150%;
+ }
+ h2 {
+ font-size: 120%;
+ }
+ h3 {
+ font-size: 100%;
+ }
+ img {
+ border: 0px;
+ vertical-align: middle;
+ }
+ table, th, td, tr {
+ border: 1px solid black;
+ font-family: Lucida Grande, Verdana, Geneva, Arial, sans-serif;
+ font-size: 13px;
+ line-height: 1.3;
+ empty-cells: show;
+ padding: 5px;
+ }
+ table {
+ border-collapse: collapse;
+ width: 100%;
+ }
+ tr {
+ page-break-inside: avoid;
+ }
+ #TIlogoLeft {
+ background-color: black;
+ padding: 0;
+ width: 20%;
+ }
+ #TIlogoRight {
+ background-color: red;
+ padding: 0;
+ }
+ #ProductName {
+ text-align: center;
+ }
+ #ReleaseDate {
+ text-align: center;
+ }
+ .LogoSection {
+ margin: 0;
+ padding: 0;
+ }
+ .HeaderSection {
+ margin: 25px 0 25px 0;
+ padding: 0;
+ }
+ .LegendSection {
+ margin: 25px 0 25px 0;
+ }
+ .ExportSection {
+ margin: 25px 0 25px 0;
+ }
+ .DisclaimerSection {
+ margin: 25px 0 25px 0;
+ }
+ .CreditSection {
+ margin: 25px 0 25px 0;
+ }
+ .LicenseSection {
+ margin: 25px 0 25px 0;
+ }
+ .ManifestTable {
+ margin: 25px 0 25px 0;
+ }
+</style>
+<!-- Override style from TI if they have Internet Access -->
+<link type="text/css" rel="stylesheet" href="timanifeststyle.css">
+<!-- @End Style -->
+<title>Texas Instruments Manifest</title>
+</head>
+
+<body><!-- Logo display, will need to fix up the URLs, this is just for testing.. Image alternate display not wporking well yet -->
+<div class="LogoSection">
+<table>
+ <tbody>
+ <tr>
+ <td id="TIlogoLeft">
+ <a href="http://www.ti.com/">
+ <!-- img src="tilogo.gif" alt="Texas Instruments Incorporated" -->
+ <img alt="" src="data:image/gif;base64,R0lGODlh3gA2AKIAAAAAAP///7u7u29vbz8/PwYGBujo6BgYGCH5BAAAAAAALAAAAADeADYAAAP/CLrc/jDKSau9OOvNu/9gKI5kaZ5oqq5s675wLM90bd94ru987//AoHBILBqPyKRyyWw6n9CodHorDALYLIHKJVqz2q44eAUHtoDB4DBu48rgLQErcNtnX7NhMDcICIB3gix5ZmtqAAZZew8EAo+QkQIDNVZqiIM1cHGKZ4YPAmaiAWw0c1gFmZqjB3SbZ6kNe6WhsAeOlDV0qjSFAXUAp7lwuREFtVsFgMvLB7fNAM+BCs+lDLd8BNYOuxfV22PL0RiWlwO1u3kDqejAEsjR6GB86FsHoYwA6gxWnVgGEegUuIelWJk6jswAGlXQ36J1xBSoQwfulIEDr/6l+VeK/+AehrAGOHRnAWRBbbWegckXAV6wk4AeRQtDQBEaBYsYlMl2hUCsBt0iKgilT9EfAlfO7SmzdKkrkQUT/fqZSECqLCSlntH375IAA1tqGUilLIBSNVnU+NmJNBRVChlF1QwAdlRWBy5P3QymwCLBYhs73cTHYBq3X33nDQ2wcWuBgef0FRD4GK3jU3VCZZUJAIw1OGg0P+4bFiubOWoOsEP1+KvZn3wurDbZ6lfcuw3yYkFjRSeYzRe7ARAbW0K3PmGIMi0OFDG1Mmha+RnufAHn3xL9ha6uTZ/rXagZ1GKAtTsHeWb+FEQvHILuX4+mLzj2j2r4TrFesTwMbE5Cuv8JzbTSGuRV1xgfUJFC3WbA0JWFalcItpgf8YU2yT/qATaedent5cBb8zk0DzIitgfKbonRFV9Wp2xl3UXq5Ccibp05598BnRigiAIJmrZAexkJQIuBwzX4CB3SQbeYQkPVAUco63DI2HzsAdYAiAvEZdYlaVQ5wXs3+bQAjovEUoBRR9LVAFLaPXCcY/KMqVRasQB5kiJgLcYgTkJiuCWKC2ZpIY/z/LRhYefkBAGW1HTyRy2UjObLHxSAOZ948EUVGCSC3SLZbB7iZKOLc2GRRgMH/VhdHnJwFCgD8iEGx0VKvpqbO+hoaCppEg3UiTES1CTkhNaQ+Qs4LQGql07/lET4mIQ6SvTSVGZ9Bmhz/bkYzK+PFKtpje6wumRm1wrLZzSdQASoZvyswdmSuk7p616HfkjBTxZBQucFgqXCFKdn1NpiUlQJhs8kteBWG0AbATbXS2tBlaeoVkmJRova4KkGPmhMFdiSYmq8cbTRYhrlkiHaNufJ9mIgVqEXnAOJM5JE4sgjudQ8bF82x+cKBP4Iiedecyjgx2/WtMNjjhcL9h+S4xq9RYJgsbeeUbmdrPTSQbPccsyijEXOfI8xyuinVJH1wdkS/MQ2Bc5Iq08DyHYwGglvPyCilbz0fa8GLV7r9+Btb7CJ14Qnzg8HpdKoOOF5Py752JNXvrblNphzEHnmnF/a+ecTbA465qKPXnnppkuOeuqKr8465K+z7nrsfc9Ouyq23z5I7rrfwXvvbhSQAAA7" />
+ </a>
+ </td>
+ <td id="TILogoRight">
+ <!-- img src="titagline.gif" alt="Technology for Innovators(tm)"-->
+ <img alt="" src="data:image/gif;base64,R0lGODlhOgEaALMAAP8AAP////92dv+3t/+Njf/W1v/t7f8hIf/19f+jo//Hx/8/P/9cXP/j4//6+v/+/iH5BAAAAAAALAAAAAA6ARoAAAT/EMhJq7046827/2AojmRpnmiqrmzrvnAsz3Rt33iu73zv/8CgcEgsGo/IpHLJbDqft0NDMCBQodis1jcADBKE7nYcCpjPgU5AQBKkVYOHAeRudqtXsh60/vRHdSoBBCGBNAkLe4o4f2psgG8pjR6GM5OLmDB/DA0GBoQADAgICRIBBQUOYgwGCg2kEgudBgUHAIGcBg0MsZ0NCnMGYgsBtqEGAbCynrW3AQONgcIFBgiErK6wAAfUtLbCscWiowoAyLDczLZu0AIJCAYOoJn0G38ObAwPEvLEts/O1vUhsA8AAjGonEmA9W6hGAVpEjiQoKBAhT8HJSRkVyEQQAAJ//a5YeMPQIFyACqCnJjSIgFCB4oB+HOSokWOAB6wIWCxnk8MfYh5QsYg5sVHfQLVMSqhztJIxWIaC6QzJy8KfZgqrNT0zR+nUNl8fSMvZ6IDwJCJRfoI7IR4Cub9nDsha6RwR02xUZpGq1utUWUq9FKgYV6/abgOHjt45tquEgY0SDDHoJg+fxhXolKNrmfH/EoR5EdAKmjQfB1qvPmGIQIJ3g4gC2egVF7LqxtP8Ng2cViTKFUCIGbNFKEEmB/VbDlYdqLRn+du8oTg6jjbmfe+CbTM2+BcuySgbQVtQoOCt7s3U8wbsqGs3ZppZLnylwFe8Uql825ogANPckUnYDoOCogxQGXADajggjcw4AA8DSSyTQASMmjhhTQscBWGHHbo4YcghijiiCSWaOKJKKao4oostugiFBEAADs=" />
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div><div class="HeaderSection">
+<h1 id="ProductName">
+<!-- @Start Product -->
+zumo cc3200 Manifest
+<!-- @End Product -->
+</h1>
+
+<h2 id="ReleaseDate">
+<!-- @Start Date -->
+09-11-2015
+<!-- @End Date -->
+</h2>
+
+
+<h2 id="SRASID">
+<!-- @Start Date -->
+Manifest ID - <a href="https://opbu.itg.ti.com/SRAS/UserForms/SRAS_Record.aspx?Action=Modify&DB=SRAS&ID=SRAS00002135">SRAS00002135</a>
+<!-- @End Date -->
+</h2>
+</div><div class="LegendSection">
+<h2>Legend</h2>
+<p>(explanation of the fields in the Manifest Table below)</p>
+<table>
+<tbody>
+<tr>
+<td>
+<b>Software Name </b>
+</td>
+<td>
+The name of the application or file
+</td>
+</tr>
+<tr>
+<td>
+<b>Version</b>
+</td>
+<td>
+Version of the application or file
+</td>
+</tr>
+<tr>
+<td>
+<b>License Type</b>
+</td>
+<td>
+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 <a href="http://spdx.org/licenses/"> SPDX Short Identifier </a> 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.
+</td>
+</tr>
+<tr>
+<td>
+<b>Location</b>
+</td>
+<td>
+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.
+</td>
+</tr>
+<tr>
+<td>
+<b>Delivered As</b>
+</td>
+<td>
+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.
+</td>
+</tr>
+<tr>
+<td>
+<b>Modified by TI</b>
+</td>
+<td>
+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.
+</td>
+</tr>
+<tr>
+<td>
+<b>Obtained from</b>
+</td>
+<td>
+This field specifies from where or from whom TI obtained
+the Software. It may be a URL to an Open Source site, a 3<sup>rd</sup>
+party licensor, or TI. See Links Disclaimer in the Disclaimers
+Section.
+</td>
+</tr>
+</tbody>
+</table>
+</div><div class="DisclaimerSection">
+<h2>Disclaimers</h2>
+<h3>Export Control Classification Number (ECCN)</h3>
+<p>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</p>
+<h3>Links in the Manifest</h3>
+<p>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.</p>
+<h3>Open Source License References</h3>
+<p>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.</p>
+</div><div class="ExportSection">
+<h2>Export Information</h2>
+<p>ECCN for Software included in this release:</p>
+Publicly Available - Open Source or TI TSPA License
+</div><div class="ManifestTable">
+<!-- h2>Manifest Table</h2 -->
+
+ <table>
+ <tbody>
+
+ <h2>
+ zumo cc3200 Manifest Table
+ </h2>
+
+
+ <p>
+
+ See the Legend above for a description of these columns.
+
+ </p>
+
+ <table id="targetpackages" name="targetpackages">
+ <thead>
+ <tr>
+ <td><b>Software Name</b></td>
+ <td><b>Version</b></td>
+ <td><b>License Type</b></td>
+ <td><b>Delivered As</b></td>
+ <td><b>Modified by TI</b></td>
+ <td></td>
+ <td></td>
+ </tr>
+ </thead>
+
+
+ <tbody>
+ <tr>
+ <td id="name" name="name" rowspan="2">
+ Zumo demos
+ </td>
+ <td id="version" name="version" rowspan="2">
+ 1.0
+ </td>
+ <td id="license" name="license" rowspan="2">
+ BSD-3-clause
+ </td>
+ <td id="delivered" name="delivered" rowspan="2">
+ source
+ </td>
+ <td id="modified" name="modified" rowspan="2">
+ na
+ </td>
+ <td><b>Location</b></td>
+ <td id="location" name="location">
+ [as installed]/* except as noted in this manifest
+ </td>
+ </tr>
+ <tr>
+ <td><b>Obtained from</b></td>
+ <td id="obtained" name="obtained">
+ TI
+ </td>
+ </tr>
+
+ <tbody>
+ <tr>
+ <td id="name" name="name" rowspan="2">
+ LSM303
+ </td>
+ <td id="version" name="version" rowspan="2">
+
+ </td>
+ <td id="license" name="license" rowspan="2">
+ MIT
+ </td>
+ <td id="delivered" name="delivered" rowspan="2">
+ source
+ </td>
+ <td id="modified" name="modified" rowspan="2">
+ yes
+ </td>
+ <td><b>Location</b></td>
+ <td id="location" name="location">
+ [as installed]/src/Processing/*/GraphClass.pde
+ </td>
+ </tr>
+ <tr>
+ <td><b>Obtained from</b></td>
+ <td id="obtained" name="obtained">
+ https://github.com/pololu/lsm303-arduino
+ </td>
+ </tr>
+
+ <tbody>
+ <tr>
+ <td id="name" name="name" rowspan="2">
+ L3G
+ </td>
+ <td id="version" name="version" rowspan="2">
+
+ </td>
+ <td id="license" name="license" rowspan="2">
+ MIT
+ </td>
+ <td id="delivered" name="delivered" rowspan="2">
+ source
+ </td>
+ <td id="modified" name="modified" rowspan="2">
+ yes
+ </td>
+ <td><b>Location</b></td>
+ <td id="location" name="location">
+ [as installed]/src/Processing/*/GraphClass.pde
+ </td>
+ </tr>
+ <tr>
+ <td><b>Obtained from</b></td>
+ <td id="obtained" name="obtained">
+ https://github.com/pololu/l3g-arduino
+ </td>
+ </tr>
+
+ <tbody>
+ <tr>
+ <td id="name" name="name" rowspan="2">
+ Pushbutton and ZumoMotors
+ </td>
+ <td id="version" name="version" rowspan="2">
+
+ </td>
+ <td id="license" name="license" rowspan="2">
+ MIT
+ </td>
+ <td id="delivered" name="delivered" rowspan="2">
+ source
+ </td>
+ <td id="modified" name="modified" rowspan="2">
+ yes
+ </td>
+ <td><b>Location</b></td>
+ <td id="location" name="location">
+ [as installed]/src/Processing/*/GraphClass.pde
+ </td>
+ </tr>
+ <tr>
+ <td><b>Obtained from</b></td>
+ <td id="obtained" name="obtained">
+ https://github.com/pololu/zumo-shield
+ </td>
+ </tr>
+
+ <tbody>
+ <tr>
+ <td id="name" name="name" rowspan="2">
+ Line Graphing class
+ </td>
+ <td id="version" name="version" rowspan="2">
+
+ </td>
+ <td id="license" name="license" rowspan="2">
+ MIT
+ </td>
+ <td id="delivered" name="delivered" rowspan="2">
+ source
+ </td>
+ <td id="modified" name="modified" rowspan="2">
+ no
+ </td>
+ <td><b>Location</b></td>
+ <td id="location" name="location">
+ [as installed]/src/Processing/*/GraphClass.pde
+ </td>
+ </tr>
+ <tr>
+ <td><b>Obtained from</b></td>
+ <td id="obtained" name="obtained">
+ https://github.com/sebnil/RealtimePlotter
+ </td>
+ </tr>
+
+ <tbody>
+ <tr>
+ <td id="name" name="name" rowspan="2">
+ Cubic Spline interpolation in C++
+ </td>
+ <td id="version" name="version" rowspan="2">
+
+ </td>
+ <td id="license" name="license" rowspan="2">
+ GPL2
+ </td>
+ <td id="delivered" name="delivered" rowspan="2">
+ source
+ </td>
+ <td id="modified" name="modified" rowspan="2">
+ no
+ </td>
+ <td><b>Location</b></td>
+ <td id="location" name="location">
+ [as installed]/src/Energia/ZumoCC3200/utility/Spline.h
+ </td>
+ </tr>
+ <tr>
+ <td><b>Obtained from</b></td>
+ <td id="obtained" name="obtained">
+ http://kluge.in-chemnitz.de/opensource/spline/
+ </td>
+ </tr>
+
+ </tbody>
+ </table>
+
+ </p>
+ </p>
+ <p>
+
+</div><div class="CreditSection">
+<h2>Credits</h2>
+<BR> <BR><BR><BR><BR>
+</div><div class="LicenseSection">
+<h2>Licenses</h2>
+<BR><h3><b> zumo cc3200 Licenses </b></h3><BR> <BR><BR>LSM303<BR><BR>Copyright (c) 2014 Pololu Corporation. For more information, see<BR><BR>http://www.pololu.com/<BR>http://forum.pololu.com/<BR><BR>Permission is hereby granted, free of charge, to any person<BR>obtaining a copy of this software and associated documentation<BR>files (the "Software"), to deal in the Software without<BR>restriction, including without limitation the rights to use,<BR>copy, modify, merge, publish, distribute, sublicense, and/or sell<BR>copies of the Software, and to permit persons to whom the<BR>Software is furnished to do so, subject to the following<BR>conditions:<BR><BR>The above copyright notice and this permission notice shall be<BR>included in all copies or substantial portions of the Software.<BR><BR>THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,<BR>EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES<BR>OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND<BR>NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT<BR>HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,<BR>WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING<BR>FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR<BR>OTHER DEALINGS IN THE SOFTWARE.<BR><BR>L3G<BR>Copyright (c) 2014 Pololu Corporation. For more information, see<BR><BR>http://www.pololu.com/<BR>http://forum.pololu.com/<BR><BR>Permission is hereby granted, free of charge, to any person<BR>obtaining a copy of this software and associated documentation<BR>files (the "Software"), to deal in the Software without<BR>restriction, including without limitation the rights to use,<BR>copy, modify, merge, publish, distribute, sublicense, and/or sell<BR>copies of the Software, and to permit persons to whom the<BR>Software is furnished to do so, subject to the following<BR>conditions:<BR><BR>The above copyright notice and this permission notice shall be<BR>included in all copies or substantial portions of the Software.<BR><BR>THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,<BR>EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES<BR>OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND<BR>NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT<BR>HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,<BR>WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING<BR>FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR<BR>OTHER DEALINGS IN THE SOFTWARE.<BR><BR>Pushbutton and ZumoMotors<BR>Copyright (c) 2014 Pololu Corporation. For more information, see<BR><BR>http://www.pololu.com/<BR>http://forum.pololu.com/<BR><BR>Permission is hereby granted, free of charge, to any person<BR>obtaining a copy of this software and associated documentation<BR>files (the "Software"), to deal in the Software without<BR>restriction, including without limitation the rights to use,<BR>copy, modify, merge, publish, distribute, sublicense, and/or sell<BR>copies of the Software, and to permit persons to whom the<BR>Software is furnished to do so, subject to the following<BR>conditions:<BR><BR>The above copyright notice and this permission notice shall be<BR>included in all copies or substantial portions of the Software.<BR><BR>THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,<BR>EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES<BR>OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND<BR>NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT<BR>HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,<BR>WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING<BR>FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR<BR>OTHER DEALINGS IN THE SOFTWARE.<BR><BR>Line Graphing class<BR>The MIT License (MIT)<BR><BR>Copyright (c) 2013 Sebatian Nilsson<BR><BR>Permission is hereby granted, free of charge, to any person obtaining a copy<BR>of this software and associated documentation files (the "Software"), to deal<BR>in the Software without restriction, including without limitation the rights<BR>to use, copy, modify, merge, publish, distribute, sublicense, and/or sell<BR>copies of the Software, and to permit persons to whom the Software is<BR>furnished to do so, subject to the following conditions:<BR><BR>The above copyright notice and this permission notice shall be included in all<BR>copies or substantial portions of the Software.<BR><BR>THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR<BR>IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,<BR>FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE<BR>AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER<BR>LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,<BR>OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE<BR>SOFTWARE.<BR><BR>Cubic Spline interpolation in C++<BR> GNU GENERAL PUBLIC LICENSE<BR> Version 2, June 1991<BR><BR> Copyright (C) 1989, 1991 Free Software Foundation, Inc.,<BR> 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA<BR> Everyone is permitted to copy and distribute verbatim copies<BR> of this license document, but changing it is not allowed.<BR><BR> Preamble<BR><BR> The licenses for most software are designed to take away your<BR>freedom to share and change it. By contrast, the GNU General Public<BR>License is intended to guarantee your freedom to share and change free<BR>software--to make sure the software is free for all its users. This<BR>General Public License applies to most of the Free Software<BR>Foundation's software and to any other program whose authors commit to<BR>using it. (Some other Free Software Foundation software is covered by<BR>the GNU Lesser General Public License instead.) You can apply it to<BR>your programs, too.<BR><BR> When we speak of free software, we are referring to freedom, not<BR>price. Our General Public Licenses are designed to make sure that you<BR>have the freedom to distribute copies of free software (and charge for<BR>this service if you wish), that you receive source code or can get it<BR>if you want it, that you can change the software or use pieces of it<BR>in new free programs; and that you know you can do these things.<BR><BR> To protect your rights, we need to make restrictions that forbid<BR>anyone to deny you these rights or to ask you to surrender the rights.<BR>These restrictions translate to certain responsibilities for you if you<BR>distribute copies of the software, or if you modify it.<BR><BR> For example, if you distribute copies of such a program, whether<BR>gratis or for a fee, you must give the recipients all the rights that<BR>you have. You must make sure that they, too, receive or can get the<BR>source code. And you must show them these terms so they know their<BR>rights.<BR><BR> We protect your rights with two steps: (1) copyright the software, and<BR>(2) offer you this license which gives you legal permission to copy,<BR>distribute and/or modify the software.<BR><BR> Also, for each author's protection and ours, we want to make certain<BR>that everyone understands that there is no warranty for this free<BR>software. If the software is modified by someone else and passed on, we<BR>want its recipients to know that what they have is not the original, so<BR>that any problems introduced by others will not reflect on the original<BR>authors' reputations.<BR><BR> Finally, any free program is threatened constantly by software<BR>patents. We wish to avoid the danger that redistributors of a free<BR>program will individually obtain patent licenses, in effect making the<BR>program proprietary. To prevent this, we have made it clear that any<BR>patent must be licensed for everyone's free use or not licensed at all.<BR><BR> The precise terms and conditions for copying, distribution and<BR>modification follow.<BR><BR> GNU GENERAL PUBLIC LICENSE<BR> TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION<BR><BR> 0. This License applies to any program or other work which contains<BR>a notice placed by the copyright holder saying it may be distributed<BR>under the terms of this General Public License. The "Program", below,<BR>refers to any such program or work, and a "work based on the Program"<BR>means either the Program or any derivative work under copyright law:<BR>that is to say, a work containing the Program or a portion of it,<BR>either verbatim or with modifications and/or translated into another<BR>language. (Hereinafter, translation is included without limitation in<BR>the term "modification".) Each licensee is addressed as "you".<BR><BR>Activities other than copying, distribution and modification are not<BR>covered by this License; they are outside its scope. The act of<BR>running the Program is not restricted, and the output from the Program<BR>is covered only if its contents constitute a work based on the<BR>Program (independent of having been made by running the Program).<BR>Whether that is true depends on what the Program does.<BR><BR> 1. You may copy and distribute verbatim copies of the Program's<BR>source code as you receive it, in any medium, provided that you<BR>conspicuously and appropriately publish on each copy an appropriate<BR>copyright notice and disclaimer of warranty; keep intact all the<BR>notices that refer to this License and to the absence of any warranty;<BR>and give any other recipients of the Program a copy of this License<BR>along with the Program.<BR><BR>You may charge a fee for the physical act of transferring a copy, and<BR>you may at your option offer warranty protection in exchange for a fee.<BR><BR> 2. You may modify your copy or copies of the Program or any portion<BR>of it, thus forming a work based on the Program, and copy and<BR>distribute such modifications or work under the terms of Section 1<BR>above, provided that you also meet all of these conditions:<BR><BR> a) You must cause the modified files to carry prominent notices<BR> stating that you changed the files and the date of any change.<BR><BR> b) You must cause any work that you distribute or publish, that in<BR> whole or in part contains or is derived from the Program or any<BR> part thereof, to be licensed as a whole at no charge to all third<BR> parties under the terms of this License.<BR><BR> c) If the modified program normally reads commands interactively<BR> when run, you must cause it, when started running for such<BR> interactive use in the most ordinary way, to print or display an<BR> announcement including an appropriate copyright notice and a<BR> notice that there is no warranty (or else, saying that you provide<BR> a warranty) and that users may redistribute the program under<BR> these conditions, and telling the user how to view a copy of this<BR> License. (Exception: if the Program itself is interactive but<BR> does not normally print such an announcement, your work based on<BR> the Program is not required to print an announcement.)<BR><BR>These requirements apply to the modified work as a whole. If<BR>identifiable sections of that work are not derived from the Program,<BR>and can be reasonably considered independent and separate works in<BR>themselves, then this License, and its terms, do not apply to those<BR>sections when you distribute them as separate works. But when you<BR>distribute the same sections as part of a whole which is a work based<BR>on the Program, the distribution of the whole must be on the terms of<BR>this License, whose permissions for other licensees extend to the<BR>entire whole, and thus to each and every part regardless of who wrote it.<BR><BR>Thus, it is not the intent of this section to claim rights or contest<BR>your rights to work written entirely by you; rather, the intent is to<BR>exercise the right to control the distribution of derivative or<BR>collective works based on the Program.<BR><BR>In addition, mere aggregation of another work not based on the Program<BR>with the Program (or with a work based on the Program) on a volume of<BR>a storage or distribution medium does not bring the other work under<BR>the scope of this License.<BR><BR> 3. You may copy and distribute the Program (or a work based on it,<BR>under Section 2) in object code or executable form under the terms of<BR>Sections 1 and 2 above provided that you also do one of the following:<BR><BR> a) Accompany it with the complete corresponding machine-readable<BR> source code, which must be distributed under the terms of Sections<BR> 1 and 2 above on a medium customarily used for software interchange; or,<BR><BR> b) Accompany it with a written offer, valid for at least three<BR> years, to give any third party, for a charge no more than your<BR> cost of physically performing source distribution, a complete<BR> machine-readable copy of the corresponding source code, to be<BR> distributed under the terms of Sections 1 and 2 above on a medium<BR> customarily used for software interchange; or,<BR><BR> c) Accompany it with the information you received as to the offer<BR> to distribute corresponding source code. (This alternative is<BR> allowed only for noncommercial distribution and only if you<BR> received the program in object code or executable form with such<BR> an offer, in accord with Subsection b above.)<BR><BR>The source code for a work means the preferred form of the work for<BR>making modifications to it. For an executable work, complete source<BR>code means all the source code for all modules it contains, plus any<BR>associated interface definition files, plus the scripts used to<BR>control compilation and installation of the executable. However, as a<BR>special exception, the source code distributed need not include<BR>anything that is normally distributed (in either source or binary<BR>form) with the major components (compiler, kernel, and so on) of the<BR>operating system on which the executable runs, unless that component<BR>itself accompanies the executable.<BR><BR>If distribution of executable or object code is made by offering<BR>access to copy from a designated place, then offering equivalent<BR>access to copy the source code from the same place counts as<BR>distribution of the source code, even though third parties are not<BR>compelled to copy the source along with the object code.<BR><BR> 4. You may not copy, modify, sublicense, or distribute the Program<BR>except as expressly provided under this License. Any attempt<BR>otherwise to copy, modify, sublicense or distribute the Program is<BR>void, and will automatically terminate your rights under this License.<BR>However, parties who have received copies, or rights, from you under<BR>this License will not have their licenses terminated so long as such<BR>parties remain in full compliance.<BR><BR> 5. You are not required to accept this License, since you have not<BR>signed it. However, nothing else grants you permission to modify or<BR>distribute the Program or its derivative works. These actions are<BR>prohibited by law if you do not accept this License. Therefore, by<BR>modifying or distributing the Program (or any work based on the<BR>Program), you indicate your acceptance of this License to do so, and<BR>all its terms and conditions for copying, distributing or modifying<BR>the Program or works based on it.<BR><BR> 6. Each time you redistribute the Program (or any work based on the<BR>Program), the recipient automatically receives a license from the<BR>original licensor to copy, distribute or modify the Program subject to<BR>these terms and conditions. You may not impose any further<BR>restrictions on the recipients' exercise of the rights granted herein.<BR>You are not responsible for enforcing compliance by third parties to<BR>this License.<BR><BR> 7. If, as a consequence of a court judgment or allegation of patent<BR>infringement or for any other reason (not limited to patent issues),<BR>conditions are imposed on you (whether by court order, agreement or<BR>otherwise) that contradict the conditions of this License, they do not<BR>excuse you from the conditions of this License. If you cannot<BR>distribute so as to satisfy simultaneously your obligations under this<BR>License and any other pertinent obligations, then as a consequence you<BR>may not distribute the Program at all. For example, if a patent<BR>license would not permit royalty-free redistribution of the Program by<BR>all those who receive copies directly or indirectly through you, then<BR>the only way you could satisfy both it and this License would be to<BR>refrain entirely from distribution of the Program.<BR><BR>If any portion of this section is held invalid or unenforceable under<BR>any particular circumstance, the balance of the section is intended to<BR>apply and the section as a whole is intended to apply in other<BR>circumstances.<BR><BR>It is not the purpose of this section to induce you to infringe any<BR>patents or other property right claims or to contest validity of any<BR>such claims; this section has the sole purpose of protecting the<BR>integrity of the free software distribution system, which is<BR>implemented by public license practices. Many people have made<BR>generous contributions to the wide range of software distributed<BR>through that system in reliance on consistent application of that<BR>system; it is up to the author/donor to decide if he or she is willing<BR>to distribute software through any other system and a licensee cannot<BR>impose that choice.<BR><BR>This section is intended to make thoroughly clear what is believed to<BR>be a consequence of the rest of this License.<BR><BR> 8. If the distribution and/or use of the Program is restricted in<BR>certain countries either by patents or by copyrighted interfaces, the<BR>original copyright holder who places the Program under this License<BR>may add an explicit geographical distribution limitation excluding<BR>those countries, so that distribution is permitted only in or among<BR>countries not thus excluded. In such case, this License incorporates<BR>the limitation as if written in the body of this License.<BR><BR> 9. The Free Software Foundation may publish revised and/or new versions<BR>of the General Public License from time to time. Such new versions will<BR>be similar in spirit to the present version, but may differ in detail to<BR>address new problems or concerns.<BR><BR>Each version is given a distinguishing version number. If the Program<BR>specifies a version number of this License which applies to it and "any<BR>later version", you have the option of following the terms and conditions<BR>either of that version or of any later version published by the Free<BR>Software Foundation. If the Program does not specify a version number of<BR>this License, you may choose any version ever published by the Free Software<BR>Foundation.<BR><BR> 10. If you wish to incorporate parts of the Program into other free<BR>programs whose distribution conditions are different, write to the author<BR>to ask for permission. For software which is copyrighted by the Free<BR>Software Foundation, write to the Free Software Foundation; we sometimes<BR>make exceptions for this. Our decision will be guided by the two goals<BR>of preserving the free status of all derivatives of our free software and<BR>of promoting the sharing and reuse of software generally.<BR><BR> NO WARRANTY<BR><BR> 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY<BR>FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN<BR>OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES<BR>PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED<BR>OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF<BR>MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS<BR>TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE<BR>PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,<BR>REPAIR OR CORRECTION.<BR><BR> 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING<BR>WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR<BR>REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,<BR>INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING<BR>OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED<BR>TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY<BR>YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER<BR>PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE<BR>POSSIBILITY OF SUCH DAMAGES.<BR><BR> END OF TERMS AND CONDITIONS<BR><BR> How to Apply These Terms to Your New Programs<BR><BR> If you develop a new program, and you want it to be of the greatest<BR>possible use to the public, the best way to achieve this is to make it<BR>free software which everyone can redistribute and change under these terms.<BR><BR> To do so, attach the following notices to the program. It is safest<BR>to attach them to the start of each source file to most effectively<BR>convey the exclusion of warranty; and each file should have at least<BR>the "copyright" line and a pointer to where the full notice is found.<BR><BR> <one line to give the program's name and a brief idea of what it does.><BR> Copyright (C) <year> <name of author><BR><BR> This program is free software; you can redistribute it and/or modify<BR> it under the terms of the GNU General Public License as published by<BR> the Free Software Foundation; either version 2 of the License, or<BR> (at your option) any later version.<BR><BR> This program is distributed in the hope that it will be useful,<BR> but WITHOUT ANY WARRANTY; without even the implied warranty of<BR> MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the<BR> GNU General Public License for more details.<BR><BR> You should have received a copy of the GNU General Public License along<BR> with this program; if not, write to the Free Software Foundation, Inc.,<BR> 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.<BR><BR>Also add information on how to contact you by electronic and paper mail.<BR><BR>If the program is interactive, make it output a short notice like this<BR>when it starts in an interactive mode:<BR><BR> Gnomovision version 69, Copyright (C) year name of author<BR> Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.<BR> This is free software, and you are welcome to redistribute it<BR> under certain conditions; type `show c' for details.<BR><BR>The hypothetical commands `show w' and `show c' should show the appropriate<BR>parts of the General Public License. Of course, the commands you use may<BR>be called something other than `show w' and `show c'; they could even be<BR>mouse-clicks or menu items--whatever suits your program.<BR><BR>You should also get your employer (if you work as a programmer) or your<BR>school, if any, to sign a "copyright disclaimer" for the program, if<BR>necessary. Here is a sample; alter the names:<BR><BR> Yoyodyne, Inc., hereby disclaims all copyright interest in the program<BR> `Gnomovision' (which makes passes at compilers) written by James Hacker.<BR><BR> <signature of Ty Coon>, 1 April 1989<BR> Ty Coon, President of Vice<BR><BR>This General Public License does not permit incorporating your program into<BR>proprietary programs. If your program is a subroutine library, you may<BR>consider it more useful to permit linking proprietary applications with the<BR>library. If this is what you want to do, use the GNU Lesser General<BR>Public License instead of this License.<BR><BR><BR><BR><BR>
+</div>
+
+</body></html>
diff --git a/makefile b/makefile
--- /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
--- /dev/null
@@ -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 <ZumoMotors.h>
+
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/AssistedDrive.ino b/src/Energia/libraries/ZumoCC3200/examples/AssistedDrive/AssistedDrive.ino
--- /dev/null
@@ -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 <IMUManager.h>
+#include <Utilities.h>
+
+/* 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
--- /dev/null
@@ -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 <Energia.h>
+#include <math.h>
+#include <PIDController.h>
+#include <ZumoMotors.h>
+
+#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
--- /dev/null
@@ -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 <Wire.h>
+
+/* 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <ti/sysbios/knl/Task.h>
+
+#include <string.h>
+
+/* 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
--- /dev/null
@@ -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 <IMUManager.h>
+#include <DCM.h>
+
+/* 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <DCM.h>
+
+#include <ti/sysbios/knl/Task.h>
+#include <string.h>
+
+
+/* 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
--- /dev/null
@@ -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 <Wire.h>
+#include <Energia.h>
+
+#include <DCM.h>
+#include <IMUManager.h>
+
+/* 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
--- /dev/null
@@ -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 <Energia.h>
+
+#include <ZumoMotors.h>
+
+#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
--- /dev/null
@@ -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 <IMUManager.h>
+#include <Balancer.h>
+
+/* 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <ti/sysbios/knl/Task.h>
+#include <string.h>
+#include <stdio.h>
+
+/* 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
--- /dev/null
@@ -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 <Wire.h>
+
+/* 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
--- /dev/null
@@ -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 <Energia.h>
+#include <math.h>
+
+#include <ZumoMotors.h>
+#include <Balancer.h>
+#include <IMUManager.h>
+
+#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
--- /dev/null
@@ -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 <ZumoMotors.h>
+#include <Utilities.h>
+#include <IMUManager.h>
+#include <WiFiClient.h>
+
+/* 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
--- /dev/null
@@ -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 <Energia.h>
+#include <math.h>
+#include <Utilities.h>
+#include <ZumoMotors.h>
+#include <CommandManager.h>
+#include <TurnAngleCommand.h>
+#include <DriveLineCommand.h>
+#include <WaitCommand.h>
+
+#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
--- /dev/null
@@ -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 <Wire.h>
+
+/* 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <ti/sysbios/knl/Task.h>
+
+#include <string.h>
+
+/* 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
--- /dev/null
@@ -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 <ZumoCC3200.h>
+#include <L3G.h>
+#include <LSM303.h>
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G imuGyro; /* gyro data */
+
+/* motor sketch external declarations */
+extern char 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <ti/sysbios/knl/Task.h>
+
+#include <string.h>
+
+/* 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
--- /dev/null
@@ -0,0 +1,47 @@
+/*
+ * ======== imuLoop ========
+ * This sketch simply reads the Zumo IMU sensors at 20Hz and prints
+ * the current value once per second to the UART.
+ */
+
+#include <Wire.h>
+
+#include <L3G.h>
+#include <LSM303.h>
+
+/* Pololu IMU data instance objects */
+LSM303 imuCompass; /* acceleration and magnetometer */
+L3G imuGyro; /* gyro data */
+
+/*
+ * ======== imuSetup ========
+ */
+void imuSetup()
+{
+ Serial.begin(9600);
+ Serial.println("imuSetup ...");
+ Wire.begin();
+
+ /* initialize Zumo accelerometer and magnetometer */
+ imuCompass.init();
+ imuCompass.enableDefault();
+
+ /* initialize Zumo gyro */
+ if (!imuGyro.init()) {
+ Serial.print("Failed to autodetect gyro type!");
+ delay(1000);
+ }
+ imuGyro.enableDefault();
+ Serial.println("imuSetup done.");
+}
+
+/*
+ * ======== imuLoop ========
+ */
+void imuLoop()
+{
+ /* update IMU data every 50 ms (200 Hz) */
+ imuGyro.read();
+ imuCompass.read();
+ delay(50);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/ManualDrive/motorLoop.ino
--- /dev/null
@@ -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 <Energia.h>
+
+#include <ZumoMotors.h>
+
+#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
--- /dev/null
@@ -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 <ZumoCC3200.h>
+#include <L3G.h>
+#include <LSM303.h>
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G imuGyro; /* gyro data */
+
+/* motor sketch external declarations */
+extern char 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <Pushbutton.h>
+#include <ti/sysbios/knl/Task.h>
+
+#include <string.h>
+
+/* 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
--- /dev/null
@@ -0,0 +1,47 @@
+/*
+ * ======== imuLoop ========
+ * This sketch simply reads the Zumo IMU sensors at 20Hz and prints
+ * the current value once per second to the UART.
+ */
+
+#include <Wire.h>
+
+#include <L3G.h>
+#include <LSM303.h>
+
+/* Pololu IMU data instance objects */
+LSM303 imuCompass; /* acceleration and magnetometer */
+L3G imuGyro; /* gyro data */
+
+/*
+ * ======== imuSetup ========
+ */
+void imuSetup()
+{
+ Serial.begin(9600);
+ Serial.println("imuSetup ...");
+ Wire.begin();
+
+ /* initialize Zumo accelerometer and magnetometer */
+ imuCompass.init();
+ imuCompass.enableDefault();
+
+ /* initialize Zumo gyro */
+ if (!imuGyro.init()) {
+ Serial.print("Failed to autodetect gyro type!");
+ delay(1000);
+ }
+ imuGyro.enableDefault();
+ Serial.println("imuSetup done.");
+}
+
+/*
+ * ======== imuLoop ========
+ */
+void imuLoop()
+{
+ /* update IMU data every 50 ms (200 Hz) */
+ imuGyro.read();
+ imuCompass.read();
+ delay(50);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/MasterSlave/motorLoop.ino
--- /dev/null
@@ -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 <Energia.h>
+
+#include <ZumoMotors.h>
+
+#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
--- /dev/null
@@ -0,0 +1,25 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdarg.h>
+
+#include "MCP.h"
+#include <WiFi.h>
+
+/* 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
--- /dev/null
@@ -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
--- /dev/null
@@ -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 <ZumoCC3200.h>
+#include <L3G.h>
+#include <LSM303.h>
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G imuGyro; /* gyro data */
+
+/* motor sketch external declarations */
+extern char 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <Pushbutton.h>
+#include <ti/sysbios/knl/Task.h>
+#include "MCP.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+/* 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
--- /dev/null
@@ -0,0 +1,47 @@
+/*
+ * ======== imuLoop ========
+ * This sketch simply reads the Zumo IMU sensors at 20Hz and prints
+ * the current value once per second to the UART.
+ */
+
+#include <Wire.h>
+
+#include <L3G.h>
+#include <LSM303.h>
+
+/* Pololu IMU data instance objects */
+LSM303 imuCompass; /* acceleration and magnetometer */
+L3G imuGyro; /* gyro data */
+
+/*
+ * ======== imuSetup ========
+ */
+void imuSetup()
+{
+ Serial.begin(9600);
+ Serial.println("imuSetup ...");
+ Wire.begin();
+
+ /* initialize Zumo accelerometer and magnetometer */
+ imuCompass.init();
+ imuCompass.enableDefault();
+
+ /* initialize Zumo gyro */
+ if (!imuGyro.init()) {
+ Serial.print("Failed to autodetect gyro type!");
+ delay(1000);
+ }
+ imuGyro.enableDefault();
+ Serial.println("imuSetup done.");
+}
+
+/*
+ * ======== imuLoop ========
+ */
+void imuLoop()
+{
+ /* update IMU data every 50 ms (200 Hz) */
+ imuGyro.read();
+ imuCompass.read();
+ delay(50);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/Multicast/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/Multicast/motorLoop.ino
--- /dev/null
@@ -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 <Energia.h>
+
+#include <ZumoMotors.h>
+
+#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
--- /dev/null
@@ -0,0 +1,301 @@
+#include <vector>
+#include <cassert>
+#include <math.h>
+
+#include "MotionPlanner.h"
+
+#include <ZumoMotors.h>
+#include <PIDController.h>
+#include <IMUManager.h>
+#include <vector>
+#include <spline.h>
+
+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<int> 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<double> 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<tk::spline> 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<double> 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<double> 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
--- /dev/null
@@ -0,0 +1,49 @@
+#ifndef MotionPlanner_h
+#define MotionPlanner_h
+
+#include <vector>
+
+#include <ZumoMotors.h>
+#include <spline.h>
+#include <PIDController.h>
+
+class MotionPlanner
+{
+ public:
+ struct Point {
+ double x;
+ double y;
+ };
+
+ MotionPlanner();
+
+ void setPoints(std::vector<int> 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<Point> points;
+ std::vector<double> 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
--- /dev/null
@@ -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 <ZumoCC3200.h>
+#include <IMUManager.h>
+#include <Utilities.h>
+#include "MotionPlanner.h"
+#include <WiFiClient.h>
+
+/* 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <ti/sysbios/knl/Task.h>
+#include <string.h>
+#include "MotionPlanner.h"
+#include <unistd.h>
+#include <cstdio>
+#include <cstdlib>
+#include <vector>
+
+/* 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<int> 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
--- /dev/null
@@ -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 <Wire.h>
+
+/* 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
--- /dev/null
@@ -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 <Energia.h>
+#include <math.h>
+
+#include <ZumoMotors.h>
+#include <PIDController.h>
+#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
--- /dev/null
@@ -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 <ZumoCC3200.h>
+#include <L3G.h>
+#include <LSM303.h>
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G imuGyro; /* gyro data */
+
+/* motor sketch external declarations */
+extern char 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <Pushbutton.h>
+#include <ti/sysbios/knl/Task.h>
+
+#include <string.h>
+
+/* 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
--- /dev/null
@@ -0,0 +1,47 @@
+/*
+ * ======== imuLoop ========
+ * This sketch simply reads the Zumo IMU sensors at 20Hz and prints
+ * the current value once per second to the UART.
+ */
+
+#include <Wire.h>
+
+#include <L3G.h>
+#include <LSM303.h>
+
+/* Pololu IMU data instance objects */
+LSM303 imuCompass; /* acceleration and magnetometer */
+L3G imuGyro; /* gyro data */
+
+/*
+ * ======== imuSetup ========
+ */
+void imuSetup()
+{
+ Serial.begin(9600);
+ Serial.println("imuSetup ...");
+ Wire.begin();
+
+ /* initialize Zumo accelerometer and magnetometer */
+ imuCompass.init();
+ imuCompass.enableDefault();
+
+ /* initialize Zumo gyro */
+ if (!imuGyro.init()) {
+ Serial.print("Failed to autodetect gyro type!");
+ delay(1000);
+ }
+ imuGyro.enableDefault();
+ Serial.println("imuSetup done.");
+}
+
+/*
+ * ======== imuLoop ========
+ */
+void imuLoop()
+{
+ /* update IMU data every 50 ms (200 Hz) */
+ imuGyro.read();
+ imuCompass.read();
+ delay(50);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPBroadcast/motorLoop.ino
--- /dev/null
@@ -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 <Energia.h>
+#include <PIDController.h>
+#include <ZumoMotors.h>
+
+#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
--- /dev/null
@@ -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 <ZumoCC3200.h>
+#include <L3G.h>
+#include <LSM303.h>
+
+/* imu sketch external declarations */
+extern LSM303 imuCompass; /* acceleration and magnetometer */
+extern L3G imuGyro; /* gyro data */
+
+/* motor sketch external declarations */
+extern char 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
--- /dev/null
@@ -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 <WiFi.h>
+#include <Pushbutton.h>
+#include <ti/sysbios/knl/Task.h>
+
+#include <string.h>
+
+/* 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
--- /dev/null
@@ -0,0 +1,47 @@
+/*
+ * ======== imuLoop ========
+ * This sketch simply reads the Zumo IMU sensors at 20Hz and prints
+ * the current value once per second to the UART.
+ */
+
+#include <Wire.h>
+
+#include <L3G.h>
+#include <LSM303.h>
+
+/* Pololu IMU data instance objects */
+LSM303 imuCompass; /* acceleration and magnetometer */
+L3G imuGyro; /* gyro data */
+
+/*
+ * ======== imuSetup ========
+ */
+void imuSetup()
+{
+ Serial.begin(9600);
+ Serial.println("imuSetup ...");
+ Wire.begin();
+
+ /* initialize Zumo accelerometer and magnetometer */
+ imuCompass.init();
+ imuCompass.enableDefault();
+
+ /* initialize Zumo gyro */
+ if (!imuGyro.init()) {
+ Serial.print("Failed to autodetect gyro type!");
+ delay(1000);
+ }
+ imuGyro.enableDefault();
+ Serial.println("imuSetup done.");
+}
+
+/*
+ * ======== imuLoop ========
+ */
+void imuLoop()
+{
+ /* update IMU data every 50 ms (200 Hz) */
+ imuGyro.read();
+ imuCompass.read();
+ delay(50);
+}
diff --git a/src/Energia/libraries/ZumoCC3200/examples/UDPExample/motorLoop.ino b/src/Energia/libraries/ZumoCC3200/examples/UDPExample/motorLoop.ino
--- /dev/null
@@ -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 <Energia.h>
+
+#include <ZumoMotors.h>
+
+#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
--- /dev/null
@@ -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 <math.h>
+
+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
--- /dev/null
@@ -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
--- /dev/null
@@ -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
--- /dev/null
@@ -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 <Energia.h>
+#include "CommandManager.h"
+
+vector<Command*> 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
--- /dev/null
@@ -0,0 +1,19 @@
+#ifndef CommandManager_h
+#define CommandManager_h
+
+#include <vector>
+#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<Command *> commandList;
+};
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/DCM.cpp b/src/Energia/libraries/ZumoCC3200/utility/DCM.cpp
--- /dev/null
@@ -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 <xdc/std.h>
+
+#include <stdlib.h>
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <assert.h>
+
+#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
--- /dev/null
@@ -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
--- /dev/null
@@ -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 <Energia.h>
+
+#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
--- /dev/null
@@ -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
--- /dev/null
@@ -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<int16_t> IMUManager::mag_min = {32767, 32767, 32767};
+LSM303::vector<int16_t> 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
--- /dev/null
@@ -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 <Energia.h>
+#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<int16_t> mag_min;
+ static LSM303::vector<int16_t> 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
--- /dev/null
@@ -0,0 +1,288 @@
+/*
+ * ======== L3G.cpp ========
+ */
+
+#include <Wire.h>
+#include <math.h>
+
+#include "L3G.h"
+
+// Defines ////////////////////////////////////////////////////////////////
+
+/* The Arduino two-wire interface uses a 7-bit number for the address,
+ * and sets the last bit correctly based on reads and writes
+ */
+#define D20_SA0_HIGH_ADDRESS 0b1101011 // also applies to D20H
+#define D20_SA0_LOW_ADDRESS 0b1101010 // also applies to D20H
+#define L3G4200D_SA0_HIGH_ADDRESS 0b1101001
+#define L3G4200D_SA0_LOW_ADDRESS 0b1101000
+
+#define TEST_REG_ERROR -1
+
+#define D20H_WHO_ID 0xD7
+#define D20_WHO_ID 0xD4
+#define L3G4200D_WHO_ID 0xD3
+
+static int testReg(byte address, L3G::regAddr reg);
+
+/*
+ * ======== L3G ========
+ * Constructors for L3G gyro
+ */
+L3G::L3G(void)
+{
+ _device = device_auto;
+
+ io_timeout = 0; // 0 = no timeout
+ did_timeout = false;
+}
+
+/*
+ * ======== timeoutOccurred ========
+ * Did a timeout occur in read() since the last call to timeoutOccurred()?
+ */
+bool L3G::timeoutOccurred()
+{
+ bool tmp = did_timeout;
+ did_timeout = false;
+ return tmp;
+}
+
+/*
+ * ======== setTimeout ========
+ */
+void L3G::setTimeout(unsigned int timeout)
+{
+ io_timeout = timeout;
+}
+
+/*
+ * ======== getTimeout ========
+ */
+unsigned int L3G::getTimeout()
+{
+ return io_timeout;
+}
+
+/*
+ * ======== init ========
+ */
+bool L3G::init(deviceType device, sa0State sa0)
+{
+ int id;
+
+ /* perform auto-detection unless device type and SA0 state were both
+ * specified
+ */
+ if (device == device_auto || sa0 == sa0_auto) {
+ /* check for L3GD20H, D20 if device is unidentified or was specified
+ * to be one of these types
+ */
+ if (device == device_auto || device == device_D20H
+ || device == device_D20) {
+ // check SA0 high address unless SA0 was specified to be low
+ if (sa0
+ != sa0_low && (id = testReg(D20_SA0_HIGH_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) {
+ /* device responds to address 1101011; it's a D20H or D20
+ * with SA0 high
+ */
+ sa0 = sa0_high;
+ if (device == device_auto) {
+ // use ID from WHO_AM_I register to determine device type
+ device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
+ }
+ }
+ // check SA0 low address unless SA0 was specified to be high
+ else if (sa0
+ != sa0_high && (id = testReg(D20_SA0_LOW_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) {
+ /* device responds to address 1101010; it's a D20H or D20
+ * with SA0 low
+ */
+ sa0 = sa0_low;
+ if (device == device_auto) {
+ // use ID from WHO_AM_I register to determine device type
+ device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
+ }
+ }
+ }
+
+ /* check for L3G4200D if device is still unidentified or was specified
+ * to be this type
+ */
+ if (device == device_auto || device == device_4200D) {
+ if (sa0
+ != sa0_low && testReg(L3G4200D_SA0_HIGH_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) {
+ /* device responds to address 1101001; it's a 4200D with SA0
+ * high
+ */
+ device = device_4200D;
+ sa0 = sa0_high;
+ }
+ else if (sa0
+ != sa0_high && testReg(L3G4200D_SA0_LOW_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) {
+ /* device responds to address 1101000; it's a 4200D with SA0
+ * low
+ */
+ device = device_4200D;
+ sa0 = sa0_low;
+ }
+ }
+
+ /* make sure device and SA0 were successfully detected; otherwise,
+ * indicate failure
+ */
+ if (device == device_auto || sa0 == sa0_auto) {
+ return false;
+ }
+ }
+
+ _device = device;
+
+ // set device address
+ switch (device) {
+ case device_D20H:
+ case device_D20:
+ address =
+ (sa0 == sa0_high) ?
+ D20_SA0_HIGH_ADDRESS : D20_SA0_LOW_ADDRESS;
+ break;
+
+ case device_4200D:
+ address =
+ (sa0 == sa0_high) ?
+ L3G4200D_SA0_HIGH_ADDRESS :
+ L3G4200D_SA0_LOW_ADDRESS;
+ break;
+ }
+
+ return true;
+}
+
+/*
+ * ======== enableDefault ========
+ * Enables the L3G's gyro
+ *
+ * Also:
+ * - Sets gyro full scale (gain) to default power-on value of +/- 250 dps
+ * (specified as +/- 245 dps for L3GD20H).
+ * - Selects 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<float> *a)
+{
+ float mag = sqrt(vector_dot(a, a));
+ a->x /= mag;
+ a->y /= mag;
+ a->z /= mag;
+}
+
+/*
+ * ======== testReg ========
+ */
+static int testReg(byte address, L3G::regAddr reg)
+{
+ Wire.beginTransmission(address);
+ Wire.write((byte) reg);
+ if (Wire.endTransmission() != 0) {
+ return TEST_REG_ERROR;
+ }
+
+ Wire.requestFrom(address, (byte) 1);
+ if (Wire.available()) {
+ return Wire.read();
+ }
+ else {
+ return TEST_REG_ERROR;
+ }
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/L3G.h b/src/Energia/libraries/ZumoCC3200/utility/L3G.h
--- /dev/null
@@ -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 <Arduino.h> // for byte data type
+
+class L3G {
+public:
+ /*
+ * ======== vector ========
+ */
+ template<typename T> struct vector {
+ T x, y, z;
+ };
+
+ enum deviceType {
+ device_4200D, device_D20, device_D20H, device_auto
+ };
+ enum sa0State {
+ sa0_low, sa0_high, sa0_auto
+ };
+
+ // register addresses
+ enum regAddr {
+ WHO_AM_I = 0x0F,
+
+ CTRL1 = 0x20, // D20H
+ CTRL_REG1 = 0x20, // D20, 4200D
+ CTRL2 = 0x21, // D20H
+ CTRL_REG2 = 0x21, // D20, 4200D
+ CTRL3 = 0x22, // D20H
+ CTRL_REG3 = 0x22, // D20, 4200D
+ CTRL4 = 0x23, // D20H
+ CTRL_REG4 = 0x23, // D20, 4200D
+ CTRL5 = 0x24, // D20H
+ CTRL_REG5 = 0x24, // D20, 4200D
+ REFERENCE = 0x25,
+ OUT_TEMP = 0x26,
+ STATUS = 0x27, // D20H
+ STATUS_REG = 0x27, // D20, 4200D
+
+ OUT_X_L = 0x28,
+ OUT_X_H = 0x29,
+ OUT_Y_L = 0x2A,
+ OUT_Y_H = 0x2B,
+ OUT_Z_L = 0x2C,
+ OUT_Z_H = 0x2D,
+
+ FIFO_CTRL = 0x2E, // D20H
+ FIFO_CTRL_REG = 0x2E, // D20, 4200D
+ FIFO_SRC = 0x2F, // D20H
+ FIFO_SRC_REG = 0x2F, // D20, 4200D
+
+ IG_CFG = 0x30, // D20H
+ INT1_CFG = 0x30, // D20, 4200D
+ IG_SRC = 0x31, // D20H
+ INT1_SRC = 0x31, // D20, 4200D
+ IG_THS_XH = 0x32, // D20H
+ INT1_THS_XH = 0x32, // D20, 4200D
+ IG_THS_XL = 0x33, // D20H
+ INT1_THS_XL = 0x33, // D20, 4200D
+ IG_THS_YH = 0x34, // D20H
+ INT1_THS_YH = 0x34, // D20, 4200D
+ IG_THS_YL = 0x35, // D20H
+ INT1_THS_YL = 0x35, // D20, 4200D
+ IG_THS_ZH = 0x36, // D20H
+ INT1_THS_ZH = 0x36, // D20, 4200D
+ IG_THS_ZL = 0x37, // D20H
+ INT1_THS_ZL = 0x37, // D20, 4200D
+ IG_DURATION = 0x38, // D20H
+ INT1_DURATION = 0x38, // D20, 4200D
+
+ LOW_ODR = 0x39 // D20H
+ };
+
+ /*
+ * ======== g ========
+ * gyro angular velocity readings
+ */
+ vector<int16_t> g;
+
+ /*
+ * ======== last_status ========
+ * status of last I2C transmission
+ */
+ byte last_status;
+
+ /*
+ * ======== L3G ========
+ * Constructor
+ */
+ L3G(void);
+
+ /*
+ * ======== init ========
+ */
+ bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
+
+ /*
+ * ======== getDeviceType ========
+ */
+ deviceType getDeviceType(void)
+ {
+ return _device;
+ }
+
+ /*
+ * ======== enableDefault ========
+ */
+ void enableDefault(void);
+
+ /*
+ * ======== writeReg ========
+ */
+ void writeReg(byte reg, byte value);
+
+ /*
+ * ======== readReg ========
+ */
+ byte readReg(byte reg);
+
+ /*
+ * ======== read ========
+ */
+ void read(void);
+
+ /*
+ * ======== setTimeout ========
+ */
+ void setTimeout(unsigned int timeout);
+
+ /*
+ * ======== getTimeout ========
+ */
+ unsigned int getTimeout(void);
+
+ /*
+ * ======== timeoutOccurred ========
+ */
+ bool timeoutOccurred(void);
+
+ /*
+ * ======== vector_cross ========
+ */
+ template<typename Ta, typename Tb, typename To>
+ static void vector_cross(const vector<Ta> *a, const vector<Tb> *b,
+ vector<To> *out);
+
+ /*
+ * ======== vector_dot ========
+ */
+ template<typename Ta, typename Tb>
+ static float vector_dot(const vector<Ta> *a, const vector<Tb> *b);
+
+ /*
+ * ======== vector_normalize ========
+ */
+ static void vector_normalize(vector<float> *a);
+
+private:
+ deviceType _device; // chip type (D20H, D20, or 4200D)
+ byte address;
+ unsigned int io_timeout;
+ bool did_timeout;
+};
+
+/*
+ * ======== vector_cross ========
+ */
+template<typename Ta, typename Tb, typename To>
+void L3G::vector_cross(const vector<Ta> *a, const vector<Tb> *b,
+ vector<To> *out)
+{
+ out->x = (a->y * b->z) - (a->z * b->y);
+ out->y = (a->z * b->x) - (a->x * b->z);
+ out->z = (a->x * b->y) - (a->y * b->x);
+}
+
+/*
+ * ======== vector_dot ========
+ */
+template<typename Ta, typename Tb>
+float L3G::vector_dot(const vector<Ta> *a, const vector<Tb> *b)
+{
+ return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
+}
+
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/LSM303.cpp b/src/Energia/libraries/ZumoCC3200/utility/LSM303.cpp
--- /dev/null
@@ -0,0 +1,479 @@
+#include <Wire.h>
+#include <math.h>
+
+#include "LSM303.h"
+
+#include <xdc/runtime/System.h>
+
+// Defines ////////////////////////////////////////////////////////////////
+
+// The Arduino two-wire interface uses a 7-bit number for the address,
+// and sets the last bit correctly based on reads and writes
+#define D_SA0_HIGH_ADDRESS 0b0011101
+#define D_SA0_LOW_ADDRESS 0b0011110
+#define DLHC_DLM_DLH_MAG_ADDRESS 0b0011110
+#define DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS 0b0011001
+#define DLM_DLH_ACC_SA0_LOW_ADDRESS 0b0011000
+
+#define TEST_REG_ERROR -1
+
+#define D_WHO_ID 0x49
+#define DLM_WHO_ID 0x3C
+
+static int testReg(byte address, LSM303::regAddr reg);
+
+// Constructors ///////////////////////////////////////////////////////////////
+
+LSM303::LSM303(void)
+{
+ /*
+ * These values lead to an assumed magnetometer bias of 0.
+ * Use the Calibrate example program to determine appropriate values
+ * for your particular unit. The Heading example demonstrates how to
+ * adjust these values in your own sketch.
+ */
+ m_min = (LSM303::vector<int16_t> ) { -32767, -32767, -32767 };
+ m_max = (LSM303::vector<int16_t> ) { +32767, +32767, +32767 };
+
+ _device = device_auto;
+
+ io_timeout = 0; // 0 = no timeout
+ did_timeout = false;
+}
+
+// Public Methods /////////////////////////////////////////////////////////////
+
+// Did a timeout occur in readAcc(), readMag(), or read() since the last call to timeoutOccurred()?
+bool LSM303::timeoutOccurred()
+{
+ bool tmp = did_timeout;
+ did_timeout = false;
+ return tmp;
+}
+
+void LSM303::setTimeout(unsigned int timeout)
+{
+ io_timeout = timeout;
+}
+
+unsigned int LSM303::getTimeout()
+{
+ return io_timeout;
+}
+
+bool LSM303::init(deviceType device, sa0State sa0)
+{
+ // perform auto-detection unless device type and SA0 state were both specified
+ if (device == device_auto || sa0 == sa0_auto) {
+ // check for LSM303D if device is unidentified or was specified to be this type
+ if (device == device_auto || device == device_D) {
+ // check SA0 high address unless SA0 was specified to be low
+ if (sa0
+ != sa0_low&& testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID) {
+ // device responds to address 0011101 with D ID; it's a D with SA0 high
+ device = device_D;
+ sa0 = sa0_high;
+ }
+ // check SA0 low address unless SA0 was specified to be high
+ else if (sa0
+ != sa0_high&& testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID) {
+ // device responds to address 0011110 with D ID; it's a D with SA0 low
+ device = device_D;
+ sa0 = sa0_low;
+ }
+ }
+
+ // check for LSM303DLHC, DLM, DLH if device is still unidentified or was specified to be one of these types
+ if (device == device_auto || device == device_DLHC
+ || device == device_DLM || device == device_DLH) {
+ // check SA0 high address unless SA0 was specified to be low
+ if (sa0
+ != sa0_low&& testReg(DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR) {
+ // device responds to address 0011001; it's a DLHC, DLM with SA0 high, or DLH with SA0 high
+ sa0 = sa0_high;
+ if (device == device_auto) {
+ // use magnetometer WHO_AM_I register to determine device type
+ //
+ // DLHC seems to respond to WHO_AM_I request the same way as DLM, even though this
+ // register isn't documented in its datasheet. Since the DLHC accelerometer address is the
+ // same as the DLM with SA0 high, but Pololu DLM boards pull SA0 low by default, we'll
+ // guess that a device whose accelerometer responds to the SA0 high address and whose
+ // magnetometer gives the DLM ID is actually a DLHC.
+ device =
+ (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M)
+ == DLM_WHO_ID) ? device_DLHC : device_DLH;
+ }
+ }
+ // check SA0 low address unless SA0 was specified to be high
+ else if (sa0
+ != sa0_high&& testReg(DLM_DLH_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR) {
+ // device responds to address 0011000; it's a DLM with SA0 low or DLH with SA0 low
+ sa0 = sa0_low;
+ if (device == device_auto) {
+ // use magnetometer WHO_AM_I register to determine device type
+ device =
+ (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M)
+ == DLM_WHO_ID) ? device_DLM : device_DLH;
+ }
+ }
+ }
+ /* make sure device and SA0 were successfully detected; otherwise, indicate failure */
+ if (device == device_auto || sa0 == sa0_auto) {
+ return false;
+ }
+ }
+ _device = device;
+ /* set device addresses and translated register addresses */
+ switch (device) {
+ case device_D:
+ acc_address = mag_address =
+ (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS;
+ translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;
+ translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;
+ translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;
+ translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;
+ translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;
+ translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;
+ break;
+ case device_DLHC:
+ acc_address = DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have configurable SA0 but uses same acc address as DLM/DLH with SA0 high
+ mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
+ translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M;
+ translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M;
+ translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M;
+ translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M;
+ translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M;
+ translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M;
+ break;
+ case device_DLM:
+ acc_address =
+ (sa0 == sa0_high) ?
+ DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS :
+ DLM_DLH_ACC_SA0_LOW_ADDRESS;
+ mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
+ translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M;
+ translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M;
+ translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M;
+ translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M;
+ translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M;
+ translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M;
+ break;
+ case device_DLH:
+ acc_address =
+ (sa0 == sa0_high) ?
+ DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS :
+ DLM_DLH_ACC_SA0_LOW_ADDRESS;
+ mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
+ translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M;
+ translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M;
+ translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M;
+ translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M;
+ translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M;
+ translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M;
+ break;
+ }
+ return true;
+}
+
+/*
+ Enables the LSM303's accelerometer and magnetometer. Also:
+ - Sets sensor full scales (gain) to default power-on values, which are
+ +/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer
+ (+/- 4 gauss on LSM303D).
+ - Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz
+ ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR
+ settings for which the electrical characteristics are specified in
+ the datasheets.)
+ - Enables high resolution modes (if available).
+ Note that this function will also reset other settings controlled by
+ the registers it writes to.
+ */
+void LSM303::enableDefault(void)
+{
+ if (_device == device_D) {
+ /* Accelerometer
+ * 0x00 = 0b00000000
+ * AFS = 0 (+/- 2 g full scale)
+ */
+ writeReg(CTRL2, 0x00);
+ /* 0x57 = 0b01010111
+ * AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
+ */
+ writeReg(CTRL1, 0x57);
+ /* Magnetometer
+ * 0x64 = 0b01100100
+ * M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
+ */
+ writeReg(CTRL5, 0x64);
+ /* 0x20 = 0b00100000
+ * MFS = 01 (+/- 4 gauss full scale)
+ */
+ writeReg(CTRL6, 0x20);
+ /* 0x00 = 0b00000000
+ * MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
+ */
+ writeReg(CTRL7, 0x00);
+ }
+ else {
+ /* Accelerometer */
+ if (_device == device_DLHC) {
+ /* 0x08 = 0b00001000
+ * FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable)
+ */
+ writeAccReg(CTRL_REG4_A, 0x08);
+ /* 0x47 = 0b01000111
+ * ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
+ */
+ writeAccReg(CTRL_REG1_A, 0x47);
+ }
+ else /* DLM, DLH */
+ {
+ /* 0x00 = 0b00000000
+ * FS = 00 (+/- 2 g full scale)
+ */
+ writeAccReg(CTRL_REG4_A, 0x00);
+ /* 0x27 = 0b00100111
+ * PM = 001 (normal mode); DR = 00 (50 Hz ODR); Zen = Yen = Xen = 1 (all axes enabled)
+ */
+ writeAccReg(CTRL_REG1_A, 0x27);
+ }
+ /* Magnetometer
+ * 0x0C = 0b00001100
+ * DO = 011 (7.5 Hz ODR)
+ */
+ writeMagReg(CRA_REG_M, 0x0C);
+ /* 0x20 = 0b00100000
+ * GN = 001 (+/- 1.3 gauss full scale)
+ */
+ writeMagReg(CRB_REG_M, 0x20);
+ /* 0x00 = 0b00000000
+ * MD = 00 (continuous-conversion mode)
+ */
+ writeMagReg(MR_REG_M, 0x00);
+ }
+}
+
+/* Writes an accelerometer register */
+void LSM303::writeAccReg(byte reg, byte value)
+{
+ Wire.beginTransmission(acc_address);
+ Wire.write(reg);
+ Wire.write(value);
+ last_status = Wire.endTransmission();
+}
+
+/* Reads an accelerometer register */
+byte LSM303::readAccReg(byte reg)
+{
+ byte value;
+ Wire.beginTransmission(acc_address);
+ Wire.write(reg);
+ last_status = Wire.endTransmission();
+ Wire.requestFrom(acc_address, (byte) 1);
+ value = Wire.read();
+ Wire.endTransmission();
+ return value;
+}
+
+/* Writes a magnetometer register */
+void LSM303::writeMagReg(byte reg, byte value)
+{
+ Wire.beginTransmission(mag_address);
+ Wire.write(reg);
+ Wire.write(value);
+ last_status = Wire.endTransmission();
+}
+
+/* Reads a magnetometer register */
+byte LSM303::readMagReg(int reg)
+{
+ byte value;
+ /* if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type) */
+ if (reg < 0) {
+ reg = translated_regs[-reg];
+ }
+ Wire.beginTransmission(mag_address);
+ Wire.write(reg);
+ last_status = Wire.endTransmission();
+ Wire.requestFrom(mag_address, (byte) 1);
+ value = Wire.read();
+ Wire.endTransmission();
+ return value;
+}
+
+void LSM303::writeReg(byte reg, byte value)
+{
+ /* mag address == acc_address for LSM303D, so it doesn't really matter which one we use. */
+ if (_device == device_D || reg < CTRL_REG1_A) {
+ writeMagReg(reg, value);
+ }
+ else {
+ writeAccReg(reg, value);
+ }
+}
+
+/* Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC.
+ * To read those two registers, use readMagReg() instead.
+ */
+byte LSM303::readReg(int reg)
+{
+ /* mag address == acc_address for LSM303D, so it doesn't really matter which one we use.
+ * Use readMagReg so it can translate OUT_[XYZ]_[HL]_M
+ */
+ if (_device == device_D || reg < CTRL_REG1_A) {
+ return readMagReg(reg);
+ }
+ else {
+ return readAccReg(reg);
+ }
+}
+
+/* Reads the 3 accelerometer channels and stores them in vector a */
+void LSM303::readAcc(void)
+{
+ Wire.beginTransmission(acc_address);
+ /* assert the MSB of the address to get the accelerometer
+ * to do slave-transmit subaddress updating.
+ */
+ Wire.write(OUT_X_L_A | (1 << 7));
+ last_status = Wire.endTransmission();
+ Wire.requestFrom(acc_address, (byte) 6);
+ unsigned int millis_start = millis();
+ while (Wire.available() < 6) {
+ if (io_timeout > 0
+ && ((unsigned int) millis() - millis_start) > io_timeout) {
+ did_timeout = true;
+ return;
+ }
+ }
+ byte xla = Wire.read();
+ byte xha = Wire.read();
+ byte yla = Wire.read();
+ byte yha = Wire.read();
+ byte zla = Wire.read();
+ byte zha = Wire.read();
+ /* combine high and low bytes
+ * This no longer drops the lowest 4 bits of the readings from the DLH/DLM/DLHC, which are always 0
+ * (12-bit resolution, left-aligned). The D has 16-bit resolution
+ */
+ a.x = (int16_t)(xha << 8 | xla);
+ a.y = (int16_t)(yha << 8 | yla);
+ a.z = (int16_t)(zha << 8 | zla);
+}
+
+// Reads the 3 magnetometer channels and stores them in vector m
+void LSM303::readMag(void)
+{
+ Wire.beginTransmission(mag_address);
+ /* If LSM303D, assert MSB to enable subaddress updating
+ * OUT_X_L_M comes first on D, OUT_X_H_M on others
+ */
+ Wire.write(
+ (_device == device_D) ?
+ translated_regs[-OUT_X_L_M] | (1 << 7) :
+ translated_regs[-OUT_X_H_M]);
+ last_status = Wire.endTransmission();
+ Wire.requestFrom(mag_address, (byte) 6);
+ unsigned int millis_start = millis();
+ while (Wire.available() < 6) {
+ if (io_timeout > 0
+ && ((unsigned int) millis() - millis_start) > io_timeout) {
+ did_timeout = true;
+ return;
+ }
+ }
+ byte xlm, xhm, ylm, yhm, zlm, zhm;
+ if (_device == device_D) {
+ /* D: X_L, X_H, Y_L, Y_H, Z_L, Z_H */
+ xlm = Wire.read();
+ xhm = Wire.read();
+ ylm = Wire.read();
+ yhm = Wire.read();
+ zlm = Wire.read();
+ zhm = Wire.read();
+ }
+ else {
+ /* DLHC, DLM, DLH: X_H, X_L... */
+ xhm = Wire.read();
+ xlm = Wire.read();
+
+ if (_device == device_DLH) {
+ /* DLH: ...Y_H, Y_L, Z_H, Z_L */
+ yhm = Wire.read();
+ ylm = Wire.read();
+ zhm = Wire.read();
+ zlm = Wire.read();
+ }
+ else {
+ /* DLM, DLHC: ...Z_H, Z_L, Y_H, Y_L */
+ zhm = Wire.read();
+ zlm = Wire.read();
+ yhm = Wire.read();
+ ylm = Wire.read();
+ }
+ }
+ /* combine high and low bytes */
+ m.x = (int16_t)(xhm << 8 | xlm);
+ m.y = (int16_t)(yhm << 8 | ylm);
+ m.z = (int16_t)(zhm << 8 | zlm);
+}
+
+// Reads all 6 channels of the LSM303 and stores them in the object variables
+void LSM303::read(void)
+{
+ readAcc();
+ readMag();
+}
+
+/*
+ * Returns the angular difference in the horizontal plane between a
+ * default vector and north, in degrees.
+
+ * The default vector here is chosen to point along the surface of the
+ * PCB, in the direction of the top of the text on the silkscreen.
+ * This is the +X axis on the Pololu LSM303D carrier and the -Y axis on
+ * the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH carriers.
+ */
+float LSM303::heading(void)
+{
+ if (_device == device_D) {
+ vector<int> north = {1, 0, 0};
+ return heading(north);
+ }
+ else {
+ vector<int> north = { 0, -1, 0 };
+ return heading(north);
+ }
+}
+
+void LSM303::vector_normalize(vector<float> *a)
+{
+ float mag = sqrt(vector_dot(a, a));
+ a->x /= mag;
+ a->y /= mag;
+ a->z /= mag;
+}
+
+// Private Methods ////////////////////////////////////////////////////////////
+
+static int testReg(byte address, LSM303::regAddr reg)
+{
+ System_printf("testReg: 0x%x : 0x%x ...", address, reg);
+ Wire.beginTransmission(address);
+ System_printf(" begin ...");
+ Wire.write((byte) reg);
+ if (Wire.endTransmission() != 0) {
+ System_printf("testReg: error\n");
+ return TEST_REG_ERROR;
+ }
+ System_printf(" request from 0x%x...", address);
+ Wire.requestFrom(address, (byte) 1);
+ System_printf(" read ...");
+ if (Wire.available()) {
+ return Wire.read();
+ }
+ else {
+ System_printf("testReg: error\n");
+ return TEST_REG_ERROR;
+ }
+}
diff --git a/src/Energia/libraries/ZumoCC3200/utility/LSM303.h b/src/Energia/libraries/ZumoCC3200/utility/LSM303.h
--- /dev/null
@@ -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 <Arduino.h> // for byte data type
+
+class LSM303
+{
+ public:
+ template <typename T> struct vector {
+ T x, y, z;
+ };
+
+ enum deviceType { device_DLH, device_DLM, device_DLHC, device_D, device_auto };
+ enum sa0State { sa0_low, sa0_high, sa0_auto };
+
+ // register addresses
+ enum regAddr {
+ TEMP_OUT_L = 0x05, // D
+ TEMP_OUT_H = 0x06, // D
+
+ STATUS_M = 0x07, // D
+
+ INT_CTRL_M = 0x12, // D
+ INT_SRC_M = 0x13, // D
+ INT_THS_L_M = 0x14, // D
+ INT_THS_H_M = 0x15, // D
+
+ OFFSET_X_L_M = 0x16, // D
+ OFFSET_X_H_M = 0x17, // D
+ OFFSET_Y_L_M = 0x18, // D
+ OFFSET_Y_H_M = 0x19, // D
+ OFFSET_Z_L_M = 0x1A, // D
+ OFFSET_Z_H_M = 0x1B, // D
+ REFERENCE_X = 0x1C, // D
+ REFERENCE_Y = 0x1D, // D
+ REFERENCE_Z = 0x1E, // D
+
+ CTRL0 = 0x1F, // D
+ CTRL1 = 0x20, // D
+ CTRL_REG1_A = 0x20, // DLH, DLM, DLHC
+ CTRL2 = 0x21, // D
+ CTRL_REG2_A = 0x21, // DLH, DLM, DLHC
+ CTRL3 = 0x22, // D
+ CTRL_REG3_A = 0x22, // DLH, DLM, DLHC
+ CTRL4 = 0x23, // D
+ CTRL_REG4_A = 0x23, // DLH, DLM, DLHC
+ CTRL5 = 0x24, // D
+ CTRL_REG5_A = 0x24, // DLH, DLM, DLHC
+ CTRL6 = 0x25, // D
+ CTRL_REG6_A = 0x25, // DLHC
+ HP_FILTER_RESET_A = 0x25, // DLH, DLM
+ CTRL7 = 0x26, // D
+ REFERENCE_A = 0x26, // DLH, DLM, DLHC
+ STATUS_A = 0x27, // D
+ STATUS_REG_A = 0x27, // DLH, DLM, DLHC
+
+ OUT_X_L_A = 0x28,
+ OUT_X_H_A = 0x29,
+ OUT_Y_L_A = 0x2A,
+ OUT_Y_H_A = 0x2B,
+ OUT_Z_L_A = 0x2C,
+ OUT_Z_H_A = 0x2D,
+
+ FIFO_CTRL = 0x2E, // D
+ FIFO_CTRL_REG_A = 0x2E, // DLHC
+ FIFO_SRC = 0x2F, // D
+ FIFO_SRC_REG_A = 0x2F, // DLHC
+
+ IG_CFG1 = 0x30, // D
+ INT1_CFG_A = 0x30, // DLH, DLM, DLHC
+ IG_SRC1 = 0x31, // D
+ INT1_SRC_A = 0x31, // DLH, DLM, DLHC
+ IG_THS1 = 0x32, // D
+ INT1_THS_A = 0x32, // DLH, DLM, DLHC
+ IG_DUR1 = 0x33, // D
+ INT1_DURATION_A = 0x33, // DLH, DLM, DLHC
+ IG_CFG2 = 0x34, // D
+ INT2_CFG_A = 0x34, // DLH, DLM, DLHC
+ IG_SRC2 = 0x35, // D
+ INT2_SRC_A = 0x35, // DLH, DLM, DLHC
+ IG_THS2 = 0x36, // D
+ INT2_THS_A = 0x36, // DLH, DLM, DLHC
+ IG_DUR2 = 0x37, // D
+ INT2_DURATION_A = 0x37, // DLH, DLM, DLHC
+
+ CLICK_CFG = 0x38, // D
+ CLICK_CFG_A = 0x38, // DLHC
+ CLICK_SRC = 0x39, // D
+ CLICK_SRC_A = 0x39, // DLHC
+ CLICK_THS = 0x3A, // D
+ CLICK_THS_A = 0x3A, // DLHC
+ TIME_LIMIT = 0x3B, // D
+ TIME_LIMIT_A = 0x3B, // DLHC
+ TIME_LATENCY = 0x3C, // D
+ TIME_LATENCY_A = 0x3C, // DLHC
+ TIME_WINDOW = 0x3D, // D
+ TIME_WINDOW_A = 0x3D, // DLHC
+
+ Act_THS = 0x3E, // D
+ Act_DUR = 0x3F, // D
+
+ CRA_REG_M = 0x00, // DLH, DLM, DLHC
+ CRB_REG_M = 0x01, // DLH, DLM, DLHC
+ MR_REG_M = 0x02, // DLH, DLM, DLHC
+
+ SR_REG_M = 0x09, // DLH, DLM, DLHC
+ IRA_REG_M = 0x0A, // DLH, DLM, DLHC
+ IRB_REG_M = 0x0B, // DLH, DLM, DLHC
+ IRC_REG_M = 0x0C, // DLH, DLM, DLHC
+
+ WHO_AM_I = 0x0F, // D
+ WHO_AM_I_M = 0x0F, // DLM
+
+ TEMP_OUT_H_M = 0x31, // DLHC
+ TEMP_OUT_L_M = 0x32, // DLHC
+
+ // dummy addresses for registers in different locations on different
+ // devices; the library translates these based on device type value
+ // with sign flipped is used as index into translated_regs array
+
+ OUT_X_H_M = -1,
+ OUT_X_L_M = -2,
+ OUT_Y_H_M = -3,
+ OUT_Y_L_M = -4,
+ OUT_Z_H_M = -5,
+ OUT_Z_L_M = -6,
+ // update dummy_reg_count if registers are added here!
+
+ // device-specific register addresses
+ DLH_OUT_X_H_M = 0x03,
+ DLH_OUT_X_L_M = 0x04,
+ DLH_OUT_Y_H_M = 0x05,
+ DLH_OUT_Y_L_M = 0x06,
+ DLH_OUT_Z_H_M = 0x07,
+ DLH_OUT_Z_L_M = 0x08,
+
+ DLM_OUT_X_H_M = 0x03,
+ DLM_OUT_X_L_M = 0x04,
+ DLM_OUT_Z_H_M = 0x05,
+ DLM_OUT_Z_L_M = 0x06,
+ DLM_OUT_Y_H_M = 0x07,
+ DLM_OUT_Y_L_M = 0x08,
+
+ DLHC_OUT_X_H_M = 0x03,
+ DLHC_OUT_X_L_M = 0x04,
+ DLHC_OUT_Z_H_M = 0x05,
+ DLHC_OUT_Z_L_M = 0x06,
+ DLHC_OUT_Y_H_M = 0x07,
+ DLHC_OUT_Y_L_M = 0x08,
+
+ D_OUT_X_L_M = 0x08,
+ D_OUT_X_H_M = 0x09,
+ D_OUT_Y_L_M = 0x0A,
+ D_OUT_Y_H_M = 0x0B,
+ D_OUT_Z_L_M = 0x0C,
+ D_OUT_Z_H_M = 0x0D
+ };
+
+ vector<int16_t> a; // accelerometer readings
+ vector<int16_t> m; // magnetometer readings
+ vector<int16_t> m_max; // maximum magnetometer values, used for calibration
+ vector<int16_t> m_min; // minimum magnetometer values, used for calibration
+
+ /*
+ * ======== last_status ========
+ */
+ byte last_status; // status of last I2C transmission
+
+ /*
+ * ======== LSM303 ========
+ */
+ LSM303(void);
+
+ /*
+ * ======== init ========
+ */
+ bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
+
+ /*
+ * ======== getDeviceType ========
+ */
+ deviceType getDeviceType(void) { return _device; }
+
+ /*
+ * ======== enableDefault ========
+ */
+ void enableDefault(void);
+
+ /*
+ * ======== writeAccReg ========
+ */
+ void writeAccReg(byte reg, byte value);
+
+ /*
+ * ======== readAccReg ========
+ */
+ byte readAccReg(byte reg);
+
+ /*
+ * ======== writeMagReg ========
+ */
+ void writeMagReg(byte reg, byte value);
+
+ /*
+ * ======== readMagReg ========
+ */
+ byte readMagReg(int reg);
+
+ /*
+ * ======== writeReg ========
+ */
+ void writeReg(byte reg, byte value);
+
+ /*
+ * ======== readReg ========
+ */
+ byte readReg(int reg);
+
+ /*
+ * ======== readAcc ========
+ */
+ void readAcc(void);
+
+ /*
+ * ======== readMag ========
+ */
+ void readMag(void);
+
+ /*
+ * ======== read ========
+ */
+ void read(void);
+
+ /*
+ * ======== setTimeout ========
+ */
+ void setTimeout(unsigned int timeout);
+
+ /*
+ * ======== getTimeout ========
+ */
+ unsigned int getTimeout(void);
+
+ /*
+ * ======== timeoutOccurred ========
+ */
+ bool timeoutOccurred(void);
+
+ /*
+ * ======== heading ========
+ */
+ float heading(void);
+
+ template <typename T> float heading(vector<T> from);
+
+ /*
+ * ======== vector_cross ========
+ */
+ template <typename Ta, typename Tb, typename To>
+ static void vector_cross(
+ const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);
+
+ /*
+ * ======== vector_dot ========
+ */
+ template <typename Ta, typename Tb>
+ static float vector_dot(const vector<Ta> *a, const vector<Tb> *b);
+
+ /*
+ * ======== vector_normalize ========
+ */
+ static void vector_normalize(vector<float> *a);
+
+ private:
+ deviceType _device; // chip type (D, DLHC, DLM, or DLH)
+ byte acc_address;
+ byte mag_address;
+ static const int dummy_reg_count = 6;
+ regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used
+ unsigned int io_timeout;
+ bool did_timeout;
+};
+
+/*
+ * ======== heading ========
+ * Returns the angular difference in the horizontal plane between the
+ * "from" vector and north, in degrees.
+ *
+ * Description of heading algorithm:
+ * Shift and scale the magnetic reading based on calibration data to find
+ * the North vector. Use the acceleration readings to determine the Up
+ * vector (gravity is measured as an upward acceleration). The cross
+ * product of North and Up vectors is East. The vectors East and North
+ * form a basis for the horizontal plane. The From vector is projected
+ * into the horizontal plane and the angle between the projected vector
+ * and horizontal north is returned.
+ */
+template <typename T> float LSM303::heading(vector<T> from)
+{
+ vector<int32_t> temp_m = {m.x, m.y, m.z};
+
+ /* subtract offset (average of min and max) from magnetometer readings */
+ temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
+ temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
+ temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
+
+ /* compute E and N */
+ vector<float> E;
+ vector<float> N;
+
+ vector_cross(&temp_m, &a, &E);
+ vector_normalize(&E);
+ vector_cross(&a, &E, &N);
+ vector_normalize(&N);
+
+ /* compute heading */
+ float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / 3.14159265358979323846;
+ if (heading < 0) {
+ heading += 360;
+ }
+ return heading;
+}
+
+/*
+ * ======== vector_cross ========
+ * To = Ta x Tb
+ */
+template <typename Ta, typename Tb, typename To>
+ void LSM303::vector_cross(
+ const vector<Ta> *a, const vector<Tb> *b, vector<To> *out)
+{
+ out->x = (a->y * b->z) - (a->z * b->y);
+ out->y = (a->z * b->x) - (a->x * b->z);
+ out->z = (a->x * b->y) - (a->y * b->x);
+}
+
+/*
+ * ======== vector_dot ========
+ * Returns Ta * Tb
+ */
+template <typename Ta, typename Tb>
+ float LSM303::vector_dot(const vector<Ta> *a, const vector<Tb> *b)
+{
+ return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
+}
+
+#endif
diff --git a/src/Energia/libraries/ZumoCC3200/utility/PIDController.cpp b/src/Energia/libraries/ZumoCC3200/utility/PIDController.cpp
--- /dev/null
@@ -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
--- /dev/null
@@ -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 <Energia.h>
+
+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
--- /dev/null
@@ -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
--- /dev/null
@@ -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 <Arduino.h>
+
+#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
--- /dev/null
@@ -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
--- /dev/null
@@ -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
--- /dev/null
@@ -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 <math.h>
+
+#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
--- /dev/null
@@ -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
--- /dev/null
@@ -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 <Energia.h>
+#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
--- /dev/null
@@ -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
--- /dev/null
@@ -0,0 +1,138 @@
+#include <Energia.h>
+
+#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
--- /dev/null
@@ -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 <Arduino.h> // 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
--- /dev/null
@@ -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 <http://www.gnu.org/licenses/>.
+ * ---------------------------------------------------------------------
+ *
+ */
+
+#ifndef TK_SPLINE_H
+#define TK_SPLINE_H
+
+#include <cstdio>
+#include <cassert>
+#include <vector>
+#include <algorithm>
+
+
+// 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<double> > m_upper; // upper band
+ std::vector< std::vector<double> > 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<double> r_solve(const std::vector<double>& b) const;
+ std::vector<double> l_solve(const std::vector<double>& b) const;
+ std::vector<double> lu_solve(const std::vector<double>& b,
+ bool is_lu_decomposed=false);
+
+};
+
+
+// spline interpolation
+class spline
+{
+public:
+ enum bd_type {
+ first_deriv = 1,
+ second_deriv = 2
+ };
+
+private:
+ std::vector<double> 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<double> 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<double>& x,
+ const std::vector<double>& 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; i<m_upper.size(); i++) {
+ m_upper[i].resize(dim);
+ }
+ for(size_t i=0; i<m_lower.size(); i++) {
+ m_lower[i].resize(dim);
+ }
+}
+int band_matrix::dim() const
+{
+ if(m_upper.size()>0) {
+ 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<dim()) && (j>=0) && (j<dim()) );
+ assert( (-num_lower()<=k) && (k<=num_upper()) );
+ // k=0 -> 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<dim()) && (j>=0) && (j<dim()) );
+ assert( (-num_lower()<=k) && (k<=num_upper()) );
+ // k=0 -> 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<dim()) );
+ return m_lower[0][i];
+}
+double & band_matrix::saved_diag(int i)
+{
+ assert( (i>=0) && (i<dim()) );
+ return m_lower[0][i];
+}
+
+// LR-Decomposition of a band matrix
+void band_matrix::lu_decompose()
+{
+ int i_max,j_max;
+ int j_min;
+ double x;
+
+ // preconditioning
+ // normalize column i so that a_ii=1
+ for(int i=0; i<this->dim(); 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; k<this->dim(); 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<double> band_matrix::l_solve(const std::vector<double>& b) const
+{
+ assert( this->dim()==(int)b.size() );
+ std::vector<double> x(this->dim());
+ int j_start;
+ double sum;
+ for(int i=0; i<this->dim(); i++) {
+ sum=0;
+ j_start=std::max(0,i-this->num_lower());
+ for(int j=j_start; j<i; j++) sum += this->operator()(i,j)*x[j];
+ x[i]=(b[i]*this->saved_diag(i)) - sum;
+ }
+ return x;
+}
+// solves Rx=y
+std::vector<double> band_matrix::r_solve(const std::vector<double>& b) const
+{
+ assert( this->dim()==(int)b.size() );
+ std::vector<double> 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<double> band_matrix::lu_solve(const std::vector<double>& b,
+ bool is_lu_decomposed)
+{
+ assert( this->dim()==(int)b.size() );
+ std::vector<double> 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<double>& x,
+ const std::vector<double>& 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<n-1; i++) {
+ assert(m_x[i]<m_x[i+1]);
+ }
+
+ if(cubic_spline==true) { // cubic spline interpolation
+ // setting up the matrix and right hand side of the equation system
+ // for the parameters b[]
+ band_matrix A(n,1,1);
+ std::vector<double> rhs(n);
+ for(int i=1; i<n-1; i++) {
+ A(i,i-1)=1.0/3.0*(x[i]-x[i-1]);
+ A(i,i)=2.0/3.0*(x[i+1]-x[i-1]);
+ A(i,i+1)=1.0/3.0*(x[i+1]-x[i]);
+ rhs[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
+ }
+ // boundary conditions
+ if(m_left == spline::second_deriv) {
+ // 2*b[0] = f''
+ A(0,0)=2.0;
+ A(0,1)=0.0;
+ rhs[0]=m_left_value;
+ } else if(m_left == spline::first_deriv) {
+ // c[0] = f', needs to be re-expressed in terms of b:
+ // (2b[0]+b[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f')
+ A(0,0)=2.0*(x[1]-x[0]);
+ A(0,1)=1.0*(x[1]-x[0]);
+ rhs[0]=3.0*((y[1]-y[0])/(x[1]-x[0])-m_left_value);
+ } else {
+ assert(false);
+ }
+ if(m_right == spline::second_deriv) {
+ // 2*b[n-1] = f''
+ A(n-1,n-1)=2.0;
+ A(n-1,n-2)=0.0;
+ rhs[n-1]=m_right_value;
+ } else if(m_right == spline::first_deriv) {
+ // c[n-1] = f', needs to be re-expressed in terms of b:
+ // (b[n-2]+2b[n-1])(x[n-1]-x[n-2])
+ // = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]))
+ A(n-1,n-1)=2.0*(x[n-1]-x[n-2]);
+ A(n-1,n-2)=1.0*(x[n-1]-x[n-2]);
+ rhs[n-1]=3.0*(m_right_value-(y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
+ } else {
+ assert(false);
+ }
+
+ // solve the equation system to obtain the parameters b[]
+ m_b=A.lu_solve(rhs);
+
+ // calculate parameters a[] and c[] based on b[]
+ m_a.resize(n);
+ m_c.resize(n);
+ for(int i=0; i<n-1; i++) {
+ m_a[i]=1.0/3.0*(m_b[i+1]-m_b[i])/(x[i+1]-x[i]);
+ m_c[i]=(y[i+1]-y[i])/(x[i+1]-x[i])
+ - 1.0/3.0*(2.0*m_b[i]+m_b[i+1])*(x[i+1]-x[i]);
+ }
+ } else { // linear interpolation
+ m_a.resize(n);
+ m_b.resize(n);
+ m_c.resize(n);
+ for(int i=0; i<n-1; i++) {
+ m_a[i]=0.0;
+ m_b[i]=0.0;
+ m_c[i]=(m_y[i+1]-m_y[i])/(m_x[i+1]-m_x[i]);
+ }
+ }
+
+ // for left extrapolation coefficients
+ m_b0 = (m_force_linear_extrapolation==false) ? m_b[0] : 0.0;
+ m_c0 = m_c[0];
+
+ // for the right extrapolation coefficients
+ // f_{n-1}(x) = b*(x-x_{n-1})^2 + c*(x-x_{n-1}) + y_{n-1}
+ double h=x[n-1]-x[n-2];
+ // m_b[n-1] is determined by the boundary condition
+ m_a[n-1]=0.0;
+ m_c[n-1]=3.0*m_a[n-2]*h*h+2.0*m_b[n-2]*h+m_c[n-2]; // = f'_{n-2}(x_{n-1})
+ if(m_force_linear_extrapolation==true)
+ m_b[n-1]=0.0;
+}
+
+double spline::operator() (double x) const
+{
+ size_t n=m_x.size();
+ // find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
+ std::vector<double>::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(x<m_x[0]) {
+ // extrapolation to the left
+ interpol=(m_b0*h + m_c0)*h + m_y[0];
+ } else if(x>m_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
--- /dev/null
@@ -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
--- /dev/null
@@ -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<a.length; x++){ // of the array, adjust them
+ rect(int(xPos+x*float(Width)/(a.length)),
+ yPos+Height-2,
+ Width/a.length-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ else {
+ for (int x=from; x<to; x++){
+
+ rect(int(xPos+(x-from)*float(Width)/(to-from)),
+ yPos+Height-2,
+ Width/(to-from)-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ }
+
+ /* =========================================================================================
+ Dot graph
+ ========================================================================================== */
+
+ void DotGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<x.length; i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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
+ );
+ }
+ }
+
+ /* =========================================================================================
+ Straight line graph
+ ========================================================================================== */
+
+ void LineGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<(x.length-1); i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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);
+ }
+ }
+
+ /* =========================================================================================
+ smoothLine
+ ========================================================================================== */
+
+ void smoothLine(float[] x ,float[] y) {
+
+ float tempyMax=yMax, tempyMin=yMin;
+
+ if(RightAxis){yMax=yMaxRight;yMin=yMinRight;}
+
+ int counter=0;
+ int xlocation=0,ylocation=0;
+
+ // if(!ErrorFlag |true ){ // sort out later!
+
+ beginShape(); strokeWeight(2);stroke(GraphColor);noFill();smooth();
+
+ for (int i=0; i<x.length; i++){
+
+ /* ===========================================================================
+ Check for errors-> Make sure time array doesn't decrease (go back in time)
+ ===========================================================================*/
+ if(i<x.length-1){
+ if(x[i]>x[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(mouseX<xPos)xlocation=xPos;
+ if(mouseX>xPos+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(mouseY<yPos)ylocation=yPos;
+ if(mouseY>yPos+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
--- /dev/null
@@ -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
--- /dev/null
@@ -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
--- /dev/null
@@ -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<a.length; x++){ // of the array, adjust them
+ rect(int(xPos+x*float(Width)/(a.length)),
+ yPos+Height-2,
+ Width/a.length-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ else {
+ for (int x=from; x<to; x++){
+
+ rect(int(xPos+(x-from)*float(Width)/(to-from)),
+ yPos+Height-2,
+ Width/(to-from)-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ }
+
+ /* =========================================================================================
+ Dot graph
+ ========================================================================================== */
+
+ void DotGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<x.length; i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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
+ );
+ }
+ }
+
+ /* =========================================================================================
+ Straight line graph
+ ========================================================================================== */
+
+ void LineGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<(x.length-1); i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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);
+ }
+ }
+
+ /* =========================================================================================
+ smoothLine
+ ========================================================================================== */
+
+ void smoothLine(float[] x ,float[] y) {
+
+ float tempyMax=yMax, tempyMin=yMin;
+
+ if(RightAxis){yMax=yMaxRight;yMin=yMinRight;}
+
+ int counter=0;
+ int xlocation=0,ylocation=0;
+
+ // if(!ErrorFlag |true ){ // sort out later!
+
+ beginShape(); strokeWeight(2);stroke(GraphColor);noFill();smooth();
+
+ for (int i=0; i<x.length; i++){
+
+ /* ===========================================================================
+ Check for errors-> Make sure time array doesn't decrease (go back in time)
+ ===========================================================================*/
+ if(i<x.length-1){
+ if(x[i]>x[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(mouseX<xPos)xlocation=xPos;
+ if(mouseX>xPos+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(mouseY<yPos)ylocation=yPos;
+ if(mouseY>yPos+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
--- /dev/null
@@ -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
--- /dev/null
@@ -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<a.length; x++){ // of the array, adjust them
+ rect(int(xPos+x*float(Width)/(a.length)),
+ yPos+Height-2,
+ Width/a.length-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ else {
+ for (int x=from; x<to; x++){
+
+ rect(int(xPos+(x-from)*float(Width)/(to-from)),
+ yPos+Height-2,
+ Width/(to-from)-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ }
+
+ /* =========================================================================================
+ Dot graph
+ ========================================================================================== */
+
+ void DotGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<x.length; i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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
+ );
+ }
+ }
+
+ /* =========================================================================================
+ Straight line graph
+ ========================================================================================== */
+
+ void LineGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<(x.length-1); i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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);
+ }
+ }
+
+ /* =========================================================================================
+ smoothLine
+ ========================================================================================== */
+
+ void smoothLine(float[] x ,float[] y) {
+
+ float tempyMax=yMax, tempyMin=yMin;
+
+ if(RightAxis){yMax=yMaxRight;yMin=yMinRight;}
+
+ int counter=0;
+ int xlocation=0,ylocation=0;
+
+ // if(!ErrorFlag |true ){ // sort out later!
+
+ beginShape(); strokeWeight(2);stroke(GraphColor);noFill();smooth();
+
+ for (int i=0; i<x.length; i++){
+
+ /* ===========================================================================
+ Check for errors-> Make sure time array doesn't decrease (go back in time)
+ ===========================================================================*/
+ if(i<x.length-1){
+ if(x[i]>x[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(mouseX<xPos)xlocation=xPos;
+ if(mouseX>xPos+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(mouseY<yPos)ylocation=yPos;
+ if(mouseY>yPos+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
--- /dev/null
@@ -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
--- /dev/null
@@ -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<a.length; x++){ // of the array, adjust them
+ rect(int(xPos+x*float(Width)/(a.length)),
+ yPos+Height-2,
+ Width/a.length-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ else {
+ for (int x=from; x<to; x++){
+
+ rect(int(xPos+(x-from)*float(Width)/(to-from)),
+ yPos+Height-2,
+ Width/(to-from)-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ }
+
+ /* =========================================================================================
+ Dot graph
+ ========================================================================================== */
+
+ void DotGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<x.length; i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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
+ );
+ }
+ }
+
+ /* =========================================================================================
+ Straight line graph
+ ========================================================================================== */
+
+ void LineGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<(x.length-1); i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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);
+ }
+ }
+
+ /* =========================================================================================
+ smoothLine
+ ========================================================================================== */
+
+ void smoothLine(float[] x ,float[] y) {
+
+ float tempyMax=yMax, tempyMin=yMin;
+
+ if(RightAxis){yMax=yMaxRight;yMin=yMinRight;}
+
+ int counter=0;
+ int xlocation=0,ylocation=0;
+
+ // if(!ErrorFlag |true ){ // sort out later!
+
+ beginShape(); strokeWeight(2);stroke(GraphColor);noFill();smooth();
+
+ for (int i=0; i<x.length; i++){
+
+ /* ===========================================================================
+ Check for errors-> Make sure time array doesn't decrease (go back in time)
+ ===========================================================================*/
+ if(i<x.length-1){
+ if(x[i]>x[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(mouseX<xPos)xlocation=xPos;
+ if(mouseX>xPos+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(mouseY<yPos)ylocation=yPos;
+ if(mouseY>yPos+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
--- /dev/null
@@ -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
--- /dev/null
@@ -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<Point> pointList = new ArrayList<Point>();
+
+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<pointList.size(); i++) {
+ if(i==0 || i==(pointList.size()-1)){
+ curveVertex((float) pointList.get(i).getX(), (float) pointList.get(i).getY());
+ curveVertex((float) pointList.get(i).getX(), (float) pointList.get(i).getY());
+ System.out.println("x: "+pointList.get(i).getX()+" y: "+(height-pointList.get(i).getY()));
+ }
+ else{
+ curveVertex((float) pointList.get(i).getX(), (float) pointList.get(i).getY());
+ System.out.println("x: "+pointList.get(i).getX()+" y: "+(height-pointList.get(i).getY()));
+ }
+ }
+ }
+ endShape();
+
+ noStroke();
+ for (int i=0; i<pointList.size(); i++)
+ {
+ if(i==0){
+ fill(0, 0, 255);
+ ellipse((float) pointList.get(i).getX(), (float) pointList.get(i).getY(), 10, 10);
+ }
+ else{
+ fill(255, 0, 0);
+ ellipse((float) pointList.get(i).getX(), (float) pointList.get(i).getY(), 5, 5);
+ }
+ }
+
+ System.out.println("drawing");
+}
+
+/* save mouse-click position as new point to draw */
+void mousePressed()
+{
+ pointList.add(new Point(mouseX, mouseY));
+ redraw();
+}
+
+void keyPressed()
+{
+ /* clear the points */
+ if(key == 'x'){
+ System.out.println("points cleared");
+ pointList.clear();
+ redraw();
+ }
+ /* send each coordinate as a null terminated string to the zumo */
+ else if(key == 's'){
+ System.out.println("sending data");
+ for(Point p: pointList){
+ /* prefix each point with a 'p' character */
+ client.write('p');
+ client.write(Integer.toString((int) p.getX()));
+ client.write('\0');
+
+ delay(300);
+
+ client.write('p');
+ client.write(Integer.toString((int) (height-p.getY())));
+ client.write('\0');
+
+ delay(300);
+ }
+ client.write('\n');
+ }
+}
+
+void delay(int t)
+{
+ int time = millis();
+ while(millis() - time <= t);
+}
+
+
diff --git a/src/Processing/UDPBroadcast/GraphClass.pde b/src/Processing/UDPBroadcast/GraphClass.pde
--- /dev/null
@@ -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<a.length; x++){ // of the array, adjust them
+ rect(int(xPos+x*float(Width)/(a.length)),
+ yPos+Height-2,
+ Width/a.length-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ else {
+ for (int x=from; x<to; x++){
+
+ rect(int(xPos+(x-from)*float(Width)/(to-from)),
+ yPos+Height-2,
+ Width/(to-from)-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ }
+
+ /* =========================================================================================
+ Dot graph
+ ========================================================================================== */
+
+ void DotGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<x.length; i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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
+ );
+ }
+ }
+
+ /* =========================================================================================
+ Straight line graph
+ ========================================================================================== */
+
+ void LineGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<(x.length-1); i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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);
+ }
+ }
+
+ /* =========================================================================================
+ smoothLine
+ ========================================================================================== */
+
+ void smoothLine(float[] x ,float[] y) {
+
+ float tempyMax=yMax, tempyMin=yMin;
+
+ if(RightAxis){yMax=yMaxRight;yMin=yMinRight;}
+
+ int counter=0;
+ int xlocation=0,ylocation=0;
+
+ // if(!ErrorFlag |true ){ // sort out later!
+
+ beginShape(); strokeWeight(2);stroke(GraphColor);noFill();smooth();
+
+ for (int i=0; i<x.length; i++){
+
+ /* ===========================================================================
+ Check for errors-> Make sure time array doesn't decrease (go back in time)
+ ===========================================================================*/
+ if(i<x.length-1){
+ if(x[i]>x[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(mouseX<xPos)xlocation=xPos;
+ if(mouseX>xPos+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(mouseY<yPos)ylocation=yPos;
+ if(mouseY>yPos+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
--- /dev/null
@@ -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
--- /dev/null
@@ -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<a.length; x++){ // of the array, adjust them
+ rect(int(xPos+x*float(Width)/(a.length)),
+ yPos+Height-2,
+ Width/a.length-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ else {
+ for (int x=from; x<to; x++){
+
+ rect(int(xPos+(x-from)*float(Width)/(to-from)),
+ yPos+Height-2,
+ Width/(to-from)-2,
+ -a[x]/(yMax-yMin)*Height);
+ }
+ }
+ }
+
+ /* =========================================================================================
+ Dot graph
+ ========================================================================================== */
+
+ void DotGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<x.length; i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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
+ );
+ }
+ }
+
+ /* =========================================================================================
+ Straight line graph
+ ========================================================================================== */
+
+ void LineGraph(float[] x ,float[] y) {
+
+ for (int i=0; i<(x.length-1); i++){
+ strokeWeight(2);stroke(GraphColor);noFill();smooth();
+ 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);
+ }
+ }
+
+ /* =========================================================================================
+ smoothLine
+ ========================================================================================== */
+
+ void smoothLine(float[] x ,float[] y) {
+
+ float tempyMax=yMax, tempyMin=yMin;
+
+ if(RightAxis){yMax=yMaxRight;yMin=yMinRight;}
+
+ int counter=0;
+ int xlocation=0,ylocation=0;
+
+ // if(!ErrorFlag |true ){ // sort out later!
+
+ beginShape(); strokeWeight(2);stroke(GraphColor);noFill();smooth();
+
+ for (int i=0; i<x.length; i++){
+
+ /* ===========================================================================
+ Check for errors-> Make sure time array doesn't decrease (go back in time)
+ ===========================================================================*/
+ if(i<x.length-1){
+ if(x[i]>x[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(mouseX<xPos)xlocation=xPos;
+ if(mouseX>xPos+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(mouseY<yPos)ylocation=yPos;
+ if(mouseY>yPos+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
--- /dev/null
@@ -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, " ");
+
+}