diff --git a/hardware/datasheets/doc2505.pdf b/hardware/datasheets/doc2505.pdf
new file mode 100644
index 0000000..6c26b0e
Binary files /dev/null and b/hardware/datasheets/doc2505.pdf differ
diff --git a/software/apps/Motion/Graph2.maxpat b/software/apps/Motion/Graph2.maxpat
new file mode 100644
index 0000000..6644143
--- /dev/null
+++ b/software/apps/Motion/Graph2.maxpat
@@ -0,0 +1,1460 @@
+{
+ "patcher" : {
+ "fileversion" : 1,
+ "appversion" : {
+ "major" : 6,
+ "minor" : 0,
+ "revision" : 5
+ }
+,
+ "rect" : [ 122.0, 49.0, 1158.0, 706.0 ],
+ "bglocked" : 0,
+ "openinpresentation" : 0,
+ "default_fontsize" : 12.0,
+ "default_fontface" : 0,
+ "default_fontname" : "Arial",
+ "gridonopen" : 0,
+ "gridsize" : [ 15.0, 15.0 ],
+ "gridsnaponopen" : 0,
+ "statusbarvisible" : 2,
+ "toolbarvisible" : 1,
+ "boxanimatetime" : 200,
+ "imprint" : 0,
+ "enablehscroll" : 1,
+ "enablevscroll" : 1,
+ "devicewidth" : 0.0,
+ "description" : "",
+ "digest" : "",
+ "tags" : "",
+ "boxes" : [ {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-46",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 134.0, 690.0, 37.0, 21.0 ],
+ "text" : "v"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-45",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 134.0, 591.0, 37.0, 21.0 ],
+ "text" : "x"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-42",
+ "maxclass" : "number",
+ "numinlets" : 1,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "bang" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 804.5, 499.0, 50.0, 20.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-40",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "int" ],
+ "patching_rect" : [ 861.0, 459.0, 32.5, 20.0 ],
+ "text" : "* 8"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-39",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "int" ],
+ "patching_rect" : [ 861.0, 418.0, 38.0, 20.0 ],
+ "text" : "- 128"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-38",
+ "maxclass" : "number",
+ "numinlets" : 1,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "bang" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 804.5, 459.0, 50.0, 20.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "hint" : "x 186 y 210",
+ "id" : "obj-16",
+ "maxclass" : "itable",
+ "name" : "",
+ "numinlets" : 2,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "bang" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 157.0, 650.0, 706.0, 121.0 ],
+ "range" : 128,
+ "signed" : 1,
+ "size" : 255,
+ "table_data" : [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "hint" : "x 0 y 236",
+ "id" : "obj-5",
+ "maxclass" : "itable",
+ "name" : "",
+ "numinlets" : 2,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "bang" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 157.0, 551.0, 706.0, 114.0 ],
+ "range" : 255,
+ "size" : 255,
+ "table_data" : [ 0, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-41",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patcher" : {
+ "fileversion" : 1,
+ "appversion" : {
+ "major" : 6,
+ "minor" : 0,
+ "revision" : 5
+ }
+,
+ "rect" : [ 54.0, 94.0, 410.0, 385.0 ],
+ "bglocked" : 0,
+ "openinpresentation" : 0,
+ "default_fontsize" : 12.0,
+ "default_fontface" : 0,
+ "default_fontname" : "Arial",
+ "gridonopen" : 0,
+ "gridsize" : [ 15.0, 15.0 ],
+ "gridsnaponopen" : 0,
+ "statusbarvisible" : 2,
+ "toolbarvisible" : 1,
+ "boxanimatetime" : 200,
+ "imprint" : 0,
+ "enablehscroll" : 1,
+ "enablevscroll" : 1,
+ "devicewidth" : 0.0,
+ "description" : "",
+ "digest" : "",
+ "tags" : "",
+ "boxes" : [ {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-6",
+ "linecount" : 4,
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 212.0, 124.0, 166.0, 62.0 ],
+ "text" : "this subpacth filters out any residual ASCII 65 messages in the serial bufer from initial startup."
+ }
+
+ }
+, {
+ "box" : {
+ "comment" : "",
+ "id" : "obj-4",
+ "maxclass" : "inlet",
+ "numinlets" : 0,
+ "numoutlets" : 1,
+ "outlettype" : [ "int" ],
+ "patching_rect" : [ 187.0, 23.0, 25.0, 25.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-3",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "int" ],
+ "patching_rect" : [ 100.0, 212.0, 32.5, 20.0 ],
+ "text" : "t i 1"
+ }
+
+ }
+, {
+ "box" : {
+ "comment" : "",
+ "id" : "obj-2",
+ "maxclass" : "outlet",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 41.0, 334.0, 25.0, 25.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "comment" : "",
+ "id" : "obj-1",
+ "maxclass" : "inlet",
+ "numinlets" : 0,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 76.0, 23.0, 25.0, 25.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-40",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 2,
+ "outlettype" : [ "", "" ],
+ "patching_rect" : [ 41.0, 145.0, 54.0, 20.0 ],
+ "text" : "gate 2 2"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-38",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 2,
+ "outlettype" : [ "bang", "" ],
+ "patching_rect" : [ 76.0, 175.0, 43.0, 20.0 ],
+ "text" : "sel 65"
+ }
+
+ }
+ ],
+ "lines" : [ {
+ "patchline" : {
+ "destination" : [ "obj-40", 1 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-1", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-2", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 109.5, 278.0, 50.5, 278.0 ],
+ "source" : [ "obj-3", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-40", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 123.0, 251.0, 182.0, 251.0, 182.0, 107.0, 50.5, 107.0 ],
+ "source" : [ "obj-3", 1 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-3", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-38", 1 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-40", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-4", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-2", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-40", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-38", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-40", 1 ]
+ }
+
+ }
+ ],
+ "dependency_cache" : [ ]
+ }
+,
+ "patching_rect" : [ 134.0, 352.0, 112.0, 20.0 ],
+ "saved_object_attributes" : {
+ "tags" : "",
+ "digest" : "",
+ "default_fontname" : "Arial",
+ "fontname" : "Arial",
+ "default_fontsize" : 12.0,
+ "description" : "",
+ "globalpatchername" : "",
+ "fontface" : 0,
+ "fontsize" : 12.0,
+ "default_fontface" : 0
+ }
+,
+ "text" : "p filter_extra_bytes"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-8",
+ "maxclass" : "newobj",
+ "numinlets" : 0,
+ "numoutlets" : 1,
+ "outlettype" : [ "bang" ],
+ "patching_rect" : [ 423.0, 65.0, 65.0, 20.0 ],
+ "text" : "closebang"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-7",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 1,
+ "outlettype" : [ "bang" ],
+ "patching_rect" : [ 355.0, 65.0, 60.0, 20.0 ],
+ "text" : "loadbang"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-37",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 726.0, 102.0, 207.0, 20.0 ],
+ "text" : "choose a serial port from this menu"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-28",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 695.0, 157.0, 79.0, 20.0 ],
+ "text" : "prepend port"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-4",
+ "items" : [ "Bluetooth-PDA-Sync", ",", "Bluetooth-Modem" ],
+ "maxclass" : "umenu",
+ "numinlets" : 1,
+ "numoutlets" : 3,
+ "outlettype" : [ "int", "", "" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 604.0, 128.0, 200.0, 20.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-32",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 604.0, 79.0, 98.0, 20.0 ],
+ "text" : "prepend append"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-33",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 1,
+ "outlettype" : [ "clear" ],
+ "patching_rect" : [ 675.0, 45.0, 43.0, 20.0 ],
+ "text" : "t clear"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-34",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 604.0, 45.0, 27.0, 20.0 ],
+ "text" : "iter"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-36",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 2,
+ "outlettype" : [ "", "" ],
+ "patching_rect" : [ 604.0, 12.0, 62.0, 20.0 ],
+ "text" : "route port"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-9",
+ "maxclass" : "message",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 279.0, 361.0, 32.5, 18.0 ],
+ "text" : "65"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-6",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 2,
+ "outlettype" : [ "bang", "" ],
+ "patching_rect" : [ 279.0, 332.0, 36.0, 20.0 ],
+ "text" : "sel 1"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-65",
+ "linecount" : 3,
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 179.0, 404.0, 425.0, 48.0 ],
+ "text" : "trigger (or [t]) forces right-left conventions. All the drawing and processing will happen before Max requests new values. When this trigger fires, it sends an ASCII A to ask Arduino for new values."
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-64",
+ "linecount" : 2,
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 117.0, 273.0, 135.0, 34.0 ],
+ "text" : "reinitializes the gates when turned on and off"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-63",
+ "linecount" : 5,
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 400.0, 250.0, 233.0, 75.0 ],
+ "text" : "checks for the ascii value of \"A\" to begin cominucation. After initial communication is made, this block shuts down. A byte is sent back to the Arduino, indicating the patch is ready to receive information."
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-57",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "int" ],
+ "patching_rect" : [ 252.0, 273.0, 34.0, 20.0 ],
+ "text" : "t 2 0"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-55",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "int" ],
+ "patching_rect" : [ 325.0, 331.0, 32.5, 20.0 ],
+ "text" : "!- 1"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-54",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 325.0, 250.0, 54.0, 20.0 ],
+ "text" : "gate 1 1"
+ }
+
+ }
+, {
+ "box" : {
+ "id" : "obj-53",
+ "maxclass" : "toggle",
+ "numinlets" : 1,
+ "numoutlets" : 1,
+ "outlettype" : [ "int" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 325.0, 300.0, 20.0, 20.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-50",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 134.0, 328.0, 54.0, 20.0 ],
+ "text" : "gate 1 0"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-48",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 2,
+ "outlettype" : [ "bang", "" ],
+ "patching_rect" : [ 325.0, 275.0, 43.0, 20.0 ],
+ "text" : "sel 65"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-35",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "" ],
+ "patching_rect" : [ 134.0, 399.0, 42.0, 21.0 ],
+ "text" : "t 65 l"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-1",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 475.0, 499.0, 37.0, 21.0 ],
+ "text" : "t"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-3",
+ "maxclass" : "number",
+ "numinlets" : 1,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "bang" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 407.0, 499.0, 56.0, 21.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-18",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 350.0, 500.0, 37.0, 21.0 ],
+ "text" : "v"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-20",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 222.0, 500.0, 37.0, 21.0 ],
+ "text" : "x"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-22",
+ "maxclass" : "number",
+ "numinlets" : 1,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "bang" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 282.0, 500.0, 56.0, 21.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-23",
+ "maxclass" : "number",
+ "numinlets" : 1,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "bang" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 157.0, 500.0, 55.0, 21.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-29",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 3,
+ "outlettype" : [ "int", "int", "int" ],
+ "patching_rect" : [ 157.0, 465.0, 269.0, 21.0 ],
+ "text" : "unpack 0 0 0"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Verdana",
+ "fontsize" : 12.0,
+ "id" : "obj-31",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 2,
+ "outlettype" : [ "", "" ],
+ "patching_rect" : [ 134.0, 375.0, 71.0, 21.0 ],
+ "text" : "zl group 3"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-30",
+ "maxclass" : "newobj",
+ "numinlets" : 3,
+ "numoutlets" : 3,
+ "outlettype" : [ "bang", "bang", "" ],
+ "patching_rect" : [ 252.0, 65.0, 62.0, 20.0 ],
+ "text" : "select 0 1"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-26",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 330.0, 156.0, 206.0, 20.0 ],
+ "text" : "click here to close the serial port"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-27",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 355.0, 127.0, 227.0, 20.0 ],
+ "text" : "click here to open the selected serial port"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-21",
+ "maxclass" : "message",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 282.0, 156.0, 39.0, 18.0 ],
+ "text" : "close"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-19",
+ "maxclass" : "message",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 304.0, 127.0, 37.0, 18.0 ],
+ "text" : "open"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-2",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 375.0, 98.0, 207.0, 20.0 ],
+ "text" : "Click here to get a list of serial ports"
+ }
+
+ }
+, {
+ "box" : {
+ "id" : "obj-11",
+ "maxclass" : "toggle",
+ "numinlets" : 1,
+ "numoutlets" : 1,
+ "outlettype" : [ "int" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 169.0, 20.0, 22.0, 22.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-12",
+ "maxclass" : "newobj",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "bang" ],
+ "patching_rect" : [ 169.0, 65.0, 65.0, 20.0 ],
+ "text" : "qmetro 10"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-13",
+ "maxclass" : "message",
+ "numinlets" : 2,
+ "numoutlets" : 1,
+ "outlettype" : [ "" ],
+ "patching_rect" : [ 324.0, 98.0, 36.0, 18.0 ],
+ "text" : "print"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-14",
+ "maxclass" : "newobj",
+ "numinlets" : 1,
+ "numoutlets" : 2,
+ "outlettype" : [ "int", "" ],
+ "patching_rect" : [ 169.0, 210.0, 155.0, 20.0 ],
+ "text" : "serial a 9600 @autoopen 0"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-15",
+ "linecount" : 2,
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 18.0, 57.0, 142.0, 34.0 ],
+ "text" : "Read serial input buffer every 10 milliseconds"
+ }
+
+ }
+, {
+ "box" : {
+ "fontname" : "Arial",
+ "fontsize" : 12.0,
+ "id" : "obj-17",
+ "maxclass" : "comment",
+ "numinlets" : 1,
+ "numoutlets" : 0,
+ "patching_rect" : [ 188.0, 20.0, 117.0, 20.0 ],
+ "text" : "Click to start"
+ }
+
+ }
+, {
+ "box" : {
+ "background" : 1,
+ "bgcolor" : [ 1.0, 0.788235, 0.470588, 1.0 ],
+ "border" : 0,
+ "bordercolor" : [ 0.0, 0.0, 0.0, 1.0 ],
+ "fontface" : 1,
+ "fontname" : "Arial",
+ "fontsize" : 13.0,
+ "hint" : "",
+ "id" : "obj-10",
+ "ignoreclick" : 1,
+ "maxclass" : "textbutton",
+ "numinlets" : 1,
+ "numoutlets" : 3,
+ "outlettype" : [ "", "", "int" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 270.0, 20.0, 20.0, 20.0 ],
+ "rounded" : 60.0,
+ "text" : "3",
+ "textcolor" : [ 0.34902, 0.34902, 0.34902, 1.0 ],
+ "textovercolor" : [ 0.2, 0.2, 0.2, 1.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "background" : 1,
+ "bgcolor" : [ 1.0, 0.788235, 0.470588, 1.0 ],
+ "border" : 0,
+ "bordercolor" : [ 0.0, 0.0, 0.0, 1.0 ],
+ "fontface" : 1,
+ "fontname" : "Arial",
+ "fontsize" : 13.0,
+ "hint" : "",
+ "id" : "obj-25",
+ "ignoreclick" : 1,
+ "maxclass" : "textbutton",
+ "numinlets" : 1,
+ "numoutlets" : 3,
+ "outlettype" : [ "", "", "int" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 706.0, 102.0, 20.0, 20.0 ],
+ "rounded" : 60.0,
+ "text" : "2",
+ "textcolor" : [ 0.34902, 0.34902, 0.34902, 1.0 ],
+ "textovercolor" : [ 0.2, 0.2, 0.2, 1.0 ]
+ }
+
+ }
+, {
+ "box" : {
+ "background" : 1,
+ "bgcolor" : [ 1.0, 0.788235, 0.470588, 1.0 ],
+ "border" : 0,
+ "bordercolor" : [ 0.0, 0.0, 0.0, 1.0 ],
+ "fontface" : 1,
+ "fontname" : "Arial",
+ "fontsize" : 13.0,
+ "hint" : "",
+ "id" : "obj-93",
+ "ignoreclick" : 1,
+ "maxclass" : "textbutton",
+ "numinlets" : 1,
+ "numoutlets" : 3,
+ "outlettype" : [ "", "", "int" ],
+ "parameter_enable" : 0,
+ "patching_rect" : [ 568.0, 98.0, 20.0, 20.0 ],
+ "rounded" : 60.0,
+ "text" : "1",
+ "textcolor" : [ 0.34902, 0.34902, 0.34902, 1.0 ],
+ "textovercolor" : [ 0.2, 0.2, 0.2, 1.0 ]
+ }
+
+ }
+ ],
+ "lines" : [ {
+ "patchline" : {
+ "destination" : [ "obj-12", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-11", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-30", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 178.5, 56.0, 261.5, 56.0 ],
+ "source" : [ "obj-11", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.168982, 0.8, 0.273315, 0.9 ],
+ "destination" : [ "obj-14", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 178.5, 147.0, 178.5, 147.0 ],
+ "source" : [ "obj-12", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.168982, 0.8, 0.273315, 0.9 ],
+ "destination" : [ "obj-14", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 333.5, 120.5, 178.5, 120.5 ],
+ "source" : [ "obj-13", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.0, 0.004775, 0.8, 0.9 ],
+ "destination" : [ "obj-36", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 314.5, 231.0, 600.0, 231.0, 600.0, 9.0, 613.5, 9.0 ],
+ "source" : [ "obj-14", 1 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.740956, 0.137093, 0.8, 0.9 ],
+ "destination" : [ "obj-50", 1 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-14", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.771514, 0.155069, 0.8, 0.9 ],
+ "destination" : [ "obj-54", 1 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 178.5, 237.0, 369.5, 237.0 ],
+ "source" : [ "obj-14", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.168982, 0.8, 0.273315, 0.9 ],
+ "destination" : [ "obj-14", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 313.5, 151.5, 178.5, 151.5 ],
+ "source" : [ "obj-19", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.168982, 0.8, 0.273315, 0.9 ],
+ "destination" : [ "obj-14", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 291.5, 185.5, 178.5, 185.5 ],
+ "source" : [ "obj-21", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-39", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-22", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-5", 1 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-23", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.076581, 0.8, 0.10276, 0.9 ],
+ "destination" : [ "obj-14", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 704.5, 200.0, 178.5, 200.0 ],
+ "source" : [ "obj-28", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-22", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-29", 1 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-23", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-29", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-3", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-29", 2 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-16", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-3", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-5", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-3", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.0, 0.0, 0.0, 0.9 ],
+ "destination" : [ "obj-19", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 283.0, 111.0, 313.5, 111.0 ],
+ "source" : [ "obj-30", 1 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.0, 0.0, 0.0, 0.9 ],
+ "destination" : [ "obj-21", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 261.5, 135.0, 291.5, 135.0 ],
+ "source" : [ "obj-30", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.0, 0.0, 0.0, 0.9 ],
+ "destination" : [ "obj-57", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 261.5, 178.5, 261.5, 178.5 ],
+ "source" : [ "obj-30", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-35", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-31", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-4", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-32", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-4", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-33", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-32", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-34", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.168982, 0.8, 0.273315, 0.9 ],
+ "destination" : [ "obj-14", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 143.5, 437.0, 107.0, 437.0, 107.0, 195.0, 178.5, 195.0 ],
+ "source" : [ "obj-35", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-29", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-35", 1 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-33", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-36", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-34", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-36", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-38", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-39", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-40", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-39", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-28", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-4", 1 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-16", 1 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-40", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-42", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-40", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-31", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-41", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-53", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-48", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-41", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-50", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-50", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 334.5, 321.0, 143.5, 321.0 ],
+ "source" : [ "obj-53", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-55", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-53", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-6", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 334.5, 326.5, 288.5, 326.5 ],
+ "source" : [ "obj-53", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-48", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-54", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-54", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 334.5, 354.0, 393.0, 354.0, 393.0, 246.0, 334.5, 246.0 ],
+ "source" : [ "obj-55", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-41", 1 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-57", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-53", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 276.5, 296.0, 334.5, 296.0 ],
+ "source" : [ "obj-57", 1 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "destination" : [ "obj-9", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-6", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.0, 0.0, 0.0, 0.9 ],
+ "destination" : [ "obj-13", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 364.5, 87.0, 336.0, 87.0, 336.0, 93.0, 333.5, 93.0 ],
+ "source" : [ "obj-7", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.0, 0.0, 0.0, 1.0 ],
+ "destination" : [ "obj-21", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "midpoints" : [ 432.5, 92.0, 309.0, 92.0, 309.0, 92.0, 291.5, 92.0 ],
+ "source" : [ "obj-8", 0 ]
+ }
+
+ }
+, {
+ "patchline" : {
+ "color" : [ 0.168982, 0.8, 0.273315, 0.9 ],
+ "destination" : [ "obj-14", 0 ],
+ "disabled" : 0,
+ "hidden" : 0,
+ "source" : [ "obj-9", 0 ]
+ }
+
+ }
+ ],
+ "dependency_cache" : [ ]
+ }
+
+}
diff --git a/software/apps/Motion/Graph2/Graph2.ino b/software/apps/Motion/Graph2/Graph2.ino
new file mode 100644
index 0000000..edd86b6
--- /dev/null
+++ b/software/apps/Motion/Graph2/Graph2.ino
@@ -0,0 +1,28 @@
+int x, v, t; //position velocity time
+#define n 10
+int xtab[n]; //table of x
+int i; // index for table
+
+void setup()
+{
+ // start serial port at 9600 bps:
+ Serial.begin(9600);
+}
+
+void loop()
+{
+ // if we get a valid byte, read analog ins:
+ if (Serial.available() > 0) {
+ x = analogRead(A0)/4; //just for this test
+ xtab[i] = x; //put it in array of x
+ i = i + 1; //index to last x (9 times ago)
+ if(i>9)i=0; //increment index
+ v = x - xtab[i]; //difference x vs old x
+ t = (t+1)%256;
+ // send sensor values:
+ Serial.write(x); // sends one byte [0-255]
+ Serial.write(v+128);
+ Serial.write(t);
+ }
+}
+
diff --git a/software/lib/MMM/Motion.cpp b/software/lib/MMM/Motion.cpp
index e69de29..a587da9 100644
--- a/software/lib/MMM/Motion.cpp
+++ b/software/lib/MMM/Motion.cpp
@@ -0,0 +1,134 @@
+/*
+ Motion.cpp - Motion library
+ Copyright (c) 2012 Copenhagen Institute of Interaction Design.
+ All right reserved.
+
+ This library is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This library 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 Lesser Public License for more details.
+
+ You should have received a copy of the GNU Lesser Public License
+ along with Foobar. If not, see .
+
+ +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ + author: dviid
+ + contact: dviid@labs.ciid.dk
+ */
+
+#include "Motion.h"
+
+#include
+#include
+#include
+
+#define PB0 PORTB0
+
+bool motion_reg_init = false;
+
+Motion MotionA(MOTIONA);
+Motion MotionB(MOTIONA);
+
+uint8_t lb;
+uint8_t hb;
+int x, xx;
+float T = N * 0.004f; // 4ms
+float v;
+
+
+Motion::Motion(MOTION m){
+ _m = m;
+ _i = false;
+};
+
+void Motion::init(INPUT sensor)
+{
+ if(!motion_reg_init){
+ TCNT1 = 1000; //4 ms
+ TIMSK1 = (1 << TOIE1);
+ motion_reg_init = true;
+ }
+ _i = true;
+ _s = sensor;
+}
+
+void Motion::updatePhysics()
+{
+
+}
+
+int Motion::getPosition() {
+ return _x;
+}
+
+float Motion::getVelocity() {
+ return _v;
+}
+
+float Motion::getAcceleration() {
+ return _a;
+}
+
+
+// clocked at 4ms period
+ISR(TIMER1_OVF_vect) {
+ TCNT1 = 1000;
+
+
+
+ if(MotionA._i) {
+ ADMUX = MotionA._s & 0x07;
+ ADCSRA |= (1 << ADSC);
+ while (ADCSRA & (1 << ADSC));
+ lb = ADCL;
+ hb = ADCH;
+ x = (hb << 8) | lb;
+ MotionA._xv[MotionA._ix] = x;
+ MotionA._ix++;
+ MotionA._ix %= N;
+
+ xx = x - MotionA._x;
+ if(abs(xx) < 2) {
+ v = 0.0f;
+ } else {
+ v = xx / T;
+ }
+
+ MotionA._a = (v - MotionA._v) / T;
+ MotionA._v = v;
+ MotionA._x = x;
+
+ }
+
+ if(MotionB._i) {
+ ADMUX = MotionB._s & 0x07;
+ ADCSRA |= (1 << ADSC);
+ while (ADCSRA & (1 << ADSC));
+ lb = ADCL;
+ hb = ADCH;
+ x = (hb << 8) | lb;
+ MotionB._xv[MotionB._ix] = x;
+ MotionB._ix++;
+ MotionB._ix %= N;
+
+
+ xx = x - MotionB._x;
+ if(abs(xx) < 2) {
+ v = 0;
+ } else {
+ v = xx / T;
+ }
+
+ MotionB._a = (v - MotionB._v) / T;
+ MotionB._v = v;
+ MotionB._x = x;
+
+ }
+
+}
+
diff --git a/software/lib/MMM/Motion.h b/software/lib/MMM/Motion.h
index e69de29..8ffeb0d 100644
--- a/software/lib/MMM/Motion.h
+++ b/software/lib/MMM/Motion.h
@@ -0,0 +1,73 @@
+/*
+ Motion.h - Motion library
+ Copyright (c) 2012 Copenhagen Institute of Interaction Design.
+ All right reserved.
+
+ This library is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This library 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 Lesser Public License for more details.
+
+ You should have received a copy of the GNU Lesser Public License
+ along with Foobar. If not, see .
+
+ +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ + author: dviid
+ + contact: dviid@labs.ciid.dk
+ */
+
+#include
+
+#define N 10
+
+enum MOTION {
+ MOTIONA = 0,
+ MOTIONB = 1
+};
+
+enum INPUT {
+ INPUTA0 = 0x0000,
+ INPUTA1 = 0x0001,
+ INPUTA2 = 0x0010,
+ INPUTA3 = 0x0011,
+ INPUTA4 = 0x0100,
+ INPUTA5 = 0x0101,
+ INPUTA6 = 0x0110,
+ INPUTA7 = 0x0111
+};
+
+
+class Motion {
+
+public:
+ Motion(MOTION m);
+
+ void init(INPUT sensor);
+
+ int getPosition();
+ float getVelocity();
+ float getAcceleration();
+
+ void updatePhysics();
+
+
+ // raw position vector
+ int _xv[N];
+ int _ix;
+ int _x;
+ float _v;
+ float _a;
+
+ MOTION _m;
+ INPUT _s;
+ bool _i;
+
+};
+
+extern Motion MotionA;
+extern Motion MotionB;
\ No newline at end of file
diff --git a/software/lib/MMM/Motor.cpp b/software/lib/MMM/Motor.cpp
index 5a8be66..4399009 100644
--- a/software/lib/MMM/Motor.cpp
+++ b/software/lib/MMM/Motor.cpp
@@ -39,70 +39,36 @@ MMotor MotorB(MOTORB); // this is motor B
MMotor::MMotor(MOTOR m)
{
_m = m;
- init();
}
void MMotor::init()
{
- if(!reg_init){
- //PWM, Phase and Frequency
- //Correctthese modes are preferred for motor control applications p.130
- TCCR1A = 0;
- TCCR1B |= (1 << WGM13);
+ if(!reg_init){
//direction pins are outputs
DDRD |= (1 << PD7);
- DDRB |= (1 << PB0);
+ DDRB |= (1 << PB0);
+ DDRB |= (1 << PB1) | (1 << PB2);
+ TCCR1A = (1 << COM1A1) | (1 << COM1B1);
+
+ // clear the bits
+ TCCR1B &= ~((1 << CS10) | (1 << CS11) | (1 << CS12));
+ TCCR1B = (1 << WGM13) | (1 << CS10);
+
+ ICR1 = 512;
reg_init = true;
}
- //set period for minimal noise
- _period(64);
-
}
-void MMotor::_set_period_bits()
-{
- // clear prescaler reg
- TCCR1B &= ~((1 << CS10) | (1 << CS11) | (1 << CS12));
-
- if(_p < RESOLUTION) TCCR1B |= (1 << CS10); // pre-s 0
- else if((_p >>= 3) < RESOLUTION) TCCR1B |= (1 << CS11); // pre-s 8
- else if((_p >>= 3) < RESOLUTION) TCCR1B |= (1 << CS11) | (1 << CS10); // pre-s 64
- else if((_p >>= 2) < RESOLUTION) TCCR1B |= (1 << CS12); // pre-s 256
- else if((_p >>= 2) < RESOLUTION) TCCR1B |= (1 << CS12) | (1 << CS10); // pre-s 1024
- else _p = RESOLUTION - 1, TCCR1B |= (1 << CS12) | (1 << CS10); // pre-s 1024
-
- ICR1 = _p;
-
-}
-
-void MMotor::_period(long ms)
-{
- _p = (F_CPU * ms) / 2000000;
- _set_period_bits();
-}
void MMotor::torque(int value)
-{
-
- if(_m == MOTORA) {
- DDRB |= (1 << PB1);
- TCCR1A |= (1 << COM1A1);
- } else if(_m == MOTORB) {
- DDRB |= (1 << PB2);
- TCCR1A |= (1 << COM1B1);
- }
-
- unsigned long duty = _p * value;
- duty >>= 10;
- if(_m == MOTORA) OCR1A = duty;
- else if(_m == MOTORB) OCR1B = duty;
- _t = value;
-
- start();
+{
+ if(_m == MOTORA) OCR1A = value;
+ else if(_m == MOTORB) OCR1B = value;
+ _t = value;
}
int MMotor::torque()
@@ -126,7 +92,7 @@ void MMotor::stop()
void MMotor::start()
{
- _set_period_bits();
+ TCCR1B = (1 << WGM13) | (1 << CS10);
}
void MMotor::restart()
diff --git a/software/lib/MMM/Motor.h b/software/lib/MMM/Motor.h
index 465a21c..1d26962 100644
--- a/software/lib/MMM/Motor.h
+++ b/software/lib/MMM/Motor.h
@@ -21,8 +21,6 @@
+ contact: dviid@labs.ciid.dk
*/
-#include
-
enum MOTOR{
MOTORA = 0,
MOTORB = 1
@@ -35,7 +33,7 @@ enum DIRECTION {
class MMotor {
-public:
+public:
MMotor(MOTOR m);
@@ -51,10 +49,7 @@ public:
void start();
void restart();
-private:
-
- void _period(long ms);
- void _set_period_bits();
+protected:
MOTOR _m;
DIRECTION _d;