Squashed 'third_party/Phoenix-frc-lib/' content from commit 666d176

Change-Id: Ibaca2fc8ffb1177e786576cc1e4cc9f7a8c98f13
git-subtree-dir: third_party/Phoenix-frc-lib
git-subtree-split: 666d176a08151793044ab74e0005f13d3732ed96
diff --git a/.cproject b/.cproject
new file mode 100644
index 0000000..f23040a
--- /dev/null
+++ b/.cproject
@@ -0,0 +1,350 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?fileVersion 4.0.0?>
+<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
+	<storageModule moduleId="org.eclipse.cdt.core.settings">
+		<cconfiguration
+			id="cdt.managedbuild.config.gnu.cross.lib.debug.334331879">
+			<storageModule
+				buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider"
+				id="cdt.managedbuild.config.gnu.cross.lib.debug.334331879" moduleId="org.eclipse.cdt.core.settings"
+				name="Debug">
+				<externalSettings>
+					<externalSetting>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="includePath" name="/AAA" />
+						<entry flags="VALUE_WORKSPACE_PATH" kind="includePath" name="/CTRLib" />
+						<entry flags="VALUE_WORKSPACE_PATH" kind="includePath" name="/CTRLibZ" />
+						<entry flags="VALUE_WORKSPACE_PATH" kind="includePath" name="/CTRE_PhoenixZ" />
+						<entry flags="VALUE_WORKSPACE_PATH" kind="libraryPath" name="/AAA/Debug" />
+						<entry flags="VALUE_WORKSPACE_PATH" kind="libraryPath" name="/CTRLib/Debug" />
+						<entry flags="VALUE_WORKSPACE_PATH" kind="libraryPath" name="/CTRLibZ/Debug" />
+						<entry flags="VALUE_WORKSPACE_PATH" kind="libraryPath"
+							name="/CTRE_PhoenixZ/Debug" />
+						<entry flags="RESOLVED" kind="libraryFile" name="AAA"
+							srcPrefixMapping="" srcRootPath="" />
+						<entry flags="RESOLVED" kind="libraryFile" name="CTRE_Phoenix"
+							srcPrefixMapping="" srcRootPath="" />
+					</externalSetting>
+				</externalSettings>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser" />
+					<extension id="org.eclipse.cdt.core.GASErrorParser"
+						point="org.eclipse.cdt.core.ErrorParser" />
+					<extension id="org.eclipse.cdt.core.GCCErrorParser"
+						point="org.eclipse.cdt.core.ErrorParser" />
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactExtension="a" artifactName="CTRE_Phoenix"
+					buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.staticLib"
+					buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.staticLib,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug"
+					cleanCommand="rm -rf" description=""
+					id="cdt.managedbuild.config.gnu.cross.lib.debug.334331879" name="Debug"
+					parent="cdt.managedbuild.config.gnu.cross.lib.debug" postbuildStep="${workspace_loc:/${ProjName}}/Eclipse_DeployForDebug.bat">
+					<folderInfo
+						id="cdt.managedbuild.config.gnu.cross.lib.debug.334331879." name="/"
+						resourcePath="">
+						<toolChain
+							id="cdt.managedbuild.toolchain.gnu.cross.lib.debug.2102704975"
+							name="Cross GCC" nonInternalBuilderId="cdt.managedbuild.builder.gnu.cross"
+							superClass="cdt.managedbuild.toolchain.gnu.cross.lib.debug">
+							<option id="cdt.managedbuild.option.gnu.cross.path.904104123"
+								name="Path" superClass="cdt.managedbuild.option.gnu.cross.path"
+								value="/usr/local/bin" valueType="string" />
+							<option id="cdt.managedbuild.option.gnu.cross.prefix.1455898653"
+								name="Prefix" superClass="cdt.managedbuild.option.gnu.cross.prefix"
+								value="arm-frc-linux-gnueabi-" valueType="string" />
+							<targetPlatform archList="all"
+								binaryParser="org.eclipse.cdt.core.ELF"
+								id="cdt.managedbuild.targetPlatform.gnu.cross.1794261647"
+								isAbstract="false" osList="all"
+								superClass="cdt.managedbuild.targetPlatform.gnu.cross" />
+							<builder autoBuildTarget="all" buildPath="${workspace_loc:/AAA}/Debug"
+								cleanBuildTarget="clean"
+								id="org.eclipse.cdt.build.core.internal.builder.2027344903"
+								incrementalBuildTarget="all" keepEnvironmentInBuildfile="false"
+								managedBuildOn="true" name="CDT Internal Builder"
+								superClass="org.eclipse.cdt.build.core.internal.builder" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.c.compiler.1377642277"
+								name="Cross GCC Compiler" superClass="cdt.managedbuild.tool.gnu.cross.c.compiler">
+								<option defaultValue="gnu.c.optimization.level.none"
+									id="gnu.c.compiler.option.optimization.level.98193995" name="Optimization Level"
+									superClass="gnu.c.compiler.option.optimization.level"
+									useByScannerDiscovery="false" valueType="enumerated" />
+								<option id="gnu.c.compiler.option.debugging.level.85177145"
+									name="Debug Level" superClass="gnu.c.compiler.option.debugging.level"
+									useByScannerDiscovery="false" value="gnu.c.debugging.level.max"
+									valueType="enumerated" />
+								<option id="gnu.c.compiler.option.misc.other.1901376053"
+									name="Other flags" superClass="gnu.c.compiler.option.misc.other"
+									useByScannerDiscovery="false" value="-c -fmessage-length=0 -std=gnu++0x"
+									valueType="string" />
+								<inputType
+									id="cdt.managedbuild.tool.gnu.c.compiler.input.517371158"
+									superClass="cdt.managedbuild.tool.gnu.c.compiler.input" />
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.compiler.373052418"
+								name="Cross G++ Compiler" superClass="cdt.managedbuild.tool.gnu.cross.cpp.compiler">
+								<option id="gnu.cpp.compiler.option.optimization.level.1679328920"
+									name="Optimization Level" superClass="gnu.cpp.compiler.option.optimization.level"
+									useByScannerDiscovery="false"
+									value="gnu.cpp.compiler.optimization.level.none" valueType="enumerated" />
+								<option id="gnu.cpp.compiler.option.debugging.level.992567940"
+									name="Debug Level" superClass="gnu.cpp.compiler.option.debugging.level"
+									useByScannerDiscovery="false" value="gnu.cpp.compiler.debugging.level.max"
+									valueType="enumerated" />
+								<option id="gnu.cpp.compiler.option.include.paths.572235010"
+									name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths"
+									useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false"
+										value="${workspace_loc:/${ProjName}}\cpp\include" />
+									<listOptionValue builtIn="false"
+										value="${workspace_loc:/${ProjName}}\build\networktables\include" />
+									<listOptionValue builtIn="false"
+										value="${workspace_loc:/${ProjName}}\build\wpilib\include" />
+									<listOptionValue builtIn="false"
+										value="${workspace_loc:/${ProjName}}\build\HAL\include" />
+									<listOptionValue builtIn="false"
+										value="${workspace_loc:/${ProjName}}\build\NI\include" />
+									<listOptionValue builtIn="false"
+										value="${workspace_loc:/${ProjName}}\build\wpiutil\include" />
+									<listOptionValue builtIn="false"
+										value="${workspace_loc:/${ProjName}}\build\ntcore\include" />
+									<listOptionValue builtIn="false"
+										value="${workspace_loc:/${ProjName}}\libraries\driver\include" />
+								</option>
+								<option id="gnu.cpp.compiler.option.other.other.1388778170"
+									name="Other flags" superClass="gnu.cpp.compiler.option.other.other"
+									useByScannerDiscovery="false" value="-c -fmessage-length=0 -std=gnu++0x"
+									valueType="string" />
+								<option id="gnu.cpp.compiler.option.include.files.1837400873"
+									name="Include files (-include)" superClass="gnu.cpp.compiler.option.include.files"
+									useByScannerDiscovery="false" valueType="includeFiles" />
+								<inputType
+									id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1080222309"
+									superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input" />
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1925388523"
+								name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker">
+								<option defaultValue="true"
+									id="gnu.c.link.option.shared.1052310965" name="Shared (-shared)"
+									superClass="gnu.c.link.option.shared" valueType="boolean" />
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1371845294"
+								name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+								<option defaultValue="true"
+									id="gnu.cpp.link.option.shared.694318821" name="Shared (-shared)"
+									superClass="gnu.cpp.link.option.shared" valueType="boolean" />
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.12557891"
+									superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency"
+										paths="$(USER_OBJS)" />
+									<additionalInput kind="additionalinput" paths="$(LIBS)" />
+								</inputType>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.212221186"
+								name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.assembler.472420872"
+								name="Cross GCC Assembler" superClass="cdt.managedbuild.tool.gnu.cross.assembler">
+								<option id="gnu.both.asm.option.include.paths.72295413"
+									name="Include paths (-I)" superClass="gnu.both.asm.option.include.paths"
+									valueType="includePath">
+									<listOptionValue builtIn="false"
+										value="&quot;${workspace_loc:/${ProjName}/libraries/driver/lib}&quot;" />
+								</option>
+								<inputType
+									id="cdt.managedbuild.tool.gnu.assembler.input.1354990345"
+									superClass="cdt.managedbuild.tool.gnu.assembler.input" />
+							</tool>
+						</toolChain>
+					</folderInfo>
+					<folderInfo
+						id="cdt.managedbuild.config.gnu.cross.lib.debug.334331879.636394267"
+						name="/" resourcePath="libraries">
+						<toolChain
+							id="cdt.managedbuild.toolchain.gnu.cross.lib.debug.981267775"
+							name="Cross GCC" superClass="cdt.managedbuild.toolchain.gnu.cross.lib.debug"
+							unusedChildren="">
+							<option
+								id="cdt.managedbuild.option.gnu.cross.path.904104123.1932853506"
+								name="Path" superClass="cdt.managedbuild.option.gnu.cross.path.904104123" />
+							<option
+								id="cdt.managedbuild.option.gnu.cross.prefix.1455898653.1467431896"
+								name="Prefix" superClass="cdt.managedbuild.option.gnu.cross.prefix.1455898653" />
+							<targetPlatform archList="all"
+								binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.targetPlatform.gnu.cross"
+								isAbstract="false" osList="all"
+								superClass="cdt.managedbuild.targetPlatform.gnu.cross" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.c.compiler.293041307"
+								name="Cross GCC Compiler" superClass="cdt.managedbuild.tool.gnu.cross.c.compiler.1377642277">
+								<inputType
+									id="cdt.managedbuild.tool.gnu.c.compiler.input.930705907"
+									superClass="cdt.managedbuild.tool.gnu.c.compiler.input" />
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.compiler.1208569112"
+								name="Cross G++ Compiler" superClass="cdt.managedbuild.tool.gnu.cross.cpp.compiler.373052418">
+								<inputType
+									id="cdt.managedbuild.tool.gnu.cpp.compiler.input.972539700"
+									superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input" />
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1842768477"
+								name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker.1925388523" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1140762990"
+								name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker.1371845294" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.121631310"
+								name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver.212221186" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.assembler.1094466593"
+								name="Cross GCC Assembler" superClass="cdt.managedbuild.tool.gnu.cross.assembler.472420872">
+								<inputType
+									id="cdt.managedbuild.tool.gnu.assembler.input.1470477502"
+									superClass="cdt.managedbuild.tool.gnu.assembler.input" />
+							</tool>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry excluding="release|TemplateFiles" flags="VALUE_WORKSPACE_PATH|RESOLVED"
+							kind="sourcePath" name="" />
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings" />
+		</cconfiguration>
+		<cconfiguration
+			id="cdt.managedbuild.config.gnu.cross.lib.release.1690000530">
+			<storageModule
+				buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider"
+				id="cdt.managedbuild.config.gnu.cross.lib.release.1690000530"
+				moduleId="org.eclipse.cdt.core.settings" name="Release">
+				<externalSettings>
+					<externalSetting>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="includePath" name="/AAA" />
+						<entry flags="VALUE_WORKSPACE_PATH" kind="libraryPath" name="/AAA/Release" />
+						<entry flags="RESOLVED" kind="libraryFile" name="AAA"
+							srcPrefixMapping="" srcRootPath="" />
+					</externalSetting>
+				</externalSettings>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser" />
+					<extension id="org.eclipse.cdt.core.GASErrorParser"
+						point="org.eclipse.cdt.core.ErrorParser" />
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser"
+						point="org.eclipse.cdt.core.ErrorParser" />
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser" />
+					<extension id="org.eclipse.cdt.core.GCCErrorParser"
+						point="org.eclipse.cdt.core.ErrorParser" />
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactExtension="a" artifactName="${ProjName}"
+					buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.staticLib"
+					buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.staticLib,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release"
+					cleanCommand="rm -rf" description=""
+					id="cdt.managedbuild.config.gnu.cross.lib.release.1690000530" name="Release"
+					parent="cdt.managedbuild.config.gnu.cross.lib.release">
+					<folderInfo
+						id="cdt.managedbuild.config.gnu.cross.lib.release.1690000530."
+						name="/" resourcePath="">
+						<toolChain
+							id="cdt.managedbuild.toolchain.gnu.cross.lib.release.304135381"
+							name="Cross GCC" superClass="cdt.managedbuild.toolchain.gnu.cross.lib.release">
+							<option id="cdt.managedbuild.option.gnu.cross.prefix.621420538"
+								name="Prefix" superClass="cdt.managedbuild.option.gnu.cross.prefix"
+								value="arm-frc-linux-gnueabi-" valueType="string" />
+							<option id="cdt.managedbuild.option.gnu.cross.path.1286548262"
+								name="Path" superClass="cdt.managedbuild.option.gnu.cross.path"
+								value="/usr/local/bin" valueType="string" />
+							<targetPlatform archList="all"
+								binaryParser="org.eclipse.cdt.core.ELF"
+								id="cdt.managedbuild.targetPlatform.gnu.cross.1320454499"
+								isAbstract="false" osList="all"
+								superClass="cdt.managedbuild.targetPlatform.gnu.cross" />
+							<builder buildPath="${workspace_loc:/AAA}/Release"
+								id="cdt.managedbuild.builder.gnu.cross.1574511369"
+								keepEnvironmentInBuildfile="false" managedBuildOn="true"
+								name="Gnu Make Builder" superClass="cdt.managedbuild.builder.gnu.cross" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.c.compiler.392783880"
+								name="Cross GCC Compiler" superClass="cdt.managedbuild.tool.gnu.cross.c.compiler">
+								<option defaultValue="gnu.c.optimization.level.most"
+									id="gnu.c.compiler.option.optimization.level.2051403180" name="Optimization Level"
+									superClass="gnu.c.compiler.option.optimization.level"
+									useByScannerDiscovery="false" valueType="enumerated" />
+								<option id="gnu.c.compiler.option.debugging.level.1833271904"
+									name="Debug Level" superClass="gnu.c.compiler.option.debugging.level"
+									useByScannerDiscovery="false" value="gnu.c.debugging.level.none"
+									valueType="enumerated" />
+								<inputType
+									id="cdt.managedbuild.tool.gnu.c.compiler.input.109959218"
+									superClass="cdt.managedbuild.tool.gnu.c.compiler.input" />
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.compiler.481184687"
+								name="Cross G++ Compiler" superClass="cdt.managedbuild.tool.gnu.cross.cpp.compiler">
+								<option id="gnu.cpp.compiler.option.optimization.level.1329371599"
+									name="Optimization Level" superClass="gnu.cpp.compiler.option.optimization.level"
+									useByScannerDiscovery="false"
+									value="gnu.cpp.compiler.optimization.level.most" valueType="enumerated" />
+								<option id="gnu.cpp.compiler.option.debugging.level.697984959"
+									name="Debug Level" superClass="gnu.cpp.compiler.option.debugging.level"
+									useByScannerDiscovery="false" value="gnu.cpp.compiler.debugging.level.none"
+									valueType="enumerated" />
+								<inputType
+									id="cdt.managedbuild.tool.gnu.cpp.compiler.input.101742635"
+									superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input" />
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1854608354"
+								name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.linker.831057110"
+								name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.1738175537"
+								name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver" />
+							<tool id="cdt.managedbuild.tool.gnu.cross.assembler.1639137951"
+								name="Cross GCC Assembler" superClass="cdt.managedbuild.tool.gnu.cross.assembler">
+								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.953068723"
+									superClass="cdt.managedbuild.tool.gnu.assembler.input" />
+							</tool>
+						</toolChain>
+					</folderInfo>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings" />
+		</cconfiguration>
+	</storageModule>
+	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+		<project id="AAA.cdt.managedbuild.target.gnu.cross.lib.1569114193"
+			name="Static Library" projectType="cdt.managedbuild.target.gnu.cross.lib" />
+	</storageModule>
+	<storageModule moduleId="scannerConfiguration">
+		<autodiscovery enabled="true" problemReportingEnabled="true"
+			selectedProfileId="" />
+		<scannerConfigBuildInfo
+			instanceId="cdt.managedbuild.config.gnu.cross.lib.release.1690000530;cdt.managedbuild.config.gnu.cross.lib.release.1690000530.;cdt.managedbuild.tool.gnu.cross.c.compiler.392783880;cdt.managedbuild.tool.gnu.c.compiler.input.109959218">
+			<autodiscovery enabled="true" problemReportingEnabled="true"
+				selectedProfileId="" />
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo
+			instanceId="cdt.managedbuild.config.gnu.cross.lib.release.1690000530;cdt.managedbuild.config.gnu.cross.lib.release.1690000530.;cdt.managedbuild.tool.gnu.cross.cpp.compiler.481184687;cdt.managedbuild.tool.gnu.cpp.compiler.input.101742635">
+			<autodiscovery enabled="true" problemReportingEnabled="true"
+				selectedProfileId="" />
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo
+			instanceId="cdt.managedbuild.config.gnu.cross.lib.debug.334331879;cdt.managedbuild.config.gnu.cross.lib.debug.334331879.;cdt.managedbuild.tool.gnu.cross.c.compiler.1377642277;cdt.managedbuild.tool.gnu.c.compiler.input.517371158">
+			<autodiscovery enabled="true" problemReportingEnabled="true"
+				selectedProfileId="" />
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo
+			instanceId="cdt.managedbuild.config.gnu.cross.lib.debug.334331879;cdt.managedbuild.config.gnu.cross.lib.debug.334331879.;cdt.managedbuild.tool.gnu.cross.cpp.compiler.373052418;cdt.managedbuild.tool.gnu.cpp.compiler.input.1080222309">
+			<autodiscovery enabled="true" problemReportingEnabled="true"
+				selectedProfileId="" />
+		</scannerConfigBuildInfo>
+	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders" />
+	<storageModule moduleId="refreshScope" versionNumber="2">
+		<configuration configurationName="Debug">
+			<resource resourceType="PROJECT" workspacePath="/AAA" />
+		</configuration>
+		<configuration configurationName="Release">
+			<resource resourceType="PROJECT" workspacePath="/AAA" />
+		</configuration>
+	</storageModule>
+	<storageModule
+		moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings" />
+	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets" />
+</cproject>
diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 0000000..bdb0cab
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1,17 @@
+# Auto detect text files and perform LF normalization
+* text=auto
+
+# Custom for Visual Studio
+*.cs     diff=csharp
+
+# Standard to msysgit
+*.doc	 diff=astextplain
+*.DOC	 diff=astextplain
+*.docx diff=astextplain
+*.DOCX diff=astextplain
+*.dot  diff=astextplain
+*.DOT  diff=astextplain
+*.pdf  diff=astextplain
+*.PDF	 diff=astextplain
+*.rtf	 diff=astextplain
+*.RTF	 diff=astextplain
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..1123c98
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,230 @@
+# Exceptions:
+!libraries/driver/lib/*.a
+!libraries/java/lib/*.so
+
+### C++ ###
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+.vs/
+*.def
+!ntcore.def
+!ntcore-jni.def
+*.opensdf
+*.vcxproj
+*.vcxproj.user
+*.sdf
+*.sublime-project
+*.sublime-workspace
+
+# Compiled Java files
+*.class
+
+# Build directories
+/.gradle
+/build*
+!build.gradle
+/native
+/arm
+/gmock/build
+/release
+
+
+# Created by https://www.gitignore.io/api/intellij,eclipse,netbeans,java,gradle,c++,cmake
+
+### Intellij ###
+# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm
+# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839
+
+# User-specific stuff:
+.idea/*
+
+## File-based project format:
+*.iws
+
+## Plugin-specific files:
+
+# IntelliJ
+/out/
+
+# mpeltonen/sbt-idea plugin
+.idea_modules/
+
+# JIRA plugin
+atlassian-ide-plugin.xml
+
+# Crashlytics plugin (for Android Studio and IntelliJ)
+com_crashlytics_export_strings.xml
+crashlytics.properties
+crashlytics-build.properties
+fabric.properties
+
+### Intellij Patch ###
+# Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721
+
+# *.iml
+# modules.xml
+
+
+### Eclipse ###
+
+.metadata
+bin/
+tmp/
+*.tmp
+*.bak
+*.swp
+*~.nib
+local.properties
+.settings/
+.loadpath
+.recommenders
+
+# Eclipse Core
+.project
+
+# External tool builders
+.externalToolBuilders/
+
+# Locally stored "Eclipse launch configurations"
+*.launch
+
+# PyDev specific (Python IDE for Eclipse)
+*.pydevproject
+
+# CDT-specific (C/C++ Development Tooling)
+.cproject
+
+# JDT-specific (Eclipse Java Development Tools)
+.classpath
+
+# Java annotation processor (APT)
+.factorypath
+
+# PDT-specific (PHP Development Tools)
+.buildpath
+
+# sbteclipse plugin
+.target
+
+# Tern plugin
+.tern-project
+
+# TeXlipse plugin
+.texlipse
+
+# STS (Spring Tool Suite)
+.springBeans
+
+# Code Recommenders
+.recommenders/
+
+
+### NetBeans ###
+nbproject/private/
+build/
+nbbuild/
+dist/
+nbdist/
+nbactions.xml
+.nb-gradle/
+
+
+### C++ ###
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+
+### CMake ###
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Makefile
+cmake_install.cmake
+install_manifest.txt
+
+
+### Java ###
+*.class
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.ear
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+
+### Gradle ###
+.gradle
+build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+#VSCode
+.vscode/
+/Debug/
+/Release/
diff --git a/.project b/.project
new file mode 100644
index 0000000..9d2f89b
--- /dev/null
+++ b/.project
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>CTRE_PhoenixZ</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<triggers>full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
+	</natures>
+</projectDescription>
diff --git a/Eclipse_DeployForDebug.bat b/Eclipse_DeployForDebug.bat
new file mode 100644
index 0000000..f3df2c9
--- /dev/null
+++ b/Eclipse_DeployForDebug.bat
@@ -0,0 +1,11 @@
+:: set working directory to where the compiled lib is
+cd %~dp0\Debug
+
+:: move the headers into the WPILIB folder for FRC applications
+xcopy /Y /E ..\..\Phoenix-frc-lib\cpp\include\ctre %HOMEPATH%\wpilib\user\cpp\include\ctre
+
+:: move the driver headers into the WPILIB folder for FRC applications
+xcopy /Y /E ..\..\Phoenix-frc-lib\libraries\driver\include\ctre %HOMEPATH%\wpilib\user\cpp\include\ctre
+
+:: move the low level static lib so Robot apps use it
+echo F|xcopy /Y /E .\libCTRE_Phoenix.a %HOMEPATH%\wpilib\user\cpp\lib\libCTRE_Phoenix.a
\ No newline at end of file
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..9bc7185
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,23 @@
+/**
+ * Phoenix Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ * 
+ * Cross The Road Electronics (CTRE) licenses to you the right to 
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and 
+ * Phoenix Software API Libraries ONLY when in use with CTR Electronics hardware products
+ * as well as the FRC roboRIO when in use in FRC Competition.
+ * 
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, 
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
\ No newline at end of file
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..1575209
--- /dev/null
+++ b/README.md
@@ -0,0 +1,15 @@
+# Phoenix-frc-lib
+CTRE Phoenix Application-Level Library for FRC Java and C++
+
+Relevant Source Files are located in the following locations:
+
+C++: cpp/include or cpp/source
+Java: java/src/com/ctre
+
+CTRE_PhoenixCCI libraries are located within the 'libraries' folder.
+Build the gradle project located in CTRE_PhoenixCCI to update this binaries.
+
+## Dependencies
+FRC C++ Toolchain
+
+Java Development Kit 8u77 (Any other version risks gradle incompatibility)
diff --git a/build.gradle b/build.gradle
new file mode 100644
index 0000000..b8913fd
--- /dev/null
+++ b/build.gradle
@@ -0,0 +1,101 @@
+import org.gradle.internal.os.OperatingSystem
+
+plugins {
+    id 'net.ltgt.errorprone' version '0.0.8'
+    id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '2.0'
+}
+
+allprojects {
+    repositories {
+        mavenCentral()
+    }
+}
+
+if (!hasProperty('releaseType')) {
+    WPILibVersion {
+        releaseType = 'dev'
+    }
+}
+
+ext.compilerPrefixToUse = project.hasProperty('compilerPrefix') ? project.compilerPrefix : 'arm-frc-linux-gnueabi-'
+
+ext.setupDefines = { project, binaries ->
+    binaries.all {
+        if (project.hasProperty('debug')) {
+            project.setupDebugDefines(cppCompiler, linker)
+        } else {
+            project.setupReleaseDefines(cppCompiler, linker)
+        }
+    }
+}
+
+apply from: "locations.gradle"
+
+ext.addUserLinks = { linker, targetPlatform, implLib ->
+    def libPattern = /.*((\\/|\\).*)+lib(?<libName>.+).(.+)$/
+    def libraryArgs = []
+    def libraryPath = file(driverLibraryLib).path
+
+    // adds all libraries found in the driver folder
+    def libraryTree = fileTree(libraryPath)
+    libraryTree.include '*.so'
+    libraryTree.include '*.a'
+
+    libraryTree.each { lib ->
+        def nameMatcher = (lib.path =~ libPattern)
+        if (nameMatcher[0].size() > 1) {
+            def name = nameMatcher.group('libName')
+            libraryArgs << '-l' + name
+        }
+    }
+    
+    if (implLib) {
+        // adds all libraries found in the impl folder
+        def implLibraryPath = file(cppLibraryLib).path
+        def implLibraryTree = fileTree(implLibraryPath)
+        implLibraryTree.include '*.so'
+        implLibraryTree.include '*.a'
+
+        implLibraryTree.each { lib ->
+            def nameMatcher = (lib.path =~ libPattern)
+            if (nameMatcher[0].size() > 1) {
+                def name = nameMatcher.group('libName')
+                libraryArgs << '-l' + name
+            }
+        }
+    }
+     
+    // Add all arguments
+    String architecture = targetPlatform.architecture
+    if (architecture.contains('arm')){
+        linker.args << '-L' + libraryPath
+        linker.args.addAll(libraryArgs)
+    }
+}
+
+apply from: "properties.gradle"
+apply from: "options.gradle"
+
+apply from: wpiDeps
+
+apply from: "cpp.gradle"
+
+// Empty task for build so that zips will be
+// built when running ./gradlew build
+task build
+
+apply from: "releases.gradle"
+
+task clean(type: Delete) {
+    description = "Deletes the build directory"
+    group = "Build"
+    delete buildDir
+}
+
+clean {
+    delete releaseDir
+}
+
+task wrapper(type: Wrapper) {
+    gradleVersion = '3.3'
+}
diff --git a/cpp.gradle b/cpp.gradle
new file mode 100644
index 0000000..971db54
--- /dev/null
+++ b/cpp.gradle
@@ -0,0 +1,97 @@
+defineWpiUtilProperties()
+if (wpiDeps == "wpi-dependencies-2018.gradle"){defineNIProperties()}
+defineHALProperties()
+defineNetworkTablesProperties()
+defineWpiLibProperties()
+//defineCsCoreProperties()
+
+def cppSetupModel = { project ->
+    project.model {
+        components {
+            CTRE_Phoenix(NativeLibrarySpec) {
+                targetPlatform 'arm'
+                setupDefines(project, binaries)
+
+                binaries.all {
+                    tasks.withType(CppCompile) {
+                        cppCompiler.args << "-DNAMESPACED_WPILIB" << "-Werror" << "-Wall"
+                        addUserLinks(linker, targetPlatform, false)
+                        addHalLibraryLinks(it, linker, targetPlatform)
+                        addWpiUtilLibraryLinks(it, linker, targetPlatform)
+                        addNetworkTablesLibraryLinks(it, linker, targetPlatform)
+                        addWpilibLibraryLinks(it, linker, targetPlatform)
+//						addCsCoreLibraryLinks(it, linker, targetPlatform)
+						if (wpiDeps == "wpi-dependencies-2018.gradle"){addNILibraryLinks(it, linker, targetPlatform)}
+                    }
+                }
+
+                sources {
+                    cpp {
+                        source {
+                            srcDirs = [cppSrc, cppLibrarySrc]
+                            includes = ["**/*.cpp"]
+                        }
+                        exportedHeaders {
+                            srcDirs = [cppInclude, driverLibraryInclude, cppLibraryInclude, wpilibInclude, halInclude, wpiUtilInclude, netTablesInclude, NIInclude]
+                            includes = ['**/*.h']
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+ext.mergeStaticSetup = { pjt ->
+    if (combineStaticLibs) {
+        pjt.tasks.whenObjectAdded { task ->
+            def name = task.name.toLowerCase()
+            if (name.contains('create') && name.contains('staticlibrary')) {
+                def libraryPath = task.outputFile.parent
+                def library = file(task.outputFile.absolutePath)
+                project(':arm:driver').model {
+                    binaries {
+                        withType(StaticLibraryBinarySpec) { binary ->
+                            ext.driverLibrary = new File(binary.staticLibraryFile.absolutePath)
+                        }
+                    }
+                }
+                task.doLast {
+                    library.renameTo(file("${library}.m"))
+                    copy {
+                        from driverLibrary
+                        into libraryPath
+                    }
+                    
+                    ByteArrayOutputStream mriFile = new ByteArrayOutputStream()
+                    mriFile << "create lib${libraryName}.a\n"
+                    mriFile << "addlib lib${libraryName}.a.m\n"
+                    mriFile << "addlib lib${libraryName}Driver.a\n"
+                    mriFile << "save\n"
+                    mriFile << "end\n"
+                    
+                    def stdIn = new ByteArrayInputStream(mriFile.buf)
+                    
+                    exec {
+                        standardInput = stdIn
+                        workingDir = libraryPath
+                        commandLine "${compilerPrefixToUse}ar", '-M'
+                    }
+                }
+            }
+        }
+    }
+}
+
+project(':arm:cpp') {
+    apply plugin: 'cpp'
+
+    apply from: "${rootDir}/toolchains/arm.gradle"
+    
+    apply from: "${rootDir}/java/java.gradle"
+
+    cppSetupModel(project)
+    
+    project.debugStripSetup()
+    mergeStaticSetup(project)
+}
diff --git a/cpp/include/ctre/Phoenix.h b/cpp/include/ctre/Phoenix.h
new file mode 100644
index 0000000..ff1cb68
--- /dev/null
+++ b/cpp/include/ctre/Phoenix.h
@@ -0,0 +1,35 @@
+#pragma once
+#include "ctre/phoenix/CANifier.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/HsvToRgb.h"
+#include "ctre/phoenix/LinearInterpolation.h"
+#include "ctre/phoenix/Motion/MotionProfileStatus.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+#include "ctre/phoenix/MotorControl/CAN/TalonSRX.h"
+#include "ctre/phoenix/MotorControl/CAN/VictorSPX.h"
+#include "ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h"
+#include "ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h"
+#include "ctre/phoenix/MotorControl/DemandType.h"
+#include "ctre/phoenix/MotorControl/Faults.h"
+#include "ctre/phoenix/MotorControl/FollowerType.h"
+#include "ctre/phoenix/MotorControl/SensorCollection.h"
+#include "ctre/phoenix/MotorControl/IMotorController.h"
+#include "ctre/phoenix/MotorControl/IMotorControllerEnhanced.h"
+#include "ctre/phoenix/Sensors/PigeonIMU.h"
+#include "ctre/phoenix/Signals/MovingAverage.h"
+#include "ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h"
+#include "ctre/phoenix/Tasking/ILoopable.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+#include "ctre/phoenix/Tasking/ButtonMonitor.h"
+#include "ctre/phoenix/Utilities.h"
+
+using namespace ctre;
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motion;
+using namespace ctre::phoenix::motorcontrol;
+using namespace ctre::phoenix::motorcontrol::can;
+using namespace ctre::phoenix::sensors;
+using namespace ctre::phoenix::signals;
+using namespace ctre::phoenix::tasking;
+using namespace ctre::phoenix::tasking::schedulers;
diff --git a/cpp/include/ctre/phoenix/CANifier.h b/cpp/include/ctre/phoenix/CANifier.h
new file mode 100644
index 0000000..5e83b22
--- /dev/null
+++ b/cpp/include/ctre/phoenix/CANifier.h
@@ -0,0 +1,140 @@
+#pragma once
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+#include <cstdint>
+#include "ctre/phoenix/LowLevel/CANBusAddressable.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/CANifierControlFrame.h"
+#include "ctre/phoenix/CANifierStatusFrame.h"
+#include "ctre/phoenix/CANifierStickyFaults.h"
+#include "ctre/phoenix/CANifierFaults.h"
+#include "ctre/phoenix/CANifierVelocityMeasPeriod.h"
+
+namespace ctre {namespace phoenix {
+	/**
+	 * CTRE CANifier
+	 *
+	 * Device for interfacing common devices to the CAN bus.
+	 */
+class CANifier: public CANBusAddressable {
+public:
+	/**
+	 * Enum for the LED Output Channels
+	 */
+	enum LEDChannel {
+		LEDChannelA = 0, LEDChannelB = 1, LEDChannelC = 2,
+	};
+
+	/**
+	 * Enum for the PWM Input Channels
+	 */
+	enum PWMChannel {
+		PWMChannel0 = 0, PWMChannel1 = 1, PWMChannel2 = 2, PWMChannel3 = 3,
+	};
+	const int PWMChannelCount = 4;
+
+	/**
+	 * General IO Pins on the CANifier
+	 */
+	enum GeneralPin {
+		QUAD_IDX = 0,	//----- Must match CANifier_CCI enums -----//
+		QUAD_B = 1,
+		QUAD_A = 2,
+		LIMR = 3,
+		LIMF = 4,
+		SDA = 5,
+		SCL = 6,
+		SPI_CS = 7,
+		SPI_MISO_PWM2P = 8,
+		SPI_MOSI_PWM1P = 9,
+		SPI_CLK_PWM0P = 10,
+	};
+
+	/**
+	 * Structure to hold the pin values.
+	 */
+	struct PinValues {
+		bool QUAD_IDX;
+		bool QUAD_B;
+		bool QUAD_A;
+		bool LIMR;
+		bool LIMF;
+		bool SDA;
+		bool SCL;
+		bool SPI_CS_PWM3;
+		bool SPI_MISO_PWM2;
+		bool SPI_MOSI_PWM1;
+		bool SPI_CLK_PWM0;
+	};
+
+	CANifier(int deviceNumber);
+	ErrorCode SetLEDOutput(double percentOutput, LEDChannel ledChannel);
+	ErrorCode SetGeneralOutput(GeneralPin outputPin, bool outputValue, bool outputEnable);
+	ErrorCode SetGeneralOutputs(int outputBits, int isOutputBits);
+	ErrorCode GetGeneralInputs(PinValues &allPins);
+	bool GetGeneralInput(GeneralPin inputPin);
+	int GetQuadraturePosition();
+	int GetQuadratureVelocity();
+	ErrorCode SetQuadraturePosition(int newPosition, int timeoutMs);
+	ErrorCode ConfigVelocityMeasurementPeriod(
+			CANifierVelocityMeasPeriod period, int timeoutMs);
+	ErrorCode ConfigVelocityMeasurementWindow(int windowSize, int timeoutMs);
+	/**
+	 * Gets the bus voltage seen by the motor controller.
+	 *
+	 * @return The bus voltage value (in volts).
+	 */
+	double GetBusVoltage();
+	ErrorCode GetLastError();
+	ErrorCode SetPWMOutput(int pwmChannel, double dutyCycle);
+	ErrorCode EnablePWMOutput(int pwmChannel, bool bEnable);
+	ErrorCode GetPWMInput(PWMChannel pwmChannel, double dutyCycleAndPeriod[]);
+
+	//------ Custom Persistent Params ----------//
+	ErrorCode ConfigSetCustomParam(int newValue, int paramIndex,
+			int timeoutMs);
+	int ConfigGetCustomParam(int paramIndex,
+			int timeoutMs);
+	//------ Generic Param API, typically not used ----------//
+	ErrorCode ConfigSetParameter(ParamEnum param, double value,
+			uint8_t subValue, int ordinal, int timeoutMs);
+	double ConfigGetParameter(ParamEnum param, int ordinal, int timeoutMs);
+
+
+	ErrorCode SetStatusFramePeriod(CANifierStatusFrame statusFrame,
+			int periodMs, int timeoutMs);
+	/**
+	 * Gets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame to get the period of.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Period of the given status frame.
+	 */
+	int GetStatusFramePeriod(CANifierStatusFrame frame, int timeoutMs);
+	ErrorCode SetControlFramePeriod(CANifierControlFrame frame, int periodMs);
+	/**
+	 * Gets the firmware version of the device.
+	 *
+	 * @return Firmware version of device.
+	 */
+	int GetFirmwareVersion();
+	/**
+	 * Returns true if the device has reset since last call.
+	 *
+	 * @return Has a Device Reset Occurred?
+	 */
+	bool HasResetOccurred();
+	ErrorCode GetFaults(CANifierFaults & toFill);
+	ErrorCode GetStickyFaults(CANifierStickyFaults & toFill);
+	ErrorCode ClearStickyFaults(int timeoutMs);
+
+private:
+	void* m_handle;
+	bool _tempPins[11];
+};
+}}
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/include/ctre/phoenix/CTRLogger.h b/cpp/include/ctre/phoenix/CTRLogger.h
new file mode 100644
index 0000000..91f8d43
--- /dev/null
+++ b/cpp/include/ctre/phoenix/CTRLogger.h
@@ -0,0 +1,16 @@
+#include "ctre/phoenix/ErrorCode.h" // ErrorCode
+#include <string>
+
+namespace ctre {
+namespace phoenix {
+
+class CTRLogger {
+public:
+	static void Close();
+	static ErrorCode Log(ErrorCode code, std::string origin);
+	static void Open(int language);
+	//static void Description(ErrorCode code, const char *&shrt, const char *&lng);
+};
+
+}
+}
diff --git a/cpp/include/ctre/phoenix/HsvToRgb.h b/cpp/include/ctre/phoenix/HsvToRgb.h
new file mode 100644
index 0000000..90a89ad
--- /dev/null
+++ b/cpp/include/ctre/phoenix/HsvToRgb.h
@@ -0,0 +1,13 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+
+class HsvToRgb {
+public:
+	static void Convert(double hDegrees, double S, double V, float* r, float* g,
+			float* b);
+};
+
+}
+}
diff --git a/cpp/include/ctre/phoenix/LinearInterpolation.h b/cpp/include/ctre/phoenix/LinearInterpolation.h
new file mode 100644
index 0000000..f445852
--- /dev/null
+++ b/cpp/include/ctre/phoenix/LinearInterpolation.h
@@ -0,0 +1,12 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+	
+class LinearInterpolation {
+public:
+	static float Calculate(float x, float x1, float y1, float x2, float y2);
+};
+
+}}
+
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h b/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h
new file mode 100644
index 0000000..6378f6f
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h
@@ -0,0 +1,236 @@
+#pragma once
+
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/core/GadgeteerUartClient.h"
+#include "ctre/phoenix/MotorControl/IMotorController.h"
+#include "ctre/phoenix/MotorControl/ControlMode.h"
+#include "ctre/phoenix/MotorControl/DemandType.h"
+#include "ctre/phoenix/MotorControl/Faults.h"
+#include "ctre/phoenix/MotorControl/FollowerType.h"
+#include "ctre/phoenix/MotorControl/StickyFaults.h"
+#include "ctre/phoenix/MotorControl/VelocityMeasPeriod.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+#include "ctre/phoenix/Motion/MotionProfileStatus.h"
+/* WPILIB */
+#include "SpeedController.h"
+
+/* forward proto's */
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace lowlevel {
+class MotControllerWithBuffer_LowLevel;
+class MotController_LowLevel;
+}
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+class SensorCollection;
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+/**
+ * Base motor controller features for all CTRE CAN motor controllers.
+ */
+class BaseMotorController: public virtual IMotorController {
+private:
+	ControlMode m_controlMode = ControlMode::PercentOutput;
+	ControlMode m_sendMode = ControlMode::PercentOutput;
+
+	int _arbId = 0;
+	double m_setPoint = 0;
+	bool _invert = false;
+
+	ctre::phoenix::motorcontrol::SensorCollection * _sensorColl;
+protected:
+	void* m_handle;
+	void* GetHandle();
+public:
+	BaseMotorController(int arbId);
+	~BaseMotorController();
+	BaseMotorController() = delete;
+	BaseMotorController(BaseMotorController const&) = delete;
+	BaseMotorController& operator=(BaseMotorController const&) = delete;
+	int GetDeviceID();
+	virtual void Set(ControlMode Mode, double value);
+	virtual void Set(ControlMode mode, double demand0, double demand1);
+	virtual void Set(ControlMode mode, double demand0, DemandType demand1Type, double demand1);
+	virtual void NeutralOutput();
+	virtual void SetNeutralMode(NeutralMode neutralMode);
+	void EnableHeadingHold(bool enable);
+	void SelectDemandType(bool value);
+	//------ Invert behavior ----------//
+	virtual void SetSensorPhase(bool PhaseSensor);
+	virtual void SetInverted(bool invert);
+	virtual bool GetInverted() const;
+	//----- general output shaping ------------------//
+	virtual ctre::phoenix::ErrorCode ConfigOpenloopRamp(double secondsFromNeutralToFull,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigClosedloopRamp(double secondsFromNeutralToFull,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigPeakOutputForward(double percentOut, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigPeakOutputReverse(double percentOut, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigNominalOutputForward(double percentOut,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigNominalOutputReverse(double percentOut,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigNeutralDeadband(double percentDeadband,
+			int timeoutMs);
+	//------ Voltage Compensation ----------//
+	virtual ctre::phoenix::ErrorCode ConfigVoltageCompSaturation(double voltage, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigVoltageMeasurementFilter(int filterWindowSamples,
+			int timeoutMs);
+	virtual void EnableVoltageCompensation(bool enable);
+	//------ General Status ----------//
+	virtual double GetBusVoltage();
+	virtual double GetMotorOutputPercent();
+	virtual double GetMotorOutputVoltage();
+	virtual double GetOutputCurrent();
+	virtual double GetTemperature();
+	//------ sensor selection ----------//
+	virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackSensor(
+			RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackSensor(
+			FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackCoefficient(
+			double coefficient, int pidIdx, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigRemoteFeedbackFilter(int deviceID,
+			RemoteSensorSource remoteSensorSource, int remoteOrdinal,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigSensorTerm(SensorTerm sensorTerm,
+			FeedbackDevice feedbackDevice, int timeoutMs);
+
+	//------- sensor status --------- //
+	virtual int GetSelectedSensorPosition(int pidIdx);
+	virtual int GetSelectedSensorVelocity(int pidIdx);
+	virtual ctre::phoenix::ErrorCode SetSelectedSensorPosition(int sensorPos, int pidIdx, int timeoutMs);
+	//------ status frame period changes ----------//
+	virtual ctre::phoenix::ErrorCode SetControlFramePeriod(ControlFrame frame, int periodMs);
+	virtual ctre::phoenix::ErrorCode SetStatusFramePeriod(StatusFrame frame, int periodMs,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode SetStatusFramePeriod(StatusFrameEnhanced frame,
+			int periodMs, int timeoutMs);
+	virtual int GetStatusFramePeriod(StatusFrame frame, int timeoutMs);
+	virtual int GetStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs);
+	//----- velocity signal conditionaing ------//
+	virtual ctre::phoenix::ErrorCode ConfigVelocityMeasurementPeriod(VelocityMeasPeriod period,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigVelocityMeasurementWindow(int windowSize,
+			int timeoutMs);
+	//------ remote limit switch ----------//
+	virtual ctre::phoenix::ErrorCode ConfigForwardLimitSwitchSource(
+			RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigReverseLimitSwitchSource(
+			RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs);
+	void OverrideLimitSwitchesEnable(bool enable);
+	//------ local limit switch ----------//
+	virtual ctre::phoenix::ErrorCode ConfigForwardLimitSwitchSource(LimitSwitchSource type,
+			LimitSwitchNormal normalOpenOrClose, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigReverseLimitSwitchSource(LimitSwitchSource type,
+			LimitSwitchNormal normalOpenOrClose, int timeoutMs);
+	//------ soft limit ----------//
+	virtual ctre::phoenix::ErrorCode ConfigForwardSoftLimitThreshold(int forwardSensorLimit,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigReverseSoftLimitThreshold(int reverseSensorLimit,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigForwardSoftLimitEnable(bool enable,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigReverseSoftLimitEnable(bool enable,
+			int timeoutMs);		
+	virtual void OverrideSoftLimitsEnable(bool enable);
+	//------ Current Lim ----------//
+	/* not available in base */
+	//------ General Close loop ----------//
+	virtual ctre::phoenix::ErrorCode Config_kP(int slotIdx, double value, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode Config_kI(int slotIdx, double value, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode Config_kD(int slotIdx, double value, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode Config_kF(int slotIdx, double value, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode Config_IntegralZone(int slotIdx, int izone,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigAllowableClosedloopError(int slotIdx,
+			int allowableCloseLoopError, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigMaxIntegralAccumulator(int slotIdx, double iaccum,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigClosedLoopPeakOutput(int slotIdx, double percentOut, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigClosedLoopPeriod(int slotIdx, int loopTimeMs, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigAuxPIDPolarity(bool invert, int timeoutMs);
+
+	//------ Close loop State ----------//
+	virtual ctre::phoenix::ErrorCode SetIntegralAccumulator(double iaccum, int pidIdx,int timeoutMs);
+	virtual int GetClosedLoopError(int pidIdx);
+	virtual double GetIntegralAccumulator(int pidIdx);
+	virtual double GetErrorDerivative(int pidIdx);
+
+	virtual ctre::phoenix::ErrorCode SelectProfileSlot(int slotIdx, int pidIdx);
+
+	virtual int GetClosedLoopTarget(int pidIdx);
+	virtual int GetActiveTrajectoryPosition();
+	virtual int GetActiveTrajectoryVelocity();
+	virtual double GetActiveTrajectoryHeading();
+
+	//------ Motion Profile Settings used in Motion Magic  ----------//
+	virtual ctre::phoenix::ErrorCode ConfigMotionCruiseVelocity(int sensorUnitsPer100ms,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigMotionAcceleration(int sensorUnitsPer100msPerSec,
+			int timeoutMs);
+	//------ Motion Profile Buffer ----------//
+	virtual ErrorCode ClearMotionProfileTrajectories();
+	virtual int GetMotionProfileTopLevelBufferCount();
+	virtual ctre::phoenix::ErrorCode PushMotionProfileTrajectory(
+			const ctre::phoenix::motion::TrajectoryPoint & trajPt);
+	virtual bool IsMotionProfileTopLevelBufferFull();
+	virtual void ProcessMotionProfileBuffer();
+	virtual ctre::phoenix::ErrorCode GetMotionProfileStatus(
+			ctre::phoenix::motion::MotionProfileStatus & statusToFill);
+	virtual ctre::phoenix::ErrorCode ClearMotionProfileHasUnderrun(int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ChangeMotionControlFramePeriod(int periodMs);
+	virtual ctre::phoenix::ErrorCode ConfigMotionProfileTrajectoryPeriod(int baseTrajDurationMs, int timeoutMs);
+	//------ error ----------//
+	virtual ctre::phoenix::ErrorCode GetLastError();
+	//------ Faults ----------//
+	virtual ctre::phoenix::ErrorCode GetFaults(Faults & toFill);
+	virtual ctre::phoenix::ErrorCode GetStickyFaults(StickyFaults & toFill);
+	virtual ctre::phoenix::ErrorCode ClearStickyFaults(int timeoutMs);
+	//------ Firmware ----------//
+	virtual int GetFirmwareVersion();
+	virtual bool HasResetOccurred();
+	//------ Custom Persistent Params ----------//
+	virtual ctre::phoenix::ErrorCode ConfigSetCustomParam(int newValue, int paramIndex,
+			int timeoutMs);
+	virtual int ConfigGetCustomParam(int paramIndex,
+			int timeoutMs);
+	//------ Generic Param API, typically not used ----------//
+	virtual ctre::phoenix::ErrorCode ConfigSetParameter(ctre::phoenix::ParamEnum param, double value,
+			uint8_t subValue, int ordinal, int timeoutMs);
+	virtual double ConfigGetParameter(ctre::phoenix::ParamEnum param, int ordinal, int timeoutMs);
+	//------ Misc. ----------//
+	virtual int GetBaseID();
+	virtual ControlMode GetControlMode();
+	// ----- Follower ------//
+	void Follow(IMotorController & masterToFollow, ctre::phoenix::motorcontrol::FollowerType followerType);
+	virtual void Follow(IMotorController & masterToFollow);
+	virtual void ValueUpdated();
+
+	//------ RAW Sensor API ----------//
+	/**
+	 * @retrieve object that can get/set individual RAW sensor values.
+	 */
+	ctre::phoenix::motorcontrol::SensorCollection & GetSensorCollection();
+};
+
+} // namespace can
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/TalonSRX.h b/cpp/include/ctre/phoenix/MotorControl/CAN/TalonSRX.h
new file mode 100644
index 0000000..7054efa
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/TalonSRX.h
@@ -0,0 +1,65 @@
+#pragma once
+
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "ctre/phoenix/MotorControl/IMotorControllerEnhanced.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+
+/**
+ * CTRE Talon SRX Motor Controller when used on CAN Bus.
+ */
+class TalonSRX: public virtual BaseMotorController,
+		public virtual IMotorControllerEnhanced {
+public:
+	TalonSRX(int deviceNumber);
+	virtual ~TalonSRX() {
+	}
+	TalonSRX() = delete;
+	TalonSRX(TalonSRX const&) = delete;
+	TalonSRX& operator=(TalonSRX const&) = delete;
+
+	virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackSensor(FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackSensor(RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs);
+
+	virtual ctre::phoenix::ErrorCode SetStatusFramePeriod(StatusFrameEnhanced frame,int periodMs, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode SetStatusFramePeriod(StatusFrame frame,int periodMs, int timeoutMs);
+
+	virtual int GetStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs);
+	virtual int GetStatusFramePeriod(StatusFrame frame, int timeoutMs);
+
+	//------ Velocity measurement ----------//
+	virtual ctre::phoenix::ErrorCode ConfigVelocityMeasurementPeriod(VelocityMeasPeriod period,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigVelocityMeasurementWindow(int windowSize,
+			int timeoutMs);
+
+	//------ limit switch ----------//
+	virtual ctre::phoenix::ErrorCode ConfigForwardLimitSwitchSource(
+			LimitSwitchSource limitSwitchSource,
+			LimitSwitchNormal normalOpenOrClose, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigReverseLimitSwitchSource(
+			LimitSwitchSource limitSwitchSource,
+			LimitSwitchNormal normalOpenOrClose, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigForwardLimitSwitchSource(
+			RemoteLimitSwitchSource limitSwitchSource,
+			LimitSwitchNormal normalOpenOrClose, int deviceID, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigReverseLimitSwitchSource(
+			RemoteLimitSwitchSource limitSwitchSource,
+			LimitSwitchNormal normalOpenOrClose, int deviceID, int timeoutMs);
+
+	//------ Current Limit ----------//
+	virtual ctre::phoenix::ErrorCode ConfigPeakCurrentLimit(int amps, int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigPeakCurrentDuration(int milliseconds,
+			int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ConfigContinuousCurrentLimit(int amps, int timeoutMs);
+	virtual void EnableCurrentLimit(bool enable);
+
+};
+// class TalonSRX
+} // namespace can
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/VictorSPX.h b/cpp/include/ctre/phoenix/MotorControl/CAN/VictorSPX.h
new file mode 100644
index 0000000..a209b85
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/VictorSPX.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "ctre/phoenix/MotorControl/IMotorController.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol{
+namespace can {
+
+/**
+ * VEX Victor SPX Motor Controller when used on CAN Bus.
+ */
+class VictorSPX: public virtual ctre::phoenix::motorcontrol::can::BaseMotorController,
+                 public virtual ctre::phoenix::motorcontrol::IMotorController {
+
+public:
+	VictorSPX(int deviceNumber);
+	virtual ~VictorSPX() {
+	}
+	VictorSPX(VictorSPX const&) = delete;
+	VictorSPX& operator=(VictorSPX const&) = delete;
+}; //class VictorSPX
+
+} //namespace can
+} //namespace motorcontrol
+} //namespace phoenix
+} //namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h b/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h
new file mode 100644
index 0000000..6174ce8
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h
@@ -0,0 +1,127 @@
+/**
+ * WPI Compliant motor controller class.
+ * WPILIB's object model requires many interfaces to be implemented to use
+ * the various features.
+ * This includes...
+ * - Software PID loops running in the robot controller
+ * - LiveWindow/Test mode features
+ * - Motor Safety (auto-turn off of motor if Set stops getting called)
+ * - Single Parameter set that assumes a simple motor controller.
+ */
+#pragma once
+
+#include "ctre/phoenix/MotorControl/CAN/TalonSRX.h"
+#include "SmartDashboard/SendableBase.h"
+#include "SmartDashboard/SendableBuilder.h"
+#include "SpeedController.h"
+#include "MotorSafety.h"
+#include "MotorSafetyHelper.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+
+
+class WPI_TalonSRX: public virtual TalonSRX,
+		public virtual frc::SpeedController,
+		public frc::SendableBase,
+		public frc::MotorSafety {
+public:
+	WPI_TalonSRX(int deviceNumber);
+	virtual ~WPI_TalonSRX();
+
+	WPI_TalonSRX() = delete;
+	WPI_TalonSRX(WPI_TalonSRX const&) = delete;
+	WPI_TalonSRX& operator=(WPI_TalonSRX const&) = delete;
+
+	//----------------------- set/get routines for WPILIB interfaces -------------------//
+	/**
+	 * Common interface for setting the speed of a simple speed controller.
+	 *
+	 * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+	 * 									Value is also saved for Get().
+	 */
+	virtual void Set(double speed);
+	virtual void PIDWrite(double output);
+
+	/**
+	 * Common interface for getting the current set speed of a speed controller.
+	 *
+	 * @return The current set speed.  Value is between -1.0 and 1.0.
+	 */
+	virtual double Get() const;
+
+	//----------------------- Intercept CTRE calls for motor safety -------------------//
+	virtual void Set(ControlMode mode, double value);
+	virtual void Set(ControlMode mode, double demand0, double demand1);
+	//----------------------- Invert routines -------------------//
+	/**
+	 * Common interface for inverting direction of a speed controller.
+	 *
+	 * @param isInverted The state of inversion, true is inverted.
+	 */
+	virtual void SetInverted(bool isInverted);
+	/**
+	 * Common interface for returning the inversion state of a speed controller.
+	 *
+	 * @return isInverted The state of inversion, true is inverted.
+	 */
+	virtual bool GetInverted() const;
+	//----------------------- turn-motor-off routines-------------------//
+	/**
+	 * Common interface for disabling a motor.
+	 */
+	virtual void Disable();
+	/**
+	 * Common interface to stop the motor until Set is called again.
+	 */
+	virtual void StopMotor();
+
+	//----------------------- Motor Safety-------------------//
+
+	/**
+	 * Set the safety expiration time.
+	 *
+	 * @param timeout The timeout (in seconds) for this motor object
+	 */
+	void SetExpiration(double timeout);
+
+	/**
+	 * Return the safety expiration time.
+	 *
+	 * @return The expiration time value.
+	 */
+	double GetExpiration() const;
+
+	/**
+	 * Check if the motor is currently alive or stopped due to a timeout.
+	 *
+	 * @return a bool value that is true if the motor has NOT timed out and should
+	 *         still be running.
+	 */
+	bool IsAlive() const;
+
+	/**
+	 * Check if motor safety is enabled.
+	 *
+	 * @return True if motor safety is enforced for this object
+	 */
+	bool IsSafetyEnabled() const;
+
+	void SetSafetyEnabled(bool enabled);
+
+	void GetDescription(llvm::raw_ostream& desc) const;
+
+protected:
+	virtual void InitSendable(frc::SendableBuilder& builder);
+private:
+	double _speed = 0;
+	std::string _desc;
+	frc::MotorSafetyHelper _safetyHelper;
+};
+
+} // namespace can
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h b/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h
new file mode 100644
index 0000000..f0c8cd0
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h
@@ -0,0 +1,127 @@
+/**
+ * WPI Compliant motor controller class.
+ * WPILIB's object model requires many interfaces to be implemented to use
+ * the various features.
+ * This includes...
+ * - Software PID loops running in the robot controller
+ * - LiveWindow/Test mode features
+ * - Motor Safety (auto-turn off of motor if Set stops getting called)
+ * - Single Parameter set that assumes a simple motor controller.
+ */
+#pragma once
+
+#include "ctre/phoenix/MotorControl/CAN/VictorSPX.h"
+#include "SmartDashboard/SendableBase.h"
+#include "SmartDashboard/SendableBuilder.h"
+#include "SpeedController.h"
+#include "MotorSafety.h"
+#include "MotorSafetyHelper.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+
+
+class WPI_VictorSPX: public virtual VictorSPX,
+		public virtual frc::SpeedController,
+		public frc::SendableBase,
+		public frc::MotorSafety {
+public:
+	WPI_VictorSPX(int deviceNumber);
+	virtual ~WPI_VictorSPX();
+
+	WPI_VictorSPX() = delete;
+	WPI_VictorSPX(WPI_VictorSPX const&) = delete;
+	WPI_VictorSPX& operator=(WPI_VictorSPX const&) = delete;
+
+	//----------------------- set/get routines for WPILIB interfaces -------------------//
+	/**
+	 * Common interface for setting the speed of a simple speed controller.
+	 *
+	 * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+	 * 									Value is also saved for Get().
+	 */
+	virtual void Set(double speed);
+	virtual void PIDWrite(double output);
+
+	/**
+	 * Common interface for getting the current set speed of a speed controller.
+	 *
+	 * @return The current set speed.  Value is between -1.0 and 1.0.
+	 */
+	virtual double Get() const;
+
+	//----------------------- Intercept CTRE calls for motor safety -------------------//
+	virtual void Set(ControlMode mode, double value);
+	virtual void Set(ControlMode mode, double demand0, double demand1);
+	//----------------------- Invert routines -------------------//
+	/**
+	 * Common interface for inverting direction of a speed controller.
+	 *
+	 * @param isInverted The state of inversion, true is inverted.
+	 */
+	virtual void SetInverted(bool isInverted);
+	/**
+	 * Common interface for returning the inversion state of a speed controller.
+	 *
+	 * @return isInverted The state of inversion, true is inverted.
+	 */
+	virtual bool GetInverted() const;
+	//----------------------- turn-motor-off routines-------------------//
+	/**
+	 * Common interface for disabling a motor.
+	 */
+	virtual void Disable();
+	/**
+	 * Common interface to stop the motor until Set is called again.
+	 */
+	virtual void StopMotor();
+
+	//----------------------- Motor Safety-------------------//
+
+	/**
+	 * Set the safety expiration time.
+	 *
+	 * @param timeout The timeout (in seconds) for this motor object
+	 */
+	void SetExpiration(double timeout);
+
+	/**
+	 * Return the safety expiration time.
+	 *
+	 * @return The expiration time value.
+	 */
+	double GetExpiration() const;
+
+	/**
+	 * Check if the motor is currently alive or stopped due to a timeout.
+	 *
+	 * @return a bool value that is true if the motor has NOT timed out and should
+	 *         still be running.
+	 */
+	bool IsAlive() const;
+
+	/**
+	 * Check if motor safety is enabled.
+	 *
+	 * @return True if motor safety is enforced for this object
+	 */
+	bool IsSafetyEnabled() const;
+
+	void SetSafetyEnabled(bool enabled);
+
+	void GetDescription(llvm::raw_ostream& desc) const;
+
+protected:
+	virtual void InitSendable(frc::SendableBuilder& builder);
+private:
+	double _speed = 0;
+	std::string _desc;
+	frc::MotorSafetyHelper _safetyHelper;
+};
+
+} // namespace can
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/DeviceCatalog.h b/cpp/include/ctre/phoenix/MotorControl/DeviceCatalog.h
new file mode 100644
index 0000000..528eecd
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/DeviceCatalog.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include "IMotorController.h"
+#include <vector>
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class DeviceCatalog {
+public:
+	void Register(IMotorController *motorController) {
+		_mcs.push_back(motorController);
+	}
+
+	int MotorControllerCount() {
+		return _mcs.size();
+	}
+
+	IMotorController* Get(int idx) {
+		return _mcs[idx];
+	}
+
+	DeviceCatalog & GetInstance() {
+		if (!_instance)
+			_instance = new DeviceCatalog();
+		return *_instance;
+	}
+private:
+	std::vector<IMotorController*> _mcs;
+
+	static DeviceCatalog * _instance;
+};
+
+}
+} // namespace phoenix
+}
+
diff --git a/cpp/include/ctre/phoenix/MotorControl/GroupMotorControllers.h b/cpp/include/ctre/phoenix/MotorControl/GroupMotorControllers.h
new file mode 100644
index 0000000..9ab95bd
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/GroupMotorControllers.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "IMotorController.h"
+#include <vector>
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class GroupMotorControllers {
+public:
+	static void Register(IMotorController *motorController);
+	static int MotorControllerCount();
+	static IMotorController* Get(int idx);
+
+private:
+	static std::vector<IMotorController*> _mcs;
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
+
diff --git a/cpp/include/ctre/phoenix/MotorControl/IFollower.h b/cpp/include/ctre/phoenix/MotorControl/IFollower.h
new file mode 100644
index 0000000..8f52c3d
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/IFollower.h
@@ -0,0 +1,19 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+/* forward proto */
+class IMotorController;
+
+class IFollower {
+public:
+	virtual ~IFollower(){}
+	virtual void Follow(ctre::phoenix::motorcontrol::IMotorController & masterToFollow) = 0;
+	virtual void ValueUpdated()= 0;
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/IMotorController.h b/cpp/include/ctre/phoenix/MotorControl/IMotorController.h
new file mode 100644
index 0000000..947022e
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/IMotorController.h
@@ -0,0 +1,205 @@
+#pragma once
+
+#include "ctre/phoenix/MotorControl/ControlMode.h"
+#include "ctre/phoenix/MotorControl/ControlFrame.h"
+#include "ctre/phoenix/MotorControl/DemandType.h"
+#include "ctre/phoenix/MotorControl/NeutralMode.h"
+#include "ctre/phoenix/MotorControl/FeedbackDevice.h"
+#include "ctre/phoenix/MotorControl/RemoteSensorSource.h"
+#include "ctre/phoenix/MotorControl/SensorTerm.h"
+#include "ctre/phoenix/MotorControl/StatusFrame.h"
+#include "ctre/phoenix/MotorControl/LimitSwitchType.h"
+#include "ctre/phoenix/MotorControl/Faults.h"
+#include "ctre/phoenix/MotorControl/StickyFaults.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+#include "ctre/phoenix/Motion/MotionProfileStatus.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "IFollower.h"
+/* WPILIB */
+#include "SpeedController.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class IMotorController: public virtual IFollower {
+public:
+	virtual ~IMotorController() {
+	}
+	//------ Set output routines. ----------//
+	virtual void Set(ControlMode Mode, double demand) = 0;
+	virtual void Set(ControlMode Mode, double demand0, double demand1) = 0;
+	virtual void Set(ControlMode mode, double demand0, DemandType demand1Type, double demand1) = 0;
+	virtual void NeutralOutput() = 0;
+	virtual void SetNeutralMode(NeutralMode neutralMode) = 0;
+
+	//------ Invert behavior ----------//
+	virtual void SetSensorPhase(bool PhaseSensor) = 0;
+	virtual void SetInverted(bool invert) = 0;
+	virtual bool GetInverted() const = 0;
+
+	//----- general output shaping ------------------//
+	virtual ErrorCode ConfigOpenloopRamp(double secondsFromNeutralToFull,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigClosedloopRamp(double secondsFromNeutralToFull,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigPeakOutputForward(double percentOut,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigPeakOutputReverse(double percentOut,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigNominalOutputForward(double percentOut,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigNominalOutputReverse(double percentOut,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigNeutralDeadband(double percentDeadband,
+			int timeoutMs) = 0;
+
+	//------ Voltage Compensation ----------//
+	virtual ErrorCode ConfigVoltageCompSaturation(double voltage,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigVoltageMeasurementFilter(int filterWindowSamples,
+			int timeoutMs) = 0;
+	virtual void EnableVoltageCompensation(bool enable) = 0;
+
+	//------ General Status ----------//
+	virtual double GetBusVoltage() = 0;
+	virtual double GetMotorOutputPercent() = 0;
+	virtual double GetMotorOutputVoltage() = 0;
+	virtual double GetOutputCurrent() = 0;
+	virtual double GetTemperature() = 0;
+
+	//------ sensor selection ----------//
+	virtual ErrorCode ConfigSelectedFeedbackSensor(
+			RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) = 0;
+	virtual ErrorCode ConfigSelectedFeedbackCoefficient(
+			double coefficient, int pidIdx, int timeoutMs) = 0;
+	virtual ErrorCode ConfigRemoteFeedbackFilter(int deviceID,
+			RemoteSensorSource remoteSensorSource, int remoteOrdinal,
+			int timeoutMs)= 0;
+	virtual ErrorCode ConfigSensorTerm(SensorTerm sensorTerm, FeedbackDevice feedbackDevice, int timeoutMs)= 0;
+
+	//------- sensor status --------- //
+	virtual int GetSelectedSensorPosition(int pidIdx) = 0;
+	virtual int GetSelectedSensorVelocity(int pidIdx) = 0;
+	virtual ErrorCode SetSelectedSensorPosition(int sensorPos, int pidIdx,
+			int timeoutMs) = 0;
+
+	//------ status frame period changes ----------//
+	virtual ErrorCode SetControlFramePeriod(ControlFrame frame,
+			int periodMs) = 0;
+	virtual ErrorCode SetStatusFramePeriod(StatusFrame frame, int periodMs,
+			int timeoutMs) = 0;
+	virtual int GetStatusFramePeriod(StatusFrame frame, int timeoutMs) = 0;
+
+	//----- velocity signal conditionaing ------//
+	/* not supported */
+
+	//------ remote limit switch ----------//
+	virtual ErrorCode ConfigForwardLimitSwitchSource(
+			RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs) = 0;
+	virtual ErrorCode ConfigReverseLimitSwitchSource(
+			RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs) = 0;
+	virtual void OverrideLimitSwitchesEnable(bool enable) = 0;
+
+	//------ local limit switch ----------//
+	/* not supported */
+
+	//------ soft limit ----------//
+	virtual ErrorCode ConfigForwardSoftLimitThreshold(int forwardSensorLimit,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigReverseSoftLimitThreshold(int reverseSensorLimit,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigForwardSoftLimitEnable(bool enable,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigReverseSoftLimitEnable(bool enable,
+			int timeoutMs) = 0;
+	virtual void OverrideSoftLimitsEnable(bool enable) = 0;
+
+	//------ Current Lim ----------//
+	/* not supported */
+
+	//------ Config Close loop ----------//
+	virtual ErrorCode Config_kP(int slotIdx, double value, int timeoutMs) = 0;
+	virtual ErrorCode Config_kI(int slotIdx, double value, int timeoutMs) = 0;
+	virtual ErrorCode Config_kD(int slotIdx, double value, int timeoutMs) = 0;
+	virtual ErrorCode Config_kF(int slotIdx, double value, int timeoutMs) = 0;
+	virtual ErrorCode Config_IntegralZone(int slotIdx, int izone,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigAllowableClosedloopError(int slotIdx,
+			int allowableCloseLoopError, int timeoutMs) = 0;
+	virtual ErrorCode ConfigMaxIntegralAccumulator(int slotIdx, double iaccum,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigClosedLoopPeakOutput(int slotIdx, double percentOut, int timeoutMs) = 0;
+	virtual ErrorCode ConfigClosedLoopPeriod(int slotIdx, int loopTimeMs, int timeoutMs) = 0;
+  virtual ErrorCode ConfigAuxPIDPolarity(bool invert, int timeoutMs) = 0;
+
+	//------ Close loop State ----------//
+	virtual ErrorCode SetIntegralAccumulator(double iaccum, int pidIdx,
+			int timeoutMs) = 0;
+	virtual int GetClosedLoopError(int pidIdx) = 0;
+	virtual double GetIntegralAccumulator(int pidIdx) = 0;
+	virtual double GetErrorDerivative(int pidIdx) = 0;
+
+	virtual ErrorCode SelectProfileSlot(int slotIdx, int pidIdx) = 0;
+
+	virtual int GetClosedLoopTarget(int pidIdx) = 0;
+	virtual int GetActiveTrajectoryPosition() = 0;
+	virtual int GetActiveTrajectoryVelocity() = 0;
+	virtual double GetActiveTrajectoryHeading() = 0;
+
+	//------ Motion Profile Settings used in Motion Magic  ----------//
+	virtual ErrorCode ConfigMotionCruiseVelocity(int sensorUnitsPer100ms,
+			int timeoutMs) = 0;
+	virtual ErrorCode ConfigMotionAcceleration(int sensorUnitsPer100msPerSec,
+			int timeoutMs) = 0;
+
+	//------ Motion Profile Buffer ----------//
+	virtual ErrorCode ClearMotionProfileTrajectories()= 0;
+	virtual int GetMotionProfileTopLevelBufferCount()= 0;
+	virtual ErrorCode PushMotionProfileTrajectory(
+			const ctre::phoenix::motion::TrajectoryPoint & trajPt)= 0;
+	virtual bool IsMotionProfileTopLevelBufferFull()= 0;
+	virtual void ProcessMotionProfileBuffer()= 0;
+	virtual ErrorCode GetMotionProfileStatus(
+			ctre::phoenix::motion::MotionProfileStatus & statusToFill)= 0;
+	virtual ErrorCode ClearMotionProfileHasUnderrun(int timeoutMs)= 0;
+	virtual ErrorCode ChangeMotionControlFramePeriod(int periodMs)= 0;
+	virtual ErrorCode ConfigMotionProfileTrajectoryPeriod(int baseTrajDurationMs, int timeoutMs)=0;
+	//------ error ----------//
+	virtual ErrorCode GetLastError() = 0;
+
+	//------ Faults ----------//
+	virtual ErrorCode GetFaults(Faults & toFill) = 0;
+	virtual ErrorCode GetStickyFaults(StickyFaults & toFill) = 0;
+	virtual ErrorCode ClearStickyFaults(int timeoutMs) = 0;
+
+	//------ Firmware ----------//
+	virtual int GetFirmwareVersion() = 0;
+	virtual bool HasResetOccurred() = 0;
+
+	//------ Custom Persistent Params ----------//
+	virtual ErrorCode ConfigSetCustomParam(int newValue, int paramIndex,
+			int timeoutMs) = 0;
+	virtual int ConfigGetCustomParam(int paramIndex, int timeoutMs) = 0;
+
+	//------ Generic Param API, typically not used ----------//
+	virtual ErrorCode ConfigSetParameter(ParamEnum param, double value,
+			uint8_t subValue, int ordinal, int timeoutMs) = 0;
+	virtual double ConfigGetParameter(ParamEnum paramEnum, int ordinal,
+			int timeoutMs) = 0;
+
+	//------ Misc. ----------//
+	virtual int GetBaseID() = 0;
+	virtual int GetDeviceID() = 0;
+	virtual ControlMode GetControlMode() = 0;
+
+	// ----- Follower ------//
+	/* in parent interface */
+};
+
+}
+} // namespace phoenix
+}
diff --git a/cpp/include/ctre/phoenix/MotorControl/IMotorControllerEnhanced.h b/cpp/include/ctre/phoenix/MotorControl/IMotorControllerEnhanced.h
new file mode 100644
index 0000000..e5c429c
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/IMotorControllerEnhanced.h
@@ -0,0 +1,119 @@
+#pragma once
+
+#include "ctre/phoenix/MotorControl/ControlMode.h"
+#include "ctre/phoenix/MotorControl/ControlFrame.h"
+#include "ctre/phoenix/MotorControl/NeutralMode.h"
+#include "ctre/phoenix/MotorControl/FeedbackDevice.h"
+#include "ctre/phoenix/MotorControl/StatusFrame.h"
+#include "ctre/phoenix/MotorControl/LimitSwitchType.h"
+#include "ctre/phoenix/MotorControl/Faults.h"
+#include "ctre/phoenix/MotorControl/StickyFaults.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+#include "ctre/phoenix/Motion/MotionProfileStatus.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "IFollower.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class IMotorControllerEnhanced: public virtual IMotorController {
+public:
+	virtual ~IMotorControllerEnhanced() {
+	}
+
+	//------ Set output routines. ----------//
+	/* in parent */
+
+	//------ Invert behavior ----------//
+	/* in parent */
+
+	//----- general output shaping ------------------//
+	/* in parent */
+
+	//------ Voltage Compensation ----------//
+	/* in parent */
+
+	//------ General Status ----------//
+	/* in parent */
+
+	//------ sensor selection ----------//
+	/* expand the options */
+	virtual ErrorCode ConfigSelectedFeedbackSensor(
+			FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) = 0;
+	virtual ErrorCode ConfigSelectedFeedbackSensor(
+			RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) = 0;
+
+	//------- sensor status --------- //
+	/* in parent */
+
+	//------ status frame period changes ----------//
+	virtual ErrorCode SetStatusFramePeriod(StatusFrame frame, int periodMs,
+			int timeoutMs) = 0;
+	virtual ErrorCode SetStatusFramePeriod(StatusFrameEnhanced frame,
+			int periodMs, int timeoutMs) = 0;
+	virtual int GetStatusFramePeriod(StatusFrame frame, int timeoutMs) = 0;
+	virtual int GetStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs) = 0;
+
+	//----- velocity signal conditionaing ------//
+	virtual ErrorCode ConfigVelocityMeasurementPeriod(VelocityMeasPeriod period,
+			int timeoutMs)= 0;
+	virtual ErrorCode ConfigVelocityMeasurementWindow(int windowSize,
+			int timeoutMs)= 0;
+
+	//------ remote limit switch ----------//
+	virtual ErrorCode ConfigForwardLimitSwitchSource(
+			RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs) = 0;
+	virtual ErrorCode ConfigReverseLimitSwitchSource(
+			RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs) = 0;
+
+	//------ local limit switch ----------//
+	virtual ErrorCode ConfigForwardLimitSwitchSource(LimitSwitchSource type,
+			LimitSwitchNormal normalOpenOrClose, int timeoutMs)= 0;
+	virtual ErrorCode ConfigReverseLimitSwitchSource(LimitSwitchSource type,
+			LimitSwitchNormal normalOpenOrClose, int timeoutMs)= 0;
+
+	//------ soft limit ----------//
+	/* in parent */
+
+	//------ Current Lim ----------//
+	virtual ErrorCode ConfigPeakCurrentLimit(int amps, int timeoutMs)= 0;
+	virtual ErrorCode ConfigPeakCurrentDuration(int milliseconds,
+			int timeoutMs)= 0;
+	virtual ErrorCode ConfigContinuousCurrentLimit(int amps, int timeoutMs)= 0;
+	virtual void EnableCurrentLimit(bool enable)= 0;
+
+	//------ General Close loop ----------//
+	/* in parent */
+
+	//------ Motion Profile Settings used in Motion Magic and Motion Profile ----------//
+	/* in parent */
+
+	//------ Motion Profile Buffer ----------//
+	/* in parent */
+
+	//------ error ----------//
+	/* in parent */
+
+	//------ Faults ----------//
+	/* in parent */
+
+	//------ Firmware ----------//
+	/* in parent */
+
+	//------ Custom Persistent Params ----------//
+	/* in parent */
+
+	//------ Generic Param API, typically not used ----------//
+	/* in parent */
+
+	//------ Misc. ----------//
+	/* in parent */
+
+}; // class IMotorControllerEnhanced
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/SensorCollection.h b/cpp/include/ctre/phoenix/MotorControl/SensorCollection.h
new file mode 100644
index 0000000..63b5894
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/SensorCollection.h
@@ -0,0 +1,197 @@
+#pragma once
+
+#include "ctre/phoenix/ErrorCode.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+class BaseMotorController;
+}
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class SensorCollection {
+public:
+
+	/**
+	 * Get the position of whatever is in the analog pin of the Talon, regardless of
+	 *   whether it is actually being used for feedback.
+	 *
+	 * @return  the 24bit analog value.  The bottom ten bits is the ADC (0 - 1023)
+	 *          on the analog pin of the Talon. The upper 14 bits tracks the overflows and underflows
+	 *          (continuous sensor).
+	 */
+
+	int GetAnalogIn();
+
+	/**
+	 * Sets analog position.
+	 *
+	 * @param   newPosition The new position.
+	 * @param   timeoutMs   
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 *
+	 * @return  an ErrorCode.
+	 */
+
+	ErrorCode SetAnalogPosition(int newPosition, int timeoutMs);
+
+	/**
+	 * Get the position of whatever is in the analog pin of the Talon, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the ADC (0 - 1023) on analog pin of the Talon.
+	 */
+
+	int GetAnalogInRaw();
+
+	/**
+	 * Get the velocity of whatever is in the analog pin of the Talon, regardless of
+	 *   whether it is actually being used for feedback.
+	 *
+	 * @return  the speed in units per 100ms where 1024 units is one rotation.
+	 */
+
+	int GetAnalogInVel();
+
+	/**
+	 * Get the quadrature position of the Talon, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the quadrature position.
+	 */
+
+	int GetQuadraturePosition();
+
+	/**
+	 * Change the quadrature reported position.  Typically this is used to "zero" the
+	 *   sensor. This only works with Quadrature sensor.  To set the selected sensor position
+	 *   regardless of what type it is, see SetSelectedSensorPosition in the motor controller class.
+	 *
+	 * @param   newPosition The position value to apply to the sensor.
+	 * @param   timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 *
+	 * @return  error code.
+	 */
+
+	ErrorCode SetQuadraturePosition(int newPosition, int timeoutMs);
+
+	/**
+	 * Get the quadrature velocity, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the quadrature velocity in units per 100ms.
+	 */
+
+	int GetQuadratureVelocity();
+
+	/**
+	 * Gets pulse width position, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the pulse width position.
+	 */
+
+	int GetPulseWidthPosition();
+
+	/**
+	 * Sets pulse width position.
+	 *
+	 * @param   newPosition The position value to apply to the sensor.
+	 * @param   timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 *
+	 * @return  an ErrErrorCode
+	 */
+	ErrorCode SetPulseWidthPosition(int newPosition, int timeoutMs);
+
+	/**
+	 * Gets pulse width velocity, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the pulse width velocity in units per 100ms (where 4096 units is 1 rotation).
+	 */
+
+	int GetPulseWidthVelocity();
+
+	/**
+	 * Gets pulse width rise to fall time.
+	 *
+	 * @return  the pulse width rise to fall time in microseconds.
+	 */
+
+	int GetPulseWidthRiseToFallUs();
+
+	/**
+	 * Gets pulse width rise to rise time.
+	 *
+	 * @return  the pulse width rise to rise time in microseconds.
+	 */
+
+	int GetPulseWidthRiseToRiseUs();
+
+	/**
+	 * Gets pin state quad a.
+	 *
+	 * @return  the pin state of quad a (1 if asserted, 0 if not asserted).
+	 */
+
+	int GetPinStateQuadA();
+
+	/**
+	 * Gets pin state quad b.
+	 *
+	 * @return  Digital level of QUADB pin (1 if asserted, 0 if not asserted).
+	 */
+
+	int GetPinStateQuadB();
+
+	/**
+	 * Gets pin state quad index.
+	 *
+	 * @return  Digital level of QUAD Index pin (1 if asserted, 0 if not asserted).
+	 */
+
+	int GetPinStateQuadIdx();
+
+	/**
+	 * Is forward limit switch closed.
+	 *
+	 * @return  '1' iff forward limit switch is closed, 0 iff switch is open. This function works
+	 *          regardless if limit switch feature is enabled.
+	 */
+
+	int IsFwdLimitSwitchClosed();
+
+	/**
+	 * Is reverse limit switch closed.
+	 *
+	 * @return  '1' iff reverse limit switch is closed, 0 iff switch is open. This function works
+	 *          regardless if limit switch feature is enabled.
+	 */
+
+	int IsRevLimitSwitchClosed();
+
+private:
+	SensorCollection(void * handle);
+	friend class ctre::phoenix::motorcontrol::can::BaseMotorController;
+	void* _handle;
+
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/RCRadio3Ch.h b/cpp/include/ctre/phoenix/RCRadio3Ch.h
new file mode 100644
index 0000000..aaea7b4
--- /dev/null
+++ b/cpp/include/ctre/phoenix/RCRadio3Ch.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <vector>
+#include "ctre/phoenix/CANifier.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+
+namespace ctre{
+namespace phoenix {
+	
+class RCRadio3Ch : public ctre::phoenix::tasking::IProcessable{
+public:
+	enum Channel{
+		Channel1,
+		Channel2,
+		Channel3,
+	};
+	enum Status{
+		LossOfCAN,
+		LossOfPwm,
+		Okay,
+	};
+	Status CurrentStatus = Status::Okay;
+
+	RCRadio3Ch(ctre::phoenix::CANifier *canifier);
+	float GetDutyCycleUs(Channel channel);
+	float GetDutyCyclePerc(Channel channel);
+	bool GetSwitchValue(Channel channel);
+	float GetPeriodUs(Channel channel);
+
+	//ILoopable
+	void Process();
+
+private:
+	int _errorCodes[4];
+	ctre::phoenix::CANifier *_canifier;
+
+
+	//This is only a 2d array??
+	double _dutyCycleAndPeriods[4][2] =
+	{
+			{ 0, 0 },
+			{ 0, 0 },
+			{ 0, 0 },
+			{ 0, 0 },
+	};
+	double Interpolate(std::vector<double> &xData, std::vector<double> &yData, double x, bool extrapolate);
+};
+
+}}
diff --git a/cpp/include/ctre/phoenix/Sensors/PigeonIMU.h b/cpp/include/ctre/phoenix/Sensors/PigeonIMU.h
new file mode 100644
index 0000000..a360f74
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Sensors/PigeonIMU.h
@@ -0,0 +1,303 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ *
+ * Cross The Road Electronics (CTRE) licenses to you the right to
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ *
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+
+#pragma once
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+#include <string>
+#include "ctre/phoenix/LowLevel/CANBusAddressable.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_ControlFrame.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_Faults.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_StatusFrame.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_StickyFaults.h"
+
+/* forward prototype */
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+class TalonSRX;
+}
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace sensors {
+/**
+ * Pigeon IMU Class.
+ * Class supports communicating over CANbus and over ribbon-cable (CAN Talon SRX).
+ */
+class PigeonIMU: public CANBusAddressable {
+public:
+	/** Data object for holding fusion information. */
+	struct FusionStatus {
+		double heading;
+		bool bIsValid;
+		bool bIsFusing;
+		std::string description;
+		/**
+		 * Same as GetLastError()
+		 */
+		int lastError;
+	};
+	/** Various calibration modes supported by Pigeon. */
+	enum CalibrationMode {
+		BootTareGyroAccel = 0,
+		Temperature = 1,
+		Magnetometer12Pt = 2,
+		Magnetometer360 = 3,
+		Accelerometer = 5,
+	};
+	/** Overall state of the Pigeon. */
+	enum PigeonState {
+		NoComm, Initializing, Ready, UserCalibration,
+	};
+	/**
+	 * Data object for status on current calibration and general status.
+	 *
+	 * Pigeon has many calibration modes supported for a variety of uses.
+	 * The modes generally collects and saves persistently information that makes
+	 * the Pigeon signals more accurate.  This includes collecting temperature, gyro, accelerometer,
+	 * and compass information.
+	 *
+	 * For FRC use-cases, typically compass and temperature calibration is not required.
+	 *
+	 * Additionally when motion driver software in the Pigeon boots, it will perform a fast boot calibration
+	 * to initially bias gyro and setup accelerometer.
+	 *
+	 * These modes can be enabled with the EnterCalibration mode.
+	 *
+	 * When a calibration mode is entered, caller can expect...
+	 *
+	 *  - PigeonState to reset to Initializing and bCalIsBooting is set to true.  Pigeon LEDs will blink the boot pattern.
+	 *  	This is similar to the normal boot cal, however it can an additional ~30 seconds since calibration generally
+	 *  	requires more information.
+	 *  	currentMode will reflect the user's selected calibration mode.
+	 *
+	 *  - PigeonState will eventually settle to UserCalibration and Pigeon LEDs will show cal specific blink patterns.
+	 *  	bCalIsBooting is now false.
+	 *
+	 *  - Follow the instructions in the Pigeon User Manual to meet the calibration specific requirements.
+	 * 		When finished calibrationError will update with the result.
+	 * 		Pigeon will solid-fill LEDs with red (for failure) or green (for success) for ~5 seconds.
+	 * 		Pigeon then perform boot-cal to cleanly apply the newly saved calibration data.
+	 */
+	struct GeneralStatus {
+		/**
+		 * The current state of the motion driver.  This reflects if the sensor signals are accurate.
+		 * Most calibration modes will force Pigeon to reinit the motion driver.
+		 */
+		PigeonIMU::PigeonState state;
+		/**
+		 * The currently applied calibration mode if state is in UserCalibration or if bCalIsBooting is true.
+		 * Otherwise it holds the last selected calibration mode (when calibrationError was updated).
+		 */
+		PigeonIMU::CalibrationMode currentMode;
+		/**
+		 * The error code for the last calibration mode.
+		 * Zero represents a successful cal (with solid green LEDs at end of cal)
+		 * and nonzero is a failed calibration (with solid red LEDs at end of cal).
+		 * Different calibration
+		 */
+		int calibrationError;
+		/**
+		 * After caller requests a calibration mode, pigeon will perform a boot-cal before
+		 * entering the requested mode.  During this period, this flag is set to true.
+		 */
+		bool bCalIsBooting;
+		/**
+		 * general string description of current status
+		 */
+		std::string description;
+		/**
+		 * Temperature in Celsius
+		 */
+		double tempC;
+		/**
+		 * Number of seconds Pigeon has been up (since boot).
+		 * This register is reset on power boot or processor reset.
+		 * Register is capped at 255 seconds with no wrap around.
+		 */
+		int upTimeSec;
+		/**
+		 * Number of times the Pigeon has automatically rebiased the gyro.
+		 * This counter overflows from 15 -> 0 with no cap.
+		 */
+		int noMotionBiasCount;
+		/**
+		 * Number of times the Pigeon has temperature compensated the various signals.
+		 * This counter overflows from 15 -> 0 with no cap.
+		 */
+		int tempCompensationCount;
+		/**
+		 * Same as GetLastError()
+		 */
+		int lastError;
+	};
+
+	PigeonIMU(int deviceNumber);
+	PigeonIMU(ctre::phoenix::motorcontrol::can::TalonSRX * talonSrx);
+
+	int SetYaw(double angleDeg, int timeoutMs);
+	int AddYaw(double angleDeg, int timeoutMs);
+	int SetYawToCompass(int timeoutMs);
+
+	int SetFusedHeading(double angleDeg, int timeoutMs);
+	int AddFusedHeading(double angleDeg, int timeoutMs);
+	int SetFusedHeadingToCompass(int timeoutMs);
+	int SetAccumZAngle(double angleDeg, int timeoutMs);
+
+	int ConfigTemperatureCompensationEnable(bool bTempCompEnable,
+			int timeoutMs);
+
+	int SetCompassDeclination(double angleDegOffset, int timeoutMs);
+	int SetCompassAngle(double angleDeg, int timeoutMs);
+
+	int EnterCalibrationMode(CalibrationMode calMode, int timeoutMs);
+	int GetGeneralStatus(PigeonIMU::GeneralStatus & genStatusToFill);
+	int GetLastError();
+	int Get6dQuaternion(double wxyz[4]);
+	int GetYawPitchRoll(double ypr[3]);
+	int GetAccumGyro(double xyz_deg[3]);
+	double GetAbsoluteCompassHeading();
+	double GetCompassHeading();
+	double GetCompassFieldStrength();
+	double GetTemp();
+	PigeonState GetState();
+	uint32_t GetUpTime();
+	int GetRawMagnetometer(int16_t rm_xyz[3]);
+
+	int GetBiasedMagnetometer(int16_t bm_xyz[3]);
+	int GetBiasedAccelerometer(int16_t ba_xyz[3]);
+	int GetRawGyro(double xyz_dps[3]);
+	int GetAccelerometerAngles(double tiltAngles[3]);
+	/**
+	 * @param status 	object reference to fill with fusion status flags.
+	 * @return The fused heading in degrees.
+	 */
+	double GetFusedHeading(FusionStatus & status);
+	/**
+	 * @return The fused heading in degrees.
+	 */
+	double GetFusedHeading();
+	uint32_t GetResetCount();
+	uint32_t GetResetFlags();
+	uint32_t GetFirmVers();
+
+	bool HasResetOccurred();
+
+	static std::string ToString(PigeonIMU::PigeonState state);
+	static std::string ToString(CalibrationMode cm);
+
+	ErrorCode ConfigSetCustomParam(int newValue, int paramIndex, int timeoutMs);
+	int ConfigGetCustomParam(int paramIndex, int timeoutMs);
+	ErrorCode ConfigSetParameter(ParamEnum param, double value,
+			uint8_t subValue, int ordinal, int timeoutMs);
+	double ConfigGetParameter(ctre::phoenix::ParamEnum param, int ordinal, int timeoutMs);
+
+	ErrorCode SetStatusFramePeriod(PigeonIMU_StatusFrame statusFrame, int periodMs,
+			int timeoutMs);
+
+	/**
+	 * Gets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame to get the period of.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Period of the given status frame.
+	 */
+	int GetStatusFramePeriod(PigeonIMU_StatusFrame frame,
+			int timeoutMs) ;
+	ErrorCode SetControlFramePeriod(PigeonIMU_ControlFrame frame,
+			int periodMs);
+	int GetFirmwareVersion() ;
+	ErrorCode GetFaults(PigeonIMU_Faults & toFill) ;
+	ErrorCode GetStickyFaults(PigeonIMU_StickyFaults & toFill);
+	ErrorCode ClearStickyFaults(int timeoutMs);
+
+
+
+
+	void* GetLowLevelHandle() {
+		return _handle;
+	}
+private:
+	/** firmware state reported over CAN */
+	enum MotionDriverState {
+		Init0 = 0,
+		WaitForPowerOff = 1,
+		ConfigAg = 2,
+		SelfTestAg = 3,
+		StartDMP = 4,
+		ConfigCompass_0 = 5,
+		ConfigCompass_1 = 6,
+		ConfigCompass_2 = 7,
+		ConfigCompass_3 = 8,
+		ConfigCompass_4 = 9,
+		ConfigCompass_5 = 10,
+		SelfTestCompass = 11,
+		WaitForGyroStable = 12,
+		AdditionalAccelAdjust = 13,
+		Idle = 14,
+		Calibration = 15,
+		LedInstrum = 16,
+		Error = 31,
+	};
+	/** sub command for the various Set param enums */
+	enum TareType {
+		SetValue = 0x00, AddOffset = 0x01, MatchCompass = 0x02, SetOffset = 0xFF,
+	};
+	/** data storage for reset signals */
+	struct ResetStats {
+		int32_t resetCount;
+		int32_t resetFlags;
+		int32_t firmVers;
+		bool hasReset;
+	};
+	ResetStats _resetStats = { 0, 0, 0, false };
+
+	/** Portion of the arbID for all status and control frames. */
+	void* _handle;
+	uint32_t _deviceNumber;
+	uint32_t _usageHist = 0;
+	uint64_t _cache;
+	uint32_t _len;
+
+	/** overall threshold for when frame data is too old */
+	const uint32_t EXPECTED_RESPONSE_TIMEOUT_MS = (200);
+
+	int PrivateSetParameter(ParamEnum paramEnum, TareType tareType,
+			double angleDeg, int timeoutMs);
+
+	PigeonIMU::PigeonState GetState(int errCode, const uint64_t & statusFrame);
+	double GetTemp(const uint64_t & statusFrame);
+};
+} // namespace signals
+} // namespace phoenix
+} // namespace ctre
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/include/ctre/phoenix/Signals/IInvertable.h b/cpp/include/ctre/phoenix/Signals/IInvertable.h
new file mode 100644
index 0000000..b964b1b
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Signals/IInvertable.h
@@ -0,0 +1,16 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace signals {
+
+class IInvertable {
+public:
+	virtual ~IInvertable(){}
+	virtual void SetInverted(bool invert) = 0;
+	virtual bool GetInverted() const = 0;
+};
+
+} // namespace  Signals
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/Signals/IOutputSignal.h b/cpp/include/ctre/phoenix/Signals/IOutputSignal.h
new file mode 100644
index 0000000..ff909fe
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Signals/IOutputSignal.h
@@ -0,0 +1,15 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace signals {
+
+class IOutputSignal {
+public:
+	virtual ~IOutputSignal(){}
+	virtual void Set(double value) = 0;
+};
+
+} // namespace  Signals
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/Signals/MovingAverage.h b/cpp/include/ctre/phoenix/Signals/MovingAverage.h
new file mode 100644
index 0000000..862b3d5
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Signals/MovingAverage.h
@@ -0,0 +1,92 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ * 
+ * Cross The Road Electronics (CTRE) licenses to you the right to 
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ * 
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, 
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+
+namespace ctre {
+namespace phoenix {
+namespace signals {
+
+class MovingAverage {
+private:
+
+	int _in; //!< head ptr for ringbuffer
+	int _ou; //!< tail ptr for ringbuffer
+	int _cnt; //!< number of element in ring buffer
+	int _cap; //!< capacity of ring buffer
+	float _sum; //!< sum of all elements in ring buffer
+	float * _d; //!< ring buffer
+public:
+	MovingAverage(int capacity) {
+		_cap = capacity;
+		_d = new float[_cap];
+		Clear();
+	}
+	float Process(float input) {
+		Push(input);
+		return _sum / (float) _cnt;
+	}
+	void Clear() {
+		_in = 0;
+		_ou = 0;
+		_cnt = 0;
+
+		_sum = 0;
+	}
+	void Push(float d) {
+		/* process it */
+		_sum += d;
+
+		/* if full, pop one */
+		if (_cnt >= _cap)
+			Pop();
+
+		/* push new one */
+		_d[_in] = d;
+		if (++_in >= _cap)
+			_in = 0;
+		++_cnt;
+	}
+	void Pop() {
+		/* get the oldest */
+		float d = _d[_ou];
+
+		/* process it */
+		_sum -= d;
+
+		/* pop it */
+		if (++_ou >= _cap)
+			_ou = 0;
+		--_cnt;
+	}
+	//-------------- Properties --------------//
+	float GetSum() {
+		return _sum;
+	}
+	int GetCount() {
+		return _cnt;
+	}
+};
+
+} // namespace  Signals
+} // namespace phoenix
+} // namespace ctre
+
diff --git a/cpp/include/ctre/phoenix/Stopwatch.h b/cpp/include/ctre/phoenix/Stopwatch.h
new file mode 100644
index 0000000..eb0c9ff
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Stopwatch.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <time.h>
+
+namespace ctre {
+namespace phoenix {
+
+class Stopwatch {
+public:
+	void Start();
+	unsigned int DurationMs();
+	float Duration();
+	
+private:
+	unsigned long _t0 = 0;
+	unsigned long _t1 = 0;
+	float _scalar = 0.001f / CLOCKS_PER_SEC;
+};
+
+}}
diff --git a/cpp/include/ctre/phoenix/Tasking/ButtonMonitor.h b/cpp/include/ctre/phoenix/Tasking/ButtonMonitor.h
new file mode 100644
index 0000000..2578d35
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/ButtonMonitor.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include "ctre/phoenix/Tasking/ILoopable.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+#include <functional>
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+/* forward proto's */
+namespace frc {
+	class GenericHID;
+}
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+
+class ButtonMonitor: public IProcessable, public ILoopable {
+public:
+
+	class IButtonPressEventHandler {
+		public:
+			virtual ~IButtonPressEventHandler(){}
+			virtual void OnButtonPress(int idx, bool isDown) = 0;
+	};
+
+	ButtonMonitor(frc::GenericHID * controller, int buttonIndex, IButtonPressEventHandler * ButtonPressEventHandler);
+	ButtonMonitor(const ButtonMonitor & rhs);
+	virtual ~ButtonMonitor() {	}
+
+	/* IProcessable */
+	virtual void Process();
+
+	/* ILoopable */
+	virtual void OnStart();
+	virtual void OnLoop();
+	virtual bool IsDone();
+	virtual void OnStop();
+
+private:
+	frc::GenericHID * _gameCntrlr;
+	int _btnIdx;
+	IButtonPressEventHandler * _handler;
+	bool _isDown = false;
+};
+}
+}
+}
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/include/ctre/phoenix/Tasking/ILoopable.h b/cpp/include/ctre/phoenix/Tasking/ILoopable.h
new file mode 100644
index 0000000..db1dd50
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/ILoopable.h
@@ -0,0 +1,13 @@
+#pragma once
+
+namespace ctre { namespace phoenix { namespace tasking {
+	
+class ILoopable{
+public:
+	virtual ~ILoopable(){}
+	virtual void OnStart() = 0;
+	virtual void OnLoop() = 0;
+	virtual bool IsDone() = 0;
+	virtual void OnStop() = 0;
+};
+}}}
diff --git a/cpp/include/ctre/phoenix/Tasking/IProcessable.h b/cpp/include/ctre/phoenix/Tasking/IProcessable.h
new file mode 100644
index 0000000..0e93180
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/IProcessable.h
@@ -0,0 +1,9 @@
+#pragma once
+namespace ctre { namespace phoenix { namespace tasking{
+	
+class IProcessable {
+public:
+	virtual ~IProcessable(){}
+	virtual void Process() = 0;
+};
+}}}
diff --git a/cpp/include/ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h b/cpp/include/ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h
new file mode 100644
index 0000000..7481d29
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <vector>
+#include "ctre/phoenix/Tasking/ILoopable.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+namespace schedulers {
+
+class ConcurrentScheduler: public ILoopable, public IProcessable {
+public:
+	std::vector<ILoopable*> _loops;
+	std::vector<bool> _enabs;
+
+	ConcurrentScheduler();
+	virtual ~ConcurrentScheduler();
+	void Add(ILoopable *aLoop, bool enable = true);
+	void RemoveAll();
+	void Start(ILoopable *toStart);
+	void Stop(ILoopable *toStop);
+	void StartAll();
+	void StopAll();
+
+	//IProcessable
+	void Process();
+
+	//ILoopable
+	bool Iterated();
+	void OnStart();
+	void OnLoop();
+	void OnStop();
+	bool IsDone();
+};
+}
+}
+}
+}
diff --git a/cpp/include/ctre/phoenix/Tasking/Schedulers/SequentialScheduler.h b/cpp/include/ctre/phoenix/Tasking/Schedulers/SequentialScheduler.h
new file mode 100644
index 0000000..f550b31
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/Schedulers/SequentialScheduler.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <vector>
+#include "ctre/phoenix/Tasking/ILoopable.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+
+namespace ctre { namespace phoenix { namespace tasking { namespace schedulers {
+
+class SequentialScheduler: public ILoopable, public IProcessable{
+public:
+	bool _running = false;
+	std::vector<ILoopable*> _loops;
+	unsigned int _idx = 0;
+	bool _iterated = false;
+
+	SequentialScheduler();
+	virtual ~SequentialScheduler();
+
+	void Add(ILoopable *aLoop);
+	ILoopable * GetCurrent();
+	void RemoveAll();
+	void Start();
+	void Stop();
+
+	//IProcessable
+	void Process();
+
+	//ILoopable
+	void OnStart();
+	void OnLoop();
+	void OnStop();
+	bool IsDone();
+};
+}}}}
diff --git a/cpp/include/ctre/phoenix/Utilities.h b/cpp/include/ctre/phoenix/Utilities.h
new file mode 100644
index 0000000..ce591c8
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Utilities.h
@@ -0,0 +1,20 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+	
+class Utilities {
+public:
+	static float abs(float f);
+	static float bound(float value, float capValue = 1);
+	static float cap(float value, float peak);
+	static void Deadband(float &value, float deadband = -.10);
+	static bool IsWithin(float value, float compareTo, float allowDelta);
+	static int SmallerOf(int value_1, int value_2);
+	static void Split_1(float forward, float turn, float *left, float *right);
+	static void Split_2(float left, float right, float *forward, float *turn);
+private:
+	static bool Contains(char array[], char item);
+};
+
+}}
diff --git a/cpp/src/CANifier.cpp b/cpp/src/CANifier.cpp
new file mode 100644
index 0000000..7b18849
--- /dev/null
+++ b/cpp/src/CANifier.cpp
@@ -0,0 +1,454 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ *
+ * Cross The Road Electronics (CTRE) licenses to you the right to
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ *
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+#include "ctre/phoenix/CANifier.h"
+#include "ctre/phoenix/CCI/CANifier_CCI.h"
+#include "ctre/phoenix/CTRLogger.h"
+#include "HAL/HAL.h"
+
+namespace ctre {
+namespace phoenix {
+	/**
+	 * Constructor.
+	 * @param deviceNumber	The CAN Device ID of the CANifier.
+	 */
+CANifier::CANifier(int deviceNumber): CANBusAddressable(deviceNumber)
+{
+	m_handle = c_CANifier_Create1(deviceNumber);
+	HAL_Report(HALUsageReporting::kResourceType_CANifier, deviceNumber + 1);
+}
+
+/**
+ * Sets the LED Output
+ * @param percentOutput Output duty cycle expressed as percentage.
+ * @param ledChannel 		Channel to set the output of.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetLEDOutput(double percentOutput, LEDChannel ledChannel) {
+	/* convert float to integral fixed pt */
+	if (percentOutput > 1) {
+		percentOutput = 1;
+	}
+	if (percentOutput < 0) {
+		percentOutput = 0;
+	}
+	int dutyCycle = (int) (percentOutput * 1023); // [0,1023]
+
+	return c_CANifier_SetLEDOutput(m_handle, dutyCycle, ledChannel);
+}
+
+/**
+ * Sets the output of a General Pin
+ * @param outputPin 		The pin to use as output.
+ * @param outputValue 	The desired output state.
+ * @param outputEnable	Whether this pin is an output. "True" enables output.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetGeneralOutput(GeneralPin outputPin, bool outputValue,
+		bool outputEnable) {
+	return  c_CANifier_SetGeneralOutput(m_handle, outputPin, outputValue,
+			outputEnable);
+}
+
+/**
+ * Sets the output of all General Pin
+ * @param outputBits 	A bit mask of all the output states.  LSB->MSB is in the order of the #GeneralPin enum.
+ * @param isOutputBits A boolean bit mask that sets the pins to be outputs or inputs.  A bit of 1 enables output.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetGeneralOutputs(int outputBits, int isOutputBits) {
+	return c_CANifier_SetGeneralOutputs(m_handle, outputBits, isOutputBits);
+}
+
+/**
+ * Sets the output of all General Pin
+ * @param allPins A structure to fill with the current state of all pins.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::GetGeneralInputs(CANifier::PinValues &allPins) {
+	ErrorCode err = c_CANifier_GetGeneralInputs(m_handle, _tempPins, sizeof(_tempPins));
+	allPins.LIMF = _tempPins[LIMF];
+	allPins.LIMR = _tempPins[LIMR];
+	allPins.QUAD_A = _tempPins[QUAD_A];
+	allPins.QUAD_B = _tempPins[QUAD_B];
+	allPins.QUAD_IDX = _tempPins[QUAD_IDX];
+	allPins.SCL = _tempPins[SCL];
+	allPins.SDA = _tempPins[SDA];
+	allPins.SPI_CLK_PWM0 = _tempPins[SPI_CLK_PWM0P];
+	allPins.SPI_MOSI_PWM1 = _tempPins[SPI_MOSI_PWM1P];
+	allPins.SPI_MISO_PWM2 = _tempPins[SPI_MISO_PWM2P];
+	allPins.SPI_CS_PWM3 = _tempPins[SPI_CS];
+	return err;
+}
+
+/**
+ * Gets the state of the specified pin
+ * @param inputPin  The index of the pin.
+ * @return The state of the pin.
+ */
+bool CANifier::GetGeneralInput(GeneralPin inputPin) {
+	bool retval = false;
+	(void)c_CANifier_GetGeneralInput(m_handle, inputPin, &retval);
+	return retval;
+}
+
+/**
+ * Gets the position of the quadrature encoder.
+ * @return The Position of the encoder.
+ */
+int CANifier::GetQuadraturePosition() {
+	int retval = 0;
+	(void)c_CANifier_GetQuadraturePosition(m_handle, &retval);
+	return retval;
+}
+/**
+ * Sets the position of the quadrature encoder.
+ * @param newPosition
+ * @return ErrorCode generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetQuadraturePosition(int newPosition, int timeoutMs) {
+	return c_CANifier_SetQuadraturePosition(m_handle, newPosition, timeoutMs);
+}
+/**
+ * Gets the velocity of the quadrature encoder.
+ * @return The Velocity of the encoder.
+ */
+int CANifier::GetQuadratureVelocity() {
+	int retval = 0;
+	(void)c_CANifier_GetQuadratureVelocity(m_handle, &retval);
+	return retval;
+}
+/**
+ * Configures the period of each velocity sample.
+ * Every 1ms a position value is sampled, and the delta between that sample
+ * and the position sampled kPeriod ms ago is inserted into a filter.
+ * kPeriod is configured with this function.
+ *
+ * @param period
+ *            Desired period for the velocity measurement. @see
+ *            #VelocityMeasPeriod
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ConfigVelocityMeasurementPeriod(CANifierVelocityMeasPeriod period, int timeoutMs) {
+	return c_CANifier_ConfigVelocityMeasurementPeriod(m_handle, period, timeoutMs);
+}
+/**
+ * Sets the number of velocity samples used in the rolling average velocity
+ * measurement.
+ *
+ * @param windowSize
+ *            Number of samples in the rolling average of velocity
+ *            measurement. Valid values are 1,2,4,8,16,32. If another
+ *            value is specified, it will truncate to nearest support value.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ConfigVelocityMeasurementWindow(int windowSize, int timeoutMs) {
+	return c_CANifier_ConfigVelocityMeasurementWindow(m_handle, windowSize, timeoutMs);
+}
+/**
+ * Gets the bus voltage seen by the device.
+ *
+ * @return The bus voltage value (in volts).
+ */
+double CANifier::GetBusVoltage() {
+	double param = 0;
+	c_CANifier_GetBusVoltage(m_handle, &param);
+	return param;
+}
+
+/**
+ * Call GetLastError() generated by this object.
+ * Not all functions return an error code but can
+ * potentially report errors.
+ *
+ * This function can be used to retrieve those error codes.
+ *
+ * @return The last ErrorCode generated.
+ */
+ErrorCode CANifier::GetLastError() {
+	return c_CANifier_GetLastError(m_handle);
+}
+
+/**
+ * Sets the PWM Output
+ * Currently supports PWM 0, PWM 1, and PWM 2
+ * @param pwmChannel  Index of the PWM channel to output.
+ * @param dutyCycle   Duty Cycle (0 to 1) to output.  Default period of the signal is 4.2 ms.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetPWMOutput(int pwmChannel, double dutyCycle) {
+	if (dutyCycle < 0) {
+		dutyCycle = 0;
+	} else if (dutyCycle > 1) {
+		dutyCycle = 1;
+	}
+	if (pwmChannel < 0) {
+		pwmChannel = 0;
+	}
+
+	int dutyCyc10bit = (int) (1023 * dutyCycle);
+
+	return c_CANifier_SetPWMOutput(m_handle, (int) pwmChannel,
+			dutyCyc10bit);
+}
+
+/**
+ * Enables PWM Outputs
+ * Currently supports PWM 0, PWM 1, and PWM 2
+ * @param pwmChannel  Index of the PWM channel to enable.
+ * @param bEnable			"True" enables output on the pwm channel.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::EnablePWMOutput(int pwmChannel, bool bEnable) {
+	if (pwmChannel < 0) {
+		pwmChannel = 0;
+	}
+
+	return c_CANifier_EnablePWMOutput(m_handle, (int) pwmChannel,
+			bEnable);
+}
+
+/**
+ * Gets the PWM Input
+ * @param pwmChannel  PWM channel to get.
+ * @param dutyCycleAndPeriod	Double array to hold Duty Cycle [0] and Period [1].
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::GetPWMInput(PWMChannel pwmChannel, double dutyCycleAndPeriod[]) {
+	return c_CANifier_GetPWMInput(m_handle, pwmChannel,
+			dutyCycleAndPeriod);
+}
+
+//------ Custom Persistent Params ----------//
+
+/**
+ * Sets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/duty cycle/output
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param newValue
+ *            Value for custom parameter.
+ * @param paramIndex
+ *            Index of custom parameter. [0-1]
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ConfigSetCustomParam(int newValue,
+		int paramIndex, int timeoutMs) {
+	return c_CANifier_ConfigSetCustomParam(m_handle, newValue, paramIndex, timeoutMs);
+}
+/**
+ * Gets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/duty cycle/output
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param paramIndex
+ *            Index of custom parameter. [0-1]
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Value of the custom param.
+ */
+int CANifier::ConfigGetCustomParam(
+		int paramIndex, int timeoutMs) {
+	int readValue;
+	c_CANifier_ConfigGetCustomParam(m_handle, &readValue, paramIndex, timeoutMs);
+	return readValue;
+}
+
+//------ Generic Param API, typically not used ----------//
+/**
+ * Sets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ *            Parameter enumeration.
+ * @param value
+ *            Value of parameter.
+ * @param subValue
+ *            Subvalue for parameter. Maximum value of 255.
+ * @param ordinal
+ *            Ordinal of parameter.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ConfigSetParameter(ParamEnum param, double value,
+		uint8_t subValue, int ordinal, int timeoutMs) {
+	return c_CANifier_ConfigSetParameter(m_handle, param, value, subValue, ordinal, timeoutMs);
+
+}
+/**
+ * Gets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ *            Parameter enumeration.
+ * @param ordinal
+ *            Ordinal of parameter.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Value of parameter.
+ */
+double CANifier::ConfigGetParameter(ParamEnum param, int ordinal, int timeoutMs) {
+	double value = 0;
+	c_CANifier_ConfigGetParameter(m_handle, param, &value, ordinal, timeoutMs);
+	return value;
+}
+
+//------ Frames ----------//
+/**
+ * Sets the period of the given status frame.
+ *
+ * @param statusFrame
+ *            Frame whose period is to be changed.
+ * @param periodMs
+ *            Period in ms for the given frame.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetStatusFramePeriod(CANifierStatusFrame statusFrame, int periodMs,
+		int timeoutMs) {
+	return c_CANifier_SetStatusFramePeriod(m_handle, statusFrame, periodMs,
+			timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ *            Frame to get the period of.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int CANifier::GetStatusFramePeriod(CANifierStatusFrame frame,
+		int timeoutMs) {
+	int periodMs = 0;
+	c_CANifier_GetStatusFramePeriod(m_handle, frame, &periodMs, timeoutMs);
+	return periodMs;
+}
+/**
+ * Sets the period of the given control frame.
+ *
+ * @param frame
+ *            Frame whose period is to be changed.
+ * @param periodMs
+ *            Period in ms for the given frame.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetControlFramePeriod(CANifierControlFrame frame,
+		int periodMs) {
+	return c_CANifier_SetControlFramePeriod(m_handle, frame, periodMs);
+}
+//------ Firmware ----------//
+/**
+ * Gets the firmware version of the device.
+ *
+ * @return Firmware version of device.
+ */
+int CANifier::GetFirmwareVersion() {
+	int retval = -1;
+	c_CANifier_GetFirmwareVersion(m_handle, &retval);
+	return retval;
+}
+/**
+ * Returns true if the device has reset since last call.
+ *
+ * @return Has a Device Reset Occurred?
+ */
+bool CANifier::HasResetOccurred() {
+	bool retval = false;
+	c_CANifier_HasResetOccurred(m_handle, &retval);
+	return retval;
+}
+//------ Faults ----------//
+/**
+ * Gets the CANifier fault status
+ *
+ * @param toFill
+ *            Container for fault statuses.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::GetFaults(CANifierFaults & toFill) {
+	int faultBits;
+	ErrorCode retval = c_CANifier_GetFaults(m_handle, &faultBits);
+	toFill = CANifierFaults(faultBits);
+	return retval;
+}
+/**
+ * Gets the CANifier sticky fault status
+ *
+ * @param toFill
+ *            Container for sticky fault statuses.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::GetStickyFaults(CANifierStickyFaults & toFill) {
+	int faultBits;
+	ErrorCode retval = c_CANifier_GetFaults(m_handle, &faultBits);
+	toFill = CANifierStickyFaults(faultBits);
+	return retval;
+}
+/**
+ * Clears the Sticky Faults
+ *
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ClearStickyFaults(int timeoutMs) {
+	return c_CANifier_ClearStickyFaults(m_handle, timeoutMs);
+}
+
+} // phoenix
+} // ctre
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/src/CTRLogger.cpp b/cpp/src/CTRLogger.cpp
new file mode 100644
index 0000000..e9c5a4b
--- /dev/null
+++ b/cpp/src/CTRLogger.cpp
@@ -0,0 +1,31 @@
+#include "ctre/phoenix/CTRLogger.h"
+#include "ctre/phoenix/CCI/Logger_CCI.h" // c_Logger_*
+#include <execinfo.h>
+
+namespace ctre {
+namespace phoenix {
+
+void CTRLogger::Open(int language) {
+	c_Logger_Open(language, true);
+}
+ErrorCode CTRLogger::Log(ErrorCode code, std::string origin) {
+	void *buf[100];
+	char **strings;
+	int size = backtrace(buf, 100);
+	strings = backtrace_symbols(buf, size);
+	std::string stackTrace;
+	for (int i = 1; i < size; i++) {
+		stackTrace += strings[i];
+		stackTrace += "\n";
+	}
+	return c_Logger_Log(code, origin.c_str(), 3, stackTrace.c_str());
+}
+void CTRLogger::Close() {
+	c_Logger_Close();
+}
+//void CTRLogger::Description(ErrorCode code, const char *&shrt, const char *&lng) {
+//	c_Logger_Description(code, shrt, lng);
+//}
+
+}
+}
diff --git a/cpp/src/CompileTest.cpp b/cpp/src/CompileTest.cpp
new file mode 100644
index 0000000..47e4f64
--- /dev/null
+++ b/cpp/src/CompileTest.cpp
@@ -0,0 +1,2 @@
+#include "ctre/Phoenix.h"
+
diff --git a/cpp/src/HsvToRgb.cpp b/cpp/src/HsvToRgb.cpp
new file mode 100644
index 0000000..c7d5f16
--- /dev/null
+++ b/cpp/src/HsvToRgb.cpp
@@ -0,0 +1,106 @@
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+#include "ctre/phoenix/HsvToRgb.h"
+#include <math.h>
+
+namespace ctre {
+namespace phoenix {
+/**
+ * Convert hue/saturation/and value into RGB values
+ *
+ * @param   hDegrees    Hue in degrees
+ * @param   S           Saturation with range of 0 to 1
+ * @param   V           Value with range of 0 to 1
+ * @param   r           Calculated Red value of RGB
+ * @param   g           Calculated Green value of RGB
+ * @param   b           Calculated Blue value of RGB
+ */
+void HsvToRgb::Convert(double hDegrees, double S, double V, float* r, float* g,
+		float* b) {
+	double R, G, B;
+	double H = hDegrees;
+
+	//Handles wrap-around
+	if (H < 0) {
+		H += 360;
+	};
+	if (H >= 360) {
+		H -= 360;
+	};
+
+	if (V <= 0)
+		R = G = B = 0;
+	else if (S <= 0)
+		R = G = B = V;
+	else {
+		double hf = H / 60.0;
+		int i = (int) floor(hf);
+		double f = hf - i;
+		double pv = V * (1 - S);
+		double qv = V * (1 - S * f);
+		double tv = V * (1 - S * (1 - f));
+		switch (i) {
+		//Red is dominant color
+		case 0:
+			R = V;
+			G = tv;
+			B = pv;
+			break;
+
+			//Green is dominant color
+		case 1:
+			R = qv;
+			G = V;
+			B = pv;
+			break;
+		case 2:
+			R = pv;
+			G = V;
+			B = tv;
+			break;
+
+			//Blue is dominant color
+		case 3:
+			R = pv;
+			G = qv;
+			B = V;
+			break;
+		case 4:
+			R = tv;
+			G = pv;
+			B = V;
+			break;
+
+			//Red is dominant color
+		case 5:
+			R = V;
+			G = pv;
+			B = qv;
+			break;
+
+			//Back-up case statements, in case our math is wrong
+		case 6:
+			R = V;
+			G = tv;
+			B = pv;
+			break;
+		case -1:
+			R = V;
+			G = pv;
+			B = qv;
+			break;
+
+			//Color is not defined
+		default:
+			//pretend color is black and white
+			R = G = B = V;
+			break;
+		}
+	}
+	*r = (float) R;
+	*g = (float) G;
+	*b = (float) B;
+}
+}
+}
+#endif
diff --git a/cpp/src/LinearInterpolation.cpp b/cpp/src/LinearInterpolation.cpp
new file mode 100644
index 0000000..dc5c358
--- /dev/null
+++ b/cpp/src/LinearInterpolation.cpp
@@ -0,0 +1,13 @@
+#include "ctre/phoenix/LinearInterpolation.h"
+
+namespace ctre {
+namespace phoenix {
+float LinearInterpolation::Calculate(float x, float x1, float y1, float x2,
+		float y2) {
+	float m = (y2 - y1) / (x2 - x1);
+
+	float retval = m * (x - x1) + y1;
+	return retval;
+}
+}
+}
diff --git a/cpp/src/MotorControl/CAN/BaseMotorController.cpp b/cpp/src/MotorControl/CAN/BaseMotorController.cpp
new file mode 100644
index 0000000..b67451e
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/BaseMotorController.cpp
@@ -0,0 +1,1738 @@
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "ctre/phoenix/MotorControl/SensorCollection.h"
+#include "ctre/phoenix/CCI/MotController_CCI.h"
+#include "ctre/phoenix/LowLevel/MotControllerWithBuffer_LowLevel.h"
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol;
+using namespace ctre::phoenix::motorcontrol::can;
+using namespace ctre::phoenix::motorcontrol::lowlevel;
+
+//--------------------- Constructors -----------------------------//
+/**
+ *
+ * Constructor for motor controllers.
+ * @param arbId
+ */
+BaseMotorController::BaseMotorController(int arbId) {
+	m_handle = c_MotController_Create1(arbId);
+	_arbId = arbId;
+
+	_sensorColl = new motorcontrol::SensorCollection((void*) m_handle);
+}
+/**
+ *
+ * Destructor
+ */
+BaseMotorController::~BaseMotorController() {
+	delete _sensorColl;
+	_sensorColl = 0;
+}
+/**
+ * @return CCI handle for child classes.
+ */
+void* BaseMotorController::GetHandle() {
+	return m_handle;
+}
+/**
+ * Returns the Device ID
+ *
+ * @return Device number.
+ */
+int BaseMotorController::GetDeviceID() {
+	int devID = 0;
+	(void) c_MotController_GetDeviceNumber(m_handle, &devID);
+	return devID;
+}
+//------ Set output routines. ----------//
+/**
+ * @param Mode Sets the appropriate output on the talon, depending on the mode.
+ * @param value The output value to apply.
+ *
+ * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
+ * In Current mode, output value is in amperes.
+ * In Velocity mode, output value is in position change / 100ms.
+ * In Position mode, output value is in encoder ticks or an analog value,
+ *   depending on the sensor.
+ * In Follower mode, the output value is the integer device ID of the talon to
+ * duplicate.
+ *
+ * @param value The setpoint value, as described above.
+ *
+ *
+ *	Standard Driving Example:
+ *	_talonLeft.set(ControlMode.PercentOutput, leftJoy);
+ *	_talonRght.set(ControlMode.PercentOutput, rghtJoy);
+ */
+void BaseMotorController::Set(ControlMode Mode, double value) {
+	Set(Mode, value, DemandType_Neutral, 0);
+}
+/**
+ * @param mode Sets the appropriate output on the talon, depending on the mode.
+ * @param demand0 The output value to apply.
+ * 	such as advanced feed forward and/or auxiliary close-looping in firmware.
+ * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
+ * In Current mode, output value is in amperes.
+ * In Velocity mode, output value is in position change / 100ms.
+ * In Position mode, output value is in encoder ticks or an analog value,
+ *   depending on the sensor. See
+ * In Follower mode, the output value is the integer device ID of the talon to
+ * duplicate.
+ *
+ * @param demand1 Supplemental value.  This will also be control mode specific for future features.
+ */
+
+void BaseMotorController::Set(ControlMode mode, double demand0, double demand1) {
+	Set(mode, demand0, DemandType_Neutral, demand1);
+}
+/**
+ * @param mode Sets the appropriate output on the talon, depending on the mode.
+ * @param demand0 The output value to apply.
+ * 	such as advanced feed forward and/or auxiliary close-looping in firmware.
+ * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
+ * In Current mode, output value is in amperes.
+ * In Velocity mode, output value is in position change / 100ms.
+ * In Position mode, output value is in encoder ticks or an analog value,
+ *   depending on the sensor. See
+ * In Follower mode, the output value is the integer device ID of the talon to
+ * duplicate.
+ *
+ * @param demand1Type The demand type for demand1.
+ * Neutral: Ignore demand1 and apply no change to the demand0 output.
+ * AuxPID: Use demand1 to set the target for the auxiliary PID 1.
+ * ArbitraryFeedForward: Use demand1 as an arbitrary additive value to the
+ *	 demand0 output.  In PercentOutput the demand0 output is the motor output,
+ *   and in closed-loop modes the demand0 output is the output of PID0.
+ * @param demand1 Supplmental output value.  Units match the set mode.
+ *
+ *
+ *  Arcade Drive Example:
+ *		_talonLeft.set(ControlMode::PercentOutput, joyForward, DemandType_ArbitraryFeedForward, +joyTurn);
+ *		_talonRght.set(ControlMode::PercentOutput, joyForward, DemandType_ArbitraryFeedForward, -joyTurn);
+ *
+ *	Drive Straight Example:
+ *	Note: Selected Sensor Configuration is necessary for both PID0 and PID1.
+ *		_talonLeft.follow(_talonRght, FollwerType_AuxOutput1);
+ *		_talonRght.set(ControlMode::PercentOutput, joyForward, DemandType_AuxPID, desiredRobotHeading);
+ *
+ *	Drive Straight to a Distance Example:
+ *	Note: Other configurations (sensor selection, PID gains, etc.) need to be set.
+ *		_talonLeft.follow(_talonRght, FollwerType_AuxOutput1);
+ *		_talonRght.set(ControlMode::MotionMagic, targetDistance, DemandType_AuxPID, desiredRobotHeading);
+ */
+void BaseMotorController::Set(ControlMode mode, double demand0, DemandType demand1Type, double demand1) {
+	m_controlMode = mode;
+	m_sendMode = mode;
+	m_setPoint = demand0;
+
+	uint32_t work;
+	switch (m_controlMode) {
+	case ControlMode::PercentOutput:
+		//case ControlMode::TimedPercentOutput:
+		c_MotController_Set_4(m_handle, (int) m_sendMode, demand0, demand1, demand1Type);
+		break;
+	case ControlMode::Follower:
+		/* did caller specify device ID */
+		if ((0 <= demand0) && (demand0 <= 62)) { // [0,62]
+			work = (uint32_t) GetBaseID();
+			work >>= 16;
+			work <<= 8;
+			work |= (uint8_t) demand0;
+		} else {
+			work = (uint32_t) demand0;
+		}
+		/* single precision guarantees 16bits of integral precision,
+		 *  so float/double cast on work is safe */
+		c_MotController_Set_4(m_handle, (int) m_sendMode, (double)work, demand1, demand1Type);
+		break;
+	case ControlMode::Velocity:
+	case ControlMode::Position:
+	case ControlMode::MotionMagic:
+	//case ControlMode::MotionMagicArc:
+	case ControlMode::MotionProfile:
+	case ControlMode::MotionProfileArc:
+		c_MotController_Set_4(m_handle, (int) m_sendMode, demand0, demand1, demand1Type);
+		break;
+	case ControlMode::Current:
+		c_MotController_SetDemand(m_handle, (int) m_sendMode,
+				(int) (1000 * demand0), 0); /* milliamps */
+		break;
+	case ControlMode::Disabled:
+		/* fall thru...*/
+	default:
+		c_MotController_SetDemand(m_handle, (int) m_sendMode, 0, 0);
+		break;
+	}
+}
+/**
+ * Neutral the motor output by setting control mode to disabled.
+ */
+void BaseMotorController::NeutralOutput() {
+	Set(ControlMode::Disabled, 0);
+}
+/**
+ * Sets the mode of operation during neutral throttle output.
+ *
+ * @param neutralMode
+ *            The desired mode of operation when the Controller output
+ *            throttle is neutral (ie brake/coast)
+ **/
+void BaseMotorController::SetNeutralMode(NeutralMode neutralMode) {
+	c_MotController_SetNeutralMode(m_handle, neutralMode);
+}
+/**
+ * Enables a future feature called "Heading Hold".
+ * For now this simply updates the CAN signal to the motor controller.
+ * Future firmware updates will use this.
+ *
+ *	@param enable true/false enable
+ */
+void BaseMotorController::EnableHeadingHold(bool enable) {
+	(void)enable;
+	/* this routine is moot as the Set() call updates the signal on each call */
+	//c_MotController_EnableHeadingHold(m_handle, enable);
+}
+/**
+ * For now this simply updates the CAN signal to the motor controller.
+ * Future firmware updates will use this to control advanced auxiliary loop behavior.
+ *
+ *	@param value
+ */
+void BaseMotorController::SelectDemandType(bool value) {
+	(void)value;
+	/* this routine is moot as the Set() call updates the signal on each call */
+	//c_MotController_SelectDemandType(m_handle, value);
+}
+
+//------ Invert behavior ----------//
+/**
+ * Sets the phase of the sensor. Use when controller forward/reverse output
+ * doesn't correlate to appropriate forward/reverse reading of sensor.
+ * Pick a value so that positive PercentOutput yields a positive change in sensor.
+ * After setting this, user can freely call SetInvert() with any value.
+ *
+ * @param PhaseSensor
+ *            Indicates whether to invert the phase of the sensor.
+ */
+void BaseMotorController::SetSensorPhase(bool PhaseSensor) {
+	c_MotController_SetSensorPhase(m_handle, PhaseSensor);
+}
+
+/**
+ * Inverts the hbridge output of the motor controller.
+ *
+ * This does not impact sensor phase and should not be used to correct sensor polarity.
+ *
+ * This will invert the hbridge output but NOT the LEDs.
+ * This ensures....
+ *  - Green LEDs always represents positive request from robot-controller/closed-looping mode.
+ *  - Green LEDs correlates to forward limit switch.
+ *  - Green LEDs correlates to forward soft limit.
+ *
+ * @param invert
+ *            Invert state to set.
+ */
+void BaseMotorController::SetInverted(bool invert) {
+	_invert = invert; /* cache for getter */
+	c_MotController_SetInverted(m_handle, _invert);
+}
+/**
+ * @return invert setting of motor output.
+ */
+bool BaseMotorController::GetInverted() const {
+	return _invert;
+}
+
+//----- general output shaping ------------------//
+/**
+ * Configures the open-loop ramp rate of throttle output.
+ *
+ * @param secondsFromNeutralToFull
+ *            Minimum desired time to go from neutral to full throttle. A
+ *            value of '0' will disable the ramp.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigOpenloopRamp(
+		double secondsFromNeutralToFull, int timeoutMs) {
+	return c_MotController_ConfigOpenLoopRamp(m_handle,
+			secondsFromNeutralToFull, timeoutMs);
+}
+
+/**
+ * Configures the closed-loop ramp rate of throttle output.
+ *
+ * @param secondsFromNeutralToFull
+ *            Minimum desired time to go from neutral to full throttle. A
+ *            value of '0' will disable the ramp.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigClosedloopRamp(
+		double secondsFromNeutralToFull, int timeoutMs) {
+	return c_MotController_ConfigClosedLoopRamp(m_handle,
+			secondsFromNeutralToFull, timeoutMs);
+}
+
+/**
+ * Configures the forward peak output percentage.
+ *
+ * @param percentOut
+ *            Desired peak output percentage. [0,+1]
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigPeakOutputForward(double percentOut,
+		int timeoutMs) {
+	return c_MotController_ConfigPeakOutputForward(m_handle, percentOut,
+			timeoutMs);
+}
+
+/**
+ * Configures the reverse peak output percentage.
+ *
+ * @param percentOut
+ *            Desired peak output percentage. [-1,0]
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigPeakOutputReverse(double percentOut,
+		int timeoutMs) {
+	return c_MotController_ConfigPeakOutputReverse(m_handle, percentOut,
+			timeoutMs);
+}
+/**
+ * Configures the forward nominal output percentage.
+ *
+ * @param percentOut
+ *            Nominal (minimum) percent output. [0,+1]
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigNominalOutputForward(double percentOut,
+		int timeoutMs) {
+	return c_MotController_ConfigNominalOutputForward(m_handle, percentOut,
+			timeoutMs);
+}
+/**
+ * Configures the reverse nominal output percentage.
+ *
+ * @param percentOut
+ *            Nominal (minimum) percent output. [-1,0]
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigNominalOutputReverse(double percentOut,
+		int timeoutMs) {
+	return c_MotController_ConfigNominalOutputReverse(m_handle, percentOut,
+			timeoutMs);
+}
+/**
+ * Configures the output deadband percentage.
+ *
+ * @param percentDeadband
+ *            Desired deadband percentage. Minimum is 0.1%, Maximum is
+ *            25%.  Pass 0.04 for 4% (factory default).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigNeutralDeadband(double percentDeadband,
+		int timeoutMs) {
+	return c_MotController_ConfigNeutralDeadband(m_handle, percentDeadband,
+			timeoutMs);
+}
+
+//------ Voltage Compensation ----------//
+/**
+ * Configures the Voltage Compensation saturation voltage.
+ *
+ * @param voltage
+ *            This is the max voltage to apply to the hbridge when voltage
+ *            compensation is enabled.  For example, if 10 (volts) is specified
+ *            and a TalonSRX is commanded to 0.5 (PercentOutput, closed-loop, etc)
+ *            then the TalonSRX will attempt to apply a duty-cycle to produce 5V.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigVoltageCompSaturation(double voltage,
+		int timeoutMs) {
+	return c_MotController_ConfigVoltageCompSaturation(m_handle, voltage,
+			timeoutMs);
+}
+
+/**
+ * Configures the voltage measurement filter.
+ *
+ * @param filterWindowSamples
+ *            Number of samples in the rolling average of voltage
+ *            measurement.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigVoltageMeasurementFilter(
+		int filterWindowSamples, int timeoutMs) {
+	return c_MotController_ConfigVoltageMeasurementFilter(m_handle,
+			filterWindowSamples, timeoutMs);
+}
+
+/**
+ * Enables voltage compensation. If enabled, voltage compensation works in
+ * all control modes.
+ *
+ * @param enable
+ *            Enable state of voltage compensation.
+ **/
+void BaseMotorController::EnableVoltageCompensation(bool enable) {
+	c_MotController_EnableVoltageCompensation(m_handle, enable);
+}
+
+//------ General Status ----------//
+/**
+ * Gets the bus voltage seen by the device.
+ *
+ * @return The bus voltage value (in volts).
+ */
+double BaseMotorController::GetBusVoltage() {
+	double param = 0;
+	c_MotController_GetBusVoltage(m_handle, &param);
+	return param;
+}
+
+/**
+ * Gets the output percentage of the motor controller.
+ *
+ * @return Output of the motor controller (in percent).  [-1,+1]
+ */
+double BaseMotorController::GetMotorOutputPercent() {
+	double param = 0;
+	c_MotController_GetMotorOutputPercent(m_handle, &param);
+	return param;
+}
+
+/**
+ * @return applied voltage to motor in volts.
+ */
+double BaseMotorController::GetMotorOutputVoltage() {
+	return GetBusVoltage() * GetMotorOutputPercent();
+}
+
+/**
+ * Gets the output current of the motor controller.
+ *
+ * @return The output current (in amps).
+ */
+double BaseMotorController::GetOutputCurrent() {
+	double param = 0;
+	c_MotController_GetOutputCurrent(m_handle, &param);
+	return param;
+}
+/**
+ * Gets the temperature of the motor controller.
+ *
+ * @return Temperature of the motor controller (in 'C)
+ */
+double BaseMotorController::GetTemperature() {
+	double param = 0;
+	c_MotController_GetTemperature(m_handle, &param);
+	return param;
+}
+
+//------ sensor selection ----------//
+/**
+ * Select the remote feedback device for the motor controller.
+ * Most CTRE CAN motor controllers will support remote sensors over CAN.
+ *
+ * @param feedbackDevice
+ *            Remote Feedback Device to select.
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSelectedFeedbackSensor(
+		RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) {
+	return c_MotController_ConfigSelectedFeedbackSensor(m_handle,
+			feedbackDevice, pidIdx, timeoutMs);
+}
+/**
+ * Select the feedback device for the motor controller.
+ *
+ * @param feedbackDevice
+ *            Feedback Device to select.
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSelectedFeedbackSensor(
+		FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) {
+	return c_MotController_ConfigSelectedFeedbackSensor(m_handle,
+			feedbackDevice, pidIdx, timeoutMs);
+}
+
+/**
+ * The Feedback Coefficient is a scalar applied to the value of the
+ * feedback sensor.  Useful when you need to scale your sensor values
+ * within the closed-loop calculations.  Default value is 1.
+ *
+ * Selected Feedback Sensor register in firmware is the decoded sensor value
+ * multiplied by the Feedback Coefficient.
+ *
+ * @param coefficient
+ *            Feedback Coefficient value.  Maximum value of 1.
+ *						Resolution is 1/(2^16).  Cannot be 0.
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSelectedFeedbackCoefficient(
+		double coefficient, int pidIdx, int timeoutMs) {
+	return c_MotController_ConfigSelectedFeedbackCoefficient(m_handle,
+			coefficient, pidIdx, timeoutMs);
+}
+
+/**
+ * Select what remote device and signal to assign to Remote Sensor 0 or Remote Sensor 1.
+ * After binding a remote device and signal to Remote Sensor X, you may select Remote Sensor X
+ * as a PID source for closed-loop features.
+ *
+ * @param deviceID
+ *            The CAN ID of the remote sensor device.
+ * @param remoteSensorSource
+ *            The remote sensor device and signal type to bind.
+ * @param remoteOrdinal
+ *            0 for configuring Remote Sensor 0
+ *            1 for configuring Remote Sensor 1
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigRemoteFeedbackFilter(int deviceID,
+		RemoteSensorSource remoteSensorSource, int remoteOrdinal,
+		int timeoutMs) {
+	return c_MotController_ConfigRemoteFeedbackFilter(m_handle, deviceID,
+			(int) remoteSensorSource, remoteOrdinal, timeoutMs);
+}
+/**
+ * Select what sensor term should be bound to switch feedback device.
+ * Sensor Sum = Sensor Sum Term 0 - Sensor Sum Term 1
+ * Sensor Difference = Sensor Diff Term 0 - Sensor Diff Term 1
+ * The four terms are specified with this routine.  Then Sensor Sum/Difference
+ * can be selected for closed-looping.
+ *
+ * @param sensorTerm Which sensor term to bind to a feedback source.
+ * @param feedbackDevice The sensor signal to attach to sensorTerm.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSensorTerm(SensorTerm sensorTerm,
+		FeedbackDevice feedbackDevice, int timeoutMs) {
+	return c_MotController_ConfigSensorTerm(m_handle, (int) sensorTerm,
+			(int) feedbackDevice, timeoutMs);
+}
+
+//------- sensor status --------- //
+/**
+ * Get the selected sensor position (in raw sensor units).
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * See Phoenix-Documentation for how to interpret.
+ *
+ * @return Position of selected sensor (in raw sensor units).
+ */
+int BaseMotorController::GetSelectedSensorPosition(int pidIdx) {
+	int retval;
+	c_MotController_GetSelectedSensorPosition(m_handle, &retval, pidIdx);
+	return retval;
+}
+/**
+ * Get the selected sensor velocity.
+ *
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return selected sensor (in raw sensor units) per 100ms.
+ * See Phoenix-Documentation for how to interpret.
+ */
+int BaseMotorController::GetSelectedSensorVelocity(int pidIdx) {
+	int retval;
+	c_MotController_GetSelectedSensorVelocity(m_handle, &retval, pidIdx);
+	return retval;
+}
+/**
+ * Sets the sensor position to the given value.
+ *
+ * @param sensorPos
+ *            Position to set for the selected sensor (in raw sensor units).
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetSelectedSensorPosition(int sensorPos,
+		int pidIdx, int timeoutMs) {
+	return c_MotController_SetSelectedSensorPosition(m_handle, sensorPos,
+			pidIdx, timeoutMs);
+}
+
+//------ status frame period changes ----------//
+/**
+ * Sets the period of the given control frame.
+ *
+ * @param frame
+ *            Frame whose period is to be changed.
+ * @param periodMs
+ *            Period in ms for the given frame.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetControlFramePeriod(ControlFrame frame,
+		int periodMs) {
+	return c_MotController_SetControlFramePeriod(m_handle, frame, periodMs);
+}
+/**
+ * Sets the period of the given status frame.
+ *
+ * User ensure CAN Bus utilization is not high.
+ *
+ * This setting is not persistent and is lost when device is reset.
+ * If this is a concern, calling application can use HasReset()
+ * to determine if the status frame needs to be reconfigured.
+ *
+ * @param frame
+ *            Frame whose period is to be changed.
+ * @param periodMs
+ *            Period in ms for the given frame.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetStatusFramePeriod(StatusFrame frame,
+		int periodMs, int timeoutMs) {
+	return c_MotController_SetStatusFramePeriod(m_handle, frame, periodMs,
+			timeoutMs);
+}
+/**
+ * Sets the period of the given status frame.
+ *
+ * User ensure CAN Bus utilization is not high.
+ *
+ * This setting is not persistent and is lost when device is reset.
+ * If this is a concern, calling application can use HasReset()
+ * to determine if the status frame needs to be reconfigured.
+ *
+ * @param frame
+ *            Frame whose period is to be changed.
+ * @param periodMs
+ *            Period in ms for the given frame.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetStatusFramePeriod(StatusFrameEnhanced frame,
+		int periodMs, int timeoutMs) {
+	return c_MotController_SetStatusFramePeriod(m_handle, frame, periodMs,
+			timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ *            Frame to get the period of.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int BaseMotorController::GetStatusFramePeriod(StatusFrame frame,
+		int timeoutMs) {
+	int periodMs = 0;
+	c_MotController_GetStatusFramePeriod(m_handle, frame, &periodMs, timeoutMs);
+	return periodMs;
+}
+
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ *            Frame to get the period of.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int BaseMotorController::GetStatusFramePeriod(StatusFrameEnhanced frame,
+		int timeoutMs) {
+	int periodMs = 0;
+	c_MotController_GetStatusFramePeriod(m_handle, frame, &periodMs, timeoutMs);
+	return periodMs;
+}
+
+//----- velocity signal conditioning ------//
+
+/**
+ * Configures the period of each velocity sample.
+ * Every 1ms a position value is sampled, and the delta between that sample
+ * and the position sampled kPeriod ms ago is inserted into a filter.
+ * kPeriod is configured with this function.
+ *
+ * @param period
+ *            Desired period for the velocity measurement. @see
+ *            #VelocityMeasPeriod
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigVelocityMeasurementPeriod(
+		VelocityMeasPeriod period, int timeoutMs) {
+	return c_MotController_ConfigVelocityMeasurementPeriod(m_handle, period,
+			timeoutMs);
+}
+/**
+ * Sets the number of velocity samples used in the rolling average velocity
+ * measurement.
+ *
+ * @param windowSize
+ *            Number of samples in the rolling average of velocity
+ *            measurement. Valid values are 1,2,4,8,16,32. If another
+ *            value is specified, it will truncate to nearest support value.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigVelocityMeasurementWindow(int windowSize,
+		int timeoutMs) {
+	return c_MotController_ConfigVelocityMeasurementWindow(m_handle, windowSize,
+			timeoutMs);
+}
+
+//------ remote limit switch ----------//
+/**
+ * Configures the forward limit switch for a remote source.
+ * For example, a CAN motor controller may need to monitor the Limit-F pin
+ * of another Talon or CANifier.
+ *
+ * @param type
+ *            Remote limit switch source.
+ *            User can choose between a remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ *            Setting for normally open, normally closed, or disabled. This setting
+ *            matches the web-based configuration drop down.
+ * @param deviceID
+ *            Device ID of remote source (Talon SRX or CANifier device ID).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigForwardLimitSwitchSource(
+		RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+		int deviceID, int timeoutMs) {
+	LimitSwitchSource cciType = LimitSwitchRoutines::Promote(type);
+	return c_MotController_ConfigForwardLimitSwitchSource(m_handle, cciType,
+			normalOpenOrClose, deviceID, timeoutMs);
+}
+/**
+ * Configures the reverse limit switch for a remote source.
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon or CANifier.
+ *
+ * @param type
+ *            Remote limit switch source.
+ *            User can choose between a remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ *            Setting for normally open, normally closed, or disabled. This setting
+ *            matches the web-based configuration drop down.
+ * @param deviceID
+ *            Device ID of remote source (Talon SRX or CANifier device ID).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigReverseLimitSwitchSource(
+		RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+		int deviceID, int timeoutMs) {
+	LimitSwitchSource cciType = LimitSwitchRoutines::Promote(type);
+	return c_MotController_ConfigReverseLimitSwitchSource(m_handle, cciType,
+			normalOpenOrClose, deviceID, timeoutMs);
+}
+/**
+ * Sets the enable state for limit switches.
+ *
+ * This routine can be used to DISABLE the limit switch feature.
+ * This is helpful to force off the limit switch detection.
+ * For example, a module can leave limit switches enable for home-ing
+ * a continuous mechanism, and once done this routine can force off
+ * disabling of the motor controller.
+ *
+ * Limit switches must be enabled using the Config routines first.
+ *
+ * @param enable
+ *            Enable state for limit switches.
+ */
+void BaseMotorController::OverrideLimitSwitchesEnable(bool enable) {
+	c_MotController_OverrideLimitSwitchesEnable(m_handle, enable);
+}
+
+//------ local limit switch ----------//
+/**
+ * Configures a limit switch for a local/remote source.
+ *
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon, CANifier, or local Gadgeteer feedback connector.
+ *
+ * If the sensor is remote, a device ID of zero is assumed.
+ * If that's not desired, use the four parameter version of this function.
+ *
+ * @param type
+ *            Limit switch source. @see #LimitSwitchSource
+ *            User can choose between the feedback connector, remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ *            Setting for normally open, normally closed, or disabled. This setting
+ *            matches the web-based configuration drop down.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigForwardLimitSwitchSource(
+		LimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+		int timeoutMs) {
+	return c_MotController_ConfigForwardLimitSwitchSource(m_handle, type,
+			normalOpenOrClose, 0, timeoutMs);
+}
+/**
+ * Configures a limit switch for a local/remote source.
+ *
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon, CANifier, or local Gadgeteer feedback connector.
+ *
+ * If the sensor is remote, a device ID of zero is assumed.
+ * If that's not desired, use the four parameter version of this function.
+ *
+ * @param type
+ *            Limit switch source. @see #LimitSwitchSource
+ *            User can choose between the feedback connector, remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ *            Setting for normally open, normally closed, or disabled. This setting
+ *            matches the web-based configuration drop down.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigReverseLimitSwitchSource(
+		LimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+		int timeoutMs) {
+	return c_MotController_ConfigReverseLimitSwitchSource(m_handle, type,
+			normalOpenOrClose, 0, timeoutMs);
+}
+
+//------ soft limit ----------//
+/**
+ * Configures the forward soft limit threshold.
+ *
+ * @param forwardSensorLimit
+ *            Forward Sensor Position Limit (in raw sensor units).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigForwardSoftLimitThreshold(int forwardSensorLimit,
+		int timeoutMs) {
+	return c_MotController_ConfigForwardSoftLimitThreshold(m_handle, forwardSensorLimit,
+			timeoutMs);
+}
+
+/**
+ * Configures the reverse soft limit threshold.
+ *
+ * @param reverseSensorLimit
+ *            Reverse Sensor Position Limit (in raw sensor units).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigReverseSoftLimitThreshold(int reverseSensorLimit,
+		int timeoutMs) {
+	return c_MotController_ConfigReverseSoftLimitThreshold(m_handle, reverseSensorLimit,
+			timeoutMs);
+}
+
+/**
+ * Configures the forward soft limit enable .
+ *
+ * @param enable
+ *            True to enable soft limit. False to disable.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigForwardSoftLimitEnable(bool enable,
+		int timeoutMs) {
+	return c_MotController_ConfigForwardSoftLimitEnable(m_handle, enable,
+			timeoutMs);
+}
+
+
+/**
+ * Configures the reverse soft limit enable.
+ *
+ * @param enable
+ *            True to enable soft limit. False to disable.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigReverseSoftLimitEnable(bool enable,
+		int timeoutMs) {
+	return c_MotController_ConfigReverseSoftLimitEnable(m_handle, enable,
+			timeoutMs);
+}
+
+/**
+ * Can be used to override-disable the soft limits.
+ * This function can be used to quickly disable soft limits without
+ * having to modify the persistent configuration.
+ *
+ * @param enable
+ *            Enable state for soft limit switches.
+ **/
+void BaseMotorController::OverrideSoftLimitsEnable(bool enable) {
+	c_MotController_OverrideSoftLimitsEnable(m_handle, enable);
+}
+
+//------ Current Lim ----------//
+/* not available in base */
+
+//------ General Close loop ----------//
+/**
+ * Sets the 'P' constant in the given parameter slot.
+ *
+ * @param slotIdx
+ *            Parameter slot for the constant.
+ * @param value
+ *            Value of the P constant.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_kP(int slotIdx, double value,
+		int timeoutMs) {
+	return c_MotController_Config_kP(m_handle, slotIdx, value, timeoutMs);
+}
+
+/**
+ * Sets the 'I' constant in the given parameter slot.
+ *
+ * @param slotIdx
+ *            Parameter slot for the constant.
+ * @param value
+ *            Value of the I constant.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_kI(int slotIdx, double value,
+		int timeoutMs) {
+	return c_MotController_Config_kI(m_handle, slotIdx, value, timeoutMs);
+}
+
+/**
+ * Sets the 'D' constant in the given parameter slot.
+ *
+ * @param slotIdx
+ *            Parameter slot for the constant.
+ * @param value
+ *            Value of the D constant.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_kD(int slotIdx, double value,
+		int timeoutMs) {
+	return c_MotController_Config_kD(m_handle, slotIdx, value, timeoutMs);
+}
+
+/**
+ * Sets the 'F' constant in the given parameter slot.
+ *
+ * @param slotIdx
+ *            Parameter slot for the constant.
+ * @param value
+ *            Value of the F constant.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_kF(int slotIdx, double value,
+		int timeoutMs) {
+	return c_MotController_Config_kF(m_handle, slotIdx, value, timeoutMs);
+}
+
+/**
+ * Sets the Integral Zone constant in the given parameter slot.
+ * If the (absolute) closed-loop error is outside of this zone, integral accumulator
+ * is automatically cleared.  This ensures than integral wind up events will stop after
+ * the sensor gets far enough from its target.
+ *
+ * @param slotIdx
+ *            Parameter slot for the constant.
+ * @param izone
+ *            Value of the Integral Zone constant (closed loop error units X 1ms).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_IntegralZone(int slotIdx, int izone,
+		int timeoutMs) {
+	return c_MotController_Config_IntegralZone(m_handle, slotIdx, izone,
+			timeoutMs);
+}
+
+/**
+ * Sets the allowable closed-loop error in the given parameter slot.
+ * If (absolute) closed-loop error is within this value, the motor output is neutral.
+ *
+ * @param slotIdx
+ *            Parameter slot for the constant.
+ * @param allowableCloseLoopError
+ *            Value of the allowable closed-loop error.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigAllowableClosedloopError(int slotIdx,
+		int allowableCloseLoopError, int timeoutMs) {
+	return c_MotController_ConfigAllowableClosedloopError(m_handle, slotIdx,
+			allowableCloseLoopError, timeoutMs);
+}
+
+/**
+ * Sets the maximum integral accumulator in the given parameter slot.
+ *
+ * @param slotIdx
+ *            Parameter slot for the constant.
+ * @param iaccum
+ *            Value of the maximum integral accumulator (closed loop error units X 1ms).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigMaxIntegralAccumulator(int slotIdx,
+		double iaccum, int timeoutMs) {
+	return c_MotController_ConfigMaxIntegralAccumulator(m_handle, slotIdx,
+			iaccum, timeoutMs);
+}
+
+/**
+ * Sets the peak closed-loop output.  This peak output is slot-specific and
+ *   is applied to the output of the associated PID loop.
+ * This setting is seperate from the generic Peak Output setting.
+ *
+ * @param slotIdx
+ *            Parameter slot for the constant.
+ * @param percentOut
+ *            Peak Percent Output from 0 to 1.  This value is absolute and
+ *						the magnitude will apply in both forward and reverse directions.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigClosedLoopPeakOutput(int slotIdx, double percentOut, int timeoutMs) {
+	return c_MotController_ConfigClosedLoopPeakOutput(m_handle, slotIdx, percentOut, timeoutMs);
+}
+
+/**
+ * Sets the loop time (in milliseconds) of the PID closed-loop calculations.
+ * Default value is 1 ms.
+ *
+ * @param slotIdx
+ *            Parameter slot for the constant.
+ * @param loopTimeMs
+ *            Loop timing of the closed-loop calculations.  Minimum value of
+ *						1 ms, maximum of 64 ms.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigClosedLoopPeriod(int slotIdx, int loopTimeMs, int timeoutMs) {
+	return c_MotController_ConfigClosedLoopPeriod(m_handle, slotIdx, loopTimeMs, timeoutMs);
+}
+
+/**
+	 * Configures the Polarity of the Auxiliary PID (PID1).
+	 *
+	 * Standard Polarity:
+	 *    Primary Output = PID0 + PID1
+	 *    Auxiliary Output = PID0 - PID1
+	 *
+	 * Inverted Polarity:
+	 *    Primary Output = PID0 - PID1
+	 *    Auxiliary Output = PID0 + PID1
+	 *
+	 * @param invert
+	 *            If true, use inverted PID1 output polarity.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code
+	 */
+	ErrorCode BaseMotorController::ConfigAuxPIDPolarity(bool invert, int timeoutMs){
+		return ConfigSetParameter(ParamEnum::ePIDLoopPolarity, invert, 0, 1, timeoutMs);
+	}
+
+/**
+ * Sets the integral accumulator. Typically this is used to clear/zero
+ * the integral accumulator, however some use cases may require seeding
+ * the accumulator for a faster response.
+ *
+ * @param iaccum
+ *            Value to set for the integral accumulator (closed loop error units X 1ms).
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetIntegralAccumulator(double iaccum, int pidIdx,
+		int timeoutMs) {
+	return c_MotController_SetIntegralAccumulator(m_handle, iaccum, pidIdx,
+			timeoutMs);
+}
+
+/**
+ * Gets the closed-loop error.
+ * The units depend on which control mode is in use.
+ * See Phoenix-Documentation information on units.
+ *
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return Closed-loop error value.
+ */
+int BaseMotorController::GetClosedLoopError(int pidIdx) {
+	int closedLoopError = 0;
+	c_MotController_GetClosedLoopError(m_handle, &closedLoopError, pidIdx);
+	return closedLoopError;
+}
+
+/**
+ * Gets the iaccum value.
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return Integral accumulator value (Closed-loop error X 1ms).
+ */
+double BaseMotorController::GetIntegralAccumulator(int pidIdx) {
+	double iaccum = 0;
+	c_MotController_GetIntegralAccumulator(m_handle, &iaccum, pidIdx);
+	return iaccum;
+}
+
+
+/**
+ * Gets the derivative of the closed-loop error.
+ *
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return The error derivative value.
+ */
+double BaseMotorController::GetErrorDerivative(int pidIdx) {
+	double derror = 0;
+	c_MotController_GetErrorDerivative(m_handle, &derror, pidIdx);
+	return derror;
+}
+
+/**
+ * Selects which profile slot to use for closed-loop control.
+ *
+ * @param slotIdx
+ *            Profile slot to select.
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ **/
+ErrorCode BaseMotorController::SelectProfileSlot(int slotIdx, int pidIdx) {
+	return c_MotController_SelectProfileSlot(m_handle, slotIdx, pidIdx);
+}
+/**
+ * Gets the current target of a given closed loop.
+ *
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return The closed loop target.
+ */
+int BaseMotorController::GetClosedLoopTarget(int pidIdx) {
+	int param = 0;
+	c_MotController_GetClosedLoopTarget(m_handle, &param, pidIdx);
+	return param;
+}
+/**
+ * Gets the active trajectory target position using
+ * MotionMagic/MotionProfile control modes.
+ *
+ * @return The Active Trajectory Position in sensor units.
+ */
+int BaseMotorController::GetActiveTrajectoryPosition() {
+	int param = 0;
+	c_MotController_GetActiveTrajectoryPosition(m_handle, &param);
+	return param;
+}
+/**
+ * Gets the active trajectory target velocity using
+ * MotionMagic/MotionProfile control modes.
+ *
+ * @return The Active Trajectory Velocity in sensor units per 100ms.
+ */
+int BaseMotorController::GetActiveTrajectoryVelocity() {
+	int param = 0;
+	c_MotController_GetActiveTrajectoryVelocity(m_handle, &param);
+	return param;
+}
+/**
+ * Gets the active trajectory target heading using
+ * MotionMagicArc/MotionProfileArc control modes.
+ *
+ * @return The Active Trajectory Heading in degreees.
+ */
+double BaseMotorController::GetActiveTrajectoryHeading() {
+	double param = 0;
+	c_MotController_GetActiveTrajectoryHeading(m_handle, &param);
+	return param;
+}
+
+//------ Motion Profile Settings used in Motion Magic and Motion Profile ----------//
+
+/**
+ * Sets the Motion Magic Cruise Velocity.  This is the peak target velocity
+ * that the motion magic curve generator can use.
+ *
+ * @param sensorUnitsPer100ms
+ *            Motion Magic Cruise Velocity (in raw sensor units per 100 ms).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigMotionCruiseVelocity(
+		int sensorUnitsPer100ms, int timeoutMs) {
+	return c_MotController_ConfigMotionCruiseVelocity(m_handle,
+			sensorUnitsPer100ms, timeoutMs);
+}
+/**
+ * Sets the Motion Magic Acceleration.  This is the target acceleration
+ * that the motion magic curve generator can use.
+ *
+ * @param sensorUnitsPer100msPerSec
+ *            Motion Magic Acceleration (in raw sensor units per 100 ms per
+ *            second).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigMotionAcceleration(
+		int sensorUnitsPer100msPerSec, int timeoutMs) {
+	return c_MotController_ConfigMotionAcceleration(m_handle,
+			sensorUnitsPer100msPerSec, timeoutMs);
+}
+
+//------ Motion Profile Buffer ----------//
+/**
+ * Clear the buffered motion profile in both motor controller's RAM (bottom), and in the API
+ * (top).
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ClearMotionProfileTrajectories() {
+	return c_MotController_ClearMotionProfileTrajectories(m_handle);
+}
+/**
+ * Retrieve just the buffer count for the api-level (top) buffer.
+ * This routine performs no CAN or data structure lookups, so its fast and ideal
+ * if caller needs to quickly poll the progress of trajectory points being
+ * emptied into motor controller's RAM. Otherwise just use GetMotionProfileStatus.
+ * @return number of trajectory points in the top buffer.
+ */
+int BaseMotorController::GetMotionProfileTopLevelBufferCount() {
+	int param = 0;
+	c_MotController_GetMotionProfileTopLevelBufferCount(m_handle, &param);
+	return param;
+}
+/**
+ * Push another trajectory point into the top level buffer (which is emptied
+ * into the motor controller's bottom buffer as room allows).
+ * @param trajPt to push into buffer.
+ * The members should be filled in with these values...
+ *
+ * 		targPos:  servo position in sensor units.
+ *		targVel:  velocity to feed-forward in sensor units
+ *                 per 100ms.
+ * 		profileSlotSelect0  Which slot to get PIDF gains. PID is used for position servo. F is used
+ *						   as the Kv constant for velocity feed-forward. Typically this is hardcoded
+ *						   to the a particular slot, but you are free gain schedule if need be.
+ *						   Choose from [0,3]
+ *		profileSlotSelect1 Which slot to get PIDF gains for auxiliary PId.
+ *						   This only has impact during MotionProfileArc Control mode.
+ *						   Choose from [0,1].
+ * 	   isLastPoint  set to nonzero to signal motor controller to keep processing this
+ *                     trajectory point, instead of jumping to the next one
+ *                     when timeDurMs expires.  Otherwise MP executer will
+ *                     eventually see an empty buffer after the last point
+ *                     expires, causing it to assert the IsUnderRun flag.
+ *                     However this may be desired if calling application
+ *                     never wants to terminate the MP.
+ *		zeroPos  set to nonzero to signal motor controller to "zero" the selected
+ *                 position sensor before executing this trajectory point.
+ *                 Typically the first point should have this set only thus
+ *                 allowing the remainder of the MP positions to be relative to
+ *                 zero.
+ *		timeDur Duration to apply this trajectory pt.
+ * 				This time unit is ADDED to the exising base time set by
+ * 				configMotionProfileTrajectoryPeriod().
+ * @return CTR_OKAY if trajectory point push ok. ErrorCode if buffer is
+ *         full due to kMotionProfileTopBufferCapacity.
+ */
+ErrorCode BaseMotorController::PushMotionProfileTrajectory(
+		const ctre::phoenix::motion::TrajectoryPoint & trajPt) {
+	ErrorCode retval = c_MotController_PushMotionProfileTrajectory_2(m_handle,
+			trajPt.position, trajPt.velocity, trajPt.auxiliaryPos,
+			trajPt.profileSlotSelect0, trajPt.profileSlotSelect1, trajPt.isLastPoint, trajPt.zeroPos,
+			(int)trajPt.timeDur);
+	return retval;
+}
+/**
+ * Retrieve just the buffer full for the api-level (top) buffer.
+ * This routine performs no CAN or data structure lookups, so its fast and ideal
+ * if caller needs to quickly poll. Otherwise just use GetMotionProfileStatus.
+ * @return number of trajectory points in the top buffer.
+ */
+bool BaseMotorController::IsMotionProfileTopLevelBufferFull() {
+	bool retval = false;
+	c_MotController_IsMotionProfileTopLevelBufferFull(m_handle, &retval);
+	return retval;
+}
+/**
+ * This must be called periodically to funnel the trajectory points from the
+ * API's top level buffer to the controller's bottom level buffer.  Recommendation
+ * is to call this twice as fast as the execution rate of the motion profile.
+ * So if MP is running with 20ms trajectory points, try calling this routine
+ * every 10ms.  All motion profile functions are thread-safe through the use of
+ * a mutex, so there is no harm in having the caller utilize threading.
+ */
+void BaseMotorController::ProcessMotionProfileBuffer() {
+	c_MotController_ProcessMotionProfileBuffer(m_handle);
+}
+/**
+ * Retrieve all status information.
+ * For best performance, Caller can snapshot all status information regarding the
+ * motion profile executer.
+ *
+ * @param [out] statusToFill  Caller supplied object to fill.
+ *
+ * The members are filled, as follows...
+ *
+ *	topBufferRem:	The available empty slots in the trajectory buffer.
+ * 	 				The robot API holds a "top buffer" of trajectory points, so your applicaion
+ * 	 				can dump several points at once.  The API will then stream them into the
+ * 	 		 		low-level buffer, allowing the motor controller to act on them.
+ *
+ *	topBufferRem: The number of points in the top trajectory buffer.
+ *
+ *	btmBufferCnt: The number of points in the low level controller buffer.
+ *
+ *	hasUnderrun: 	Set if isUnderrun ever gets set.
+ * 	 	 	 	 	Only is cleared by clearMotionProfileHasUnderrun() to ensure
+ *
+ *	isUnderrun:		This is set if controller needs to shift a point from its buffer into
+ *					the active trajectory point however
+ *					the buffer is empty.
+ *					This gets cleared automatically when is resolved.
+ *
+ *	activePointValid:	True if the active trajectory point has not empty, false otherwise. The members in activePoint are only valid if this signal is set.
+ *
+ *	isLast:	is set/cleared based on the MP executer's current
+ *                trajectory point's IsLast value.  This assumes
+ *                IsLast was set when PushMotionProfileTrajectory
+ *                was used to insert the currently processed trajectory
+ *                point.
+ *
+ *	profileSlotSelect0: The currently processed trajectory point's
+ *      			  selected slot.  This can differ in the currently selected slot used
+ *       				 for Position and Velocity servo modes.   Must be within  [0,3].
+*
+ *	profileSlotSelect1: The currently processed trajectory point's
+ *      			  selected slot for auxiliary PID.  This can differ in the currently selected slot used
+ *       				 for Position and Velocity servo modes.  Must be within  [0,1].
+ *
+ *	outputEnable:		The current output mode of the motion profile
+ *						executer (disabled, enabled, or hold).  When changing the set()
+ *						value in MP mode, it's important to check this signal to
+ *						confirm the change takes effect before interacting with the top buffer.
+ */
+ErrorCode BaseMotorController::GetMotionProfileStatus(
+		ctre::phoenix::motion::MotionProfileStatus & statusToFill) {
+
+	int outputEnable = 0;
+	ErrorCode retval = c_MotController_GetMotionProfileStatus_2(m_handle,
+			&statusToFill.topBufferRem, &statusToFill.topBufferCnt,
+			&statusToFill.btmBufferCnt, &statusToFill.hasUnderrun,
+			&statusToFill.isUnderrun, &statusToFill.activePointValid,
+			&statusToFill.isLast, &statusToFill.profileSlotSelect0,
+			&outputEnable, &statusToFill.timeDurMs, &statusToFill.profileSlotSelect1);
+
+	statusToFill.outputEnable =
+			(ctre::phoenix::motion::SetValueMotionProfile) outputEnable;
+
+	return retval;
+}
+/**
+ * Clear the "Has Underrun" flag.  Typically this is called after application
+ * has confirmed an underrun had occured.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ClearMotionProfileHasUnderrun(int timeoutMs) {
+	return c_MotController_ClearMotionProfileHasUnderrun(m_handle, timeoutMs);
+}
+/**
+ * Calling application can opt to speed up the handshaking between the robot API
+ * and the controller to increase the download rate of the controller's Motion Profile.
+ * Ideally the period should be no more than half the period of a trajectory
+ * point.
+ * @param periodMs The transmit period in ms.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ChangeMotionControlFramePeriod(int periodMs) {
+	return c_MotController_ChangeMotionControlFramePeriod(m_handle, periodMs);
+}
+/**
+ * When trajectory points are processed in the motion profile executer, the MPE determines
+ * how long to apply the active trajectory point by summing baseTrajDurationMs with the
+ * timeDur of the trajectory point (see TrajectoryPoint).
+ *
+ * This allows general selection of the execution rate of the points with 1ms resolution,
+ * while allowing some degree of change from point to point.
+ * @param baseTrajDurationMs The base duration time of every trajectory point.
+ * 							This is summed with the trajectory points unique timeDur.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigMotionProfileTrajectoryPeriod(int baseTrajDurationMs, int timeoutMs) {
+	return c_MotController_ConfigMotionProfileTrajectoryPeriod(m_handle, baseTrajDurationMs, timeoutMs);
+}
+
+//------ error ----------//
+/**
+ * Gets the last error generated by this object.
+ * Not all functions return an error code but can potentially report errors.
+ * This function can be used to retrieve those error codes.
+ *
+ * @return Last Error Code generated by a function.
+ */
+ErrorCode BaseMotorController::GetLastError() {
+	return c_MotController_GetLastError(m_handle);
+}
+
+//------ Faults ----------//
+/**
+ * Polls the various fault flags.
+ * @param toFill Caller's object to fill with latest fault flags.
+ * @return Last Error Code generated by a function.
+ */
+ErrorCode BaseMotorController::GetFaults(Faults & toFill) {
+	int faultBits;
+	ErrorCode retval = c_MotController_GetFaults(m_handle, &faultBits);
+	toFill = Faults(faultBits);
+	return retval;
+}
+/**
+ * Polls the various sticky fault flags.
+ * @param toFill Caller's object to fill with latest sticky fault flags.
+ * @return Last Error Code generated by a function.
+ */
+ErrorCode BaseMotorController::GetStickyFaults(StickyFaults & toFill) {
+	int faultBits;
+	ErrorCode retval = c_MotController_GetStickyFaults(m_handle, &faultBits);
+	toFill = StickyFaults(faultBits);
+	return retval;
+}
+/**
+ * Clears all sticky faults.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Last Error Code generated by a function.
+ */
+ErrorCode BaseMotorController::ClearStickyFaults(int timeoutMs) {
+	return c_MotController_ClearStickyFaults(m_handle, timeoutMs);
+}
+
+//------ Firmware ----------//
+/**
+ * Gets the firmware version of the device.
+ *
+ * @return Firmware version of device.  For example: version 1-dot-2 is 0x0102.
+ */
+int BaseMotorController::GetFirmwareVersion() {
+	int retval = -1;
+	c_MotController_GetFirmwareVersion(m_handle, &retval);
+	return retval;
+}
+/**
+ * Returns true if the device has reset since last call.
+ *
+ * @return Has a Device Reset Occurred?
+ */
+bool BaseMotorController::HasResetOccurred() {
+	bool retval = false;
+	c_MotController_HasResetOccurred(m_handle, &retval);
+	return retval;
+}
+
+//------ Custom Persistent Params ----------//
+/**
+ * Sets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/limit/target
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param newValue
+ *            Value for custom parameter.
+ * @param paramIndex
+ *            Index of custom parameter [0,1]
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSetCustomParam(int newValue,
+		int paramIndex, int timeoutMs) {
+	return c_MotController_ConfigSetCustomParam(m_handle, newValue, paramIndex,
+			timeoutMs);
+}
+
+/**
+ * Gets the value of a custom parameter.
+ *
+ * @param paramIndex
+ *            Index of custom parameter [0,1].
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Value of the custom param.
+ */
+int BaseMotorController::ConfigGetCustomParam(int paramIndex, int timeoutMs) {
+	int readValue;
+	c_MotController_ConfigGetCustomParam(m_handle, &readValue, paramIndex,
+			timeoutMs);
+	return readValue;
+}
+
+//------ Generic Param API  ----------//
+/**
+ * Sets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ *            Parameter enumeration.
+ * @param value
+ *            Value of parameter.
+ * @param subValue
+ *            Subvalue for parameter. Maximum value of 255.
+ * @param ordinal
+ *            Ordinal of parameter.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSetParameter(ParamEnum param, double value,
+		uint8_t subValue, int ordinal, int timeoutMs) {
+	return c_MotController_ConfigSetParameter(m_handle, param, value, subValue,
+			ordinal, timeoutMs);
+}
+
+/**
+ * Gets a parameter.
+ *
+ * @param param
+ *            Parameter enumeration.
+ * @param ordinal
+ *            Ordinal of parameter.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Value of parameter.
+ */
+double BaseMotorController::ConfigGetParameter(ParamEnum param, int ordinal,
+		int timeoutMs) {
+	double value = 0;
+	c_MotController_ConfigGetParameter(m_handle, param, &value, ordinal,
+			timeoutMs);
+	return (double) value;
+}
+
+//------ Misc. ----------//
+int BaseMotorController::GetBaseID() {
+	return _arbId;
+}
+/**
+ * @return control mode motor controller is in
+ */
+ControlMode BaseMotorController::GetControlMode() {
+	return m_controlMode;
+}
+
+// ----- Follower ------//
+/**
+ * Set the control mode and output value so that this motor controller will
+ * follow another motor controller. Currently supports following Victor SPX
+ * and Talon SRX.
+ *
+ * @param masterToFollow
+ *						Motor Controller object to follow.
+ * @param followerType
+ *						Type of following control.  Use AuxOutput1 to follow the master
+ *						device's auxiliary output 1.
+ *						Use PercentOutput for standard follower mode.
+ */
+void BaseMotorController::Follow(IMotorController & masterToFollow, FollowerType followerType) {
+	uint32_t baseId = masterToFollow.GetBaseID();
+	uint32_t id24 = (uint16_t) (baseId >> 0x10);
+	id24 <<= 8;
+	id24 |= (uint8_t) (baseId);
+
+	switch (followerType) {
+		case FollowerType_PercentOutput:
+			Set(ControlMode::Follower, (double) id24);
+			break;
+		case FollowerType_AuxOutput1:
+			/* follow the motor controller, but set the aux flag
+			 * to ensure we follow the processed output */
+			Set(ControlMode::Follower, (double) id24, DemandType_AuxPID, 0);
+			break;
+		default:
+			NeutralOutput();
+			break;
+	}
+}
+/**
+ * Set the control mode and output value so that this motor controller will
+ * follow another motor controller.
+ * Currently supports following Victor SPX and Talon SRX.
+ */
+void BaseMotorController::Follow(IMotorController & masterToFollow) {
+	Follow(masterToFollow, FollowerType_PercentOutput);
+}
+/** When master makes a device, this routine is called to signal the update. */
+void BaseMotorController::ValueUpdated() {
+	//do nothing
+}
+/**
+ * @return object that can get/set individual raw sensor values.
+ */
+ctre::phoenix::motorcontrol::SensorCollection & BaseMotorController::GetSensorCollection() {
+	return *_sensorColl;
+}
diff --git a/cpp/src/MotorControl/CAN/TalonSRX.cpp b/cpp/src/MotorControl/CAN/TalonSRX.cpp
new file mode 100644
index 0000000..c18800c
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/TalonSRX.cpp
@@ -0,0 +1,343 @@
+#include "ctre/phoenix/MotorControl/CAN/TalonSRX.h"
+#include "ctre/phoenix/CCI/MotController_CCI.h"
+#include "HAL/HAL.h"
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol::can;
+using namespace ctre::phoenix::motorcontrol;
+
+/**
+ * Constructor
+ * @param deviceNumber [0,62]
+ */
+TalonSRX::TalonSRX(int deviceNumber) :
+		BaseMotorController(deviceNumber | 0x02040000) {
+			HAL_Report(HALUsageReporting::kResourceType_CANTalonSRX, deviceNumber + 1);
+}
+/**
+ * Select the feedback device for the motor controller.
+ *
+ * @param feedbackDevice
+ *            Feedback Device to select.
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigSelectedFeedbackSensor(FeedbackDevice feedbackDevice,
+		int pidIdx, int timeoutMs) {
+	return BaseMotorController::ConfigSelectedFeedbackSensor(feedbackDevice,
+			pidIdx, timeoutMs);
+}
+/**
+ * Select the remote feedback device for the motor controller.
+ * Most CTRE CAN motor controllers will support remote sensors over CAN.
+ *
+ * @param feedbackDevice
+ *            Remote Feedback Device to select.
+ * @param pidIdx
+ *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigSelectedFeedbackSensor(RemoteFeedbackDevice feedbackDevice,
+		int pidIdx, int timeoutMs) {
+	return BaseMotorController::ConfigSelectedFeedbackSensor(feedbackDevice,
+			pidIdx, timeoutMs);
+}
+/**
+ * Sets the period of the given status frame.
+ *
+ * User ensure CAN Bus utilization is not high.
+ *
+ * This setting is not persistent and is lost when device is reset.
+ * If this is a concern, calling application can use HasReset()
+ * to determine if the status frame needs to be reconfigured.
+ *
+ * @param frame
+ *            Frame whose period is to be changed.
+ * @param periodMs
+ *            Period in ms for the given frame.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::SetStatusFramePeriod(StatusFrameEnhanced frame,
+		int periodMs, int timeoutMs) {
+	return BaseMotorController::SetStatusFramePeriod(frame, periodMs, timeoutMs);
+}
+/**
+ * Sets the period of the given status frame.
+ *
+ * User ensure CAN Bus utilization is not high.
+ *
+ * This setting is not persistent and is lost when device is reset.
+ * If this is a concern, calling application can use HasReset()
+ * to determine if the status frame needs to be reconfigured.
+ *
+ * @param frame
+ *            Frame whose period is to be changed.
+ * @param periodMs
+ *            Period in ms for the given frame.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::SetStatusFramePeriod(StatusFrame frame,
+		int periodMs, int timeoutMs) {
+	return BaseMotorController::SetStatusFramePeriod(frame, periodMs, timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ *            Frame to get the period of.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int TalonSRX::GetStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs) {
+	return BaseMotorController::GetStatusFramePeriod(frame, timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ *            Frame to get the period of.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int TalonSRX::GetStatusFramePeriod(StatusFrame frame, int timeoutMs) {
+	return BaseMotorController::GetStatusFramePeriod(frame, timeoutMs);
+}
+/**
+ * Configures the period of each velocity sample.
+ * Every 1ms a position value is sampled, and the delta between that sample
+ * and the position sampled kPeriod ms ago is inserted into a filter.
+ * kPeriod is configured with this function.
+ *
+ * @param period
+ *            Desired period for the velocity measurement. @see
+ *            #VelocityMeasPeriod
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigVelocityMeasurementPeriod(VelocityMeasPeriod period,
+		int timeoutMs) {
+	return BaseMotorController::ConfigVelocityMeasurementPeriod(period,
+			timeoutMs);
+}
+/**
+ * Sets the number of velocity samples used in the rolling average velocity
+ * measurement.
+ *
+ * @param windowSize
+ *            Number of samples in the rolling average of velocity
+ *            measurement. Valid values are 1,2,4,8,16,32. If another
+ *            value is specified, it will truncate to nearest support value.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigVelocityMeasurementWindow(int windowSize,
+		int timeoutMs) {
+	return BaseMotorController::ConfigVelocityMeasurementWindow(windowSize,
+			timeoutMs);
+}
+/**
+ * Configures a limit switch for a local/remote source.
+ *
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon, CANifier, or local Gadgeteer feedback connector.
+ *
+ * If the sensor is remote, a device ID of zero is assumed.
+ * If that's not desired, use the four parameter version of this function.
+ *
+ * @param limitSwitchSource
+ *            Limit switch source.
+ *            User can choose between the feedback connector, remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ *            Setting for normally open, normally closed, or disabled. This setting
+ *            matches the web-based configuration drop down.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigForwardLimitSwitchSource(
+		LimitSwitchSource limitSwitchSource,
+		LimitSwitchNormal normalOpenOrClose, int timeoutMs) {
+
+	return BaseMotorController::ConfigForwardLimitSwitchSource(
+			limitSwitchSource, normalOpenOrClose, timeoutMs);
+}
+/**
+ * Configures a limit switch for a local/remote source.
+ *
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon, CANifier, or local Gadgeteer feedback connector.
+ *
+ * If the sensor is remote, a device ID of zero is assumed.
+ * If that's not desired, use the four parameter version of this function.
+ *
+ * @param limitSwitchSource
+ *            Limit switch source. @see #LimitSwitchSource
+ *            User can choose between the feedback connector, remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ *            Setting for normally open, normally closed, or disabled. This setting
+ *            matches the web-based configuration drop down.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigReverseLimitSwitchSource(
+		LimitSwitchSource limitSwitchSource,
+		LimitSwitchNormal normalOpenOrClose, int timeoutMs) {
+	return BaseMotorController::ConfigReverseLimitSwitchSource(
+			limitSwitchSource, normalOpenOrClose, timeoutMs);
+}
+/**
+ * Configures the forward limit switch for a remote source.
+ * For example, a CAN motor controller may need to monitor the Limit-F pin
+ * of another Talon or CANifier.
+ *
+ * @param limitSwitchSource
+ *            Remote limit switch source.
+ *            User can choose between a remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ *            Setting for normally open, normally closed, or disabled. This setting
+ *            matches the web-based configuration drop down.
+ * @param deviceID
+ *            Device ID of remote source (Talon SRX or CANifier device ID).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigForwardLimitSwitchSource(
+		RemoteLimitSwitchSource limitSwitchSource,
+		LimitSwitchNormal normalOpenOrClose, int deviceID, int timeoutMs) {
+
+	return BaseMotorController::ConfigForwardLimitSwitchSource(
+			limitSwitchSource, normalOpenOrClose, deviceID, timeoutMs);
+}
+/**
+ * Configures the reverse limit switch for a remote source.
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon or CANifier.
+ *
+ * @param limitSwitchSource
+ *            Remote limit switch source.
+ *            User can choose between a remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ *            Setting for normally open, normally closed, or disabled. This setting
+ *            matches the web-based configuration drop down.
+ * @param deviceID
+ *            Device ID of remote source (Talon SRX or CANifier device ID).
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigReverseLimitSwitchSource(
+		RemoteLimitSwitchSource limitSwitchSource,
+		LimitSwitchNormal normalOpenOrClose, int deviceID, int timeoutMs) {
+
+	return BaseMotorController::ConfigReverseLimitSwitchSource(
+			limitSwitchSource, normalOpenOrClose, deviceID, timeoutMs);
+}
+
+//------ Current Lim ----------//
+/**
+ * Configure the peak allowable current (when current limit is enabled).
+ *
+ * Current limit is activated when current exceeds the peak limit for longer than the peak duration.
+ * Then software will limit to the continuous limit.
+ * This ensures current limiting while allowing for momentary excess current events.
+ *
+ * For simpler current-limiting (single threshold) use ConfigContinuousCurrentLimit()
+ * and set the peak to zero: ConfigPeakCurrentLimit(0).
+ *
+ * @param amps	Amperes to limit.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigPeakCurrentLimit(int amps, int timeoutMs) {
+	return c_MotController_ConfigPeakCurrentLimit(m_handle, amps, timeoutMs);
+}
+/**
+ * Configure the peak allowable duration (when current limit is enabled).
+ *
+ * Current limit is activated when current exceeds the peak limit for longer than the peak duration.
+ * Then software will limit to the continuous limit.
+ * This ensures current limiting while allowing for momentary excess current events.
+ *
+ * For simpler current-limiting (single threshold) use ConfigContinuousCurrentLimit()
+ * and set the peak to zero: ConfigPeakCurrentLimit(0).
+ *
+ * @param milliseconds How long to allow current-draw past peak limit.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigPeakCurrentDuration(int milliseconds, int timeoutMs) {
+	return c_MotController_ConfigPeakCurrentDuration(m_handle, milliseconds,
+			timeoutMs);
+}
+/**
+ * Configure the continuous allowable current-draw (when current limit is enabled).
+ *
+ * Current limit is activated when current exceeds the peak limit for longer than the peak duration.
+ * Then software will limit to the continuous limit.
+ * This ensures current limiting while allowing for momentary excess current events.
+ *
+ * For simpler current-limiting (single threshold) use ConfigContinuousCurrentLimit()
+ * and set the peak to zero: ConfigPeakCurrentLimit(0).
+ *
+ * @param amps	Amperes to limit.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigContinuousCurrentLimit(int amps, int timeoutMs) {
+	return c_MotController_ConfigContinuousCurrentLimit(m_handle, amps, timeoutMs);
+}
+/**
+ * Enable or disable Current Limit.
+ * @param enable
+ *            Enable state of current limit.
+ * @see ConfigPeakCurrentLimit, ConfigPeakCurrentDuration, ConfigContinuousCurrentLimit
+ */
+void TalonSRX::EnableCurrentLimit(bool enable) {
+	c_MotController_EnableCurrentLimit(m_handle, enable);
+}
+
diff --git a/cpp/src/MotorControl/CAN/VictorSPX.cpp b/cpp/src/MotorControl/CAN/VictorSPX.cpp
new file mode 100644
index 0000000..11bcdf8
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/VictorSPX.cpp
@@ -0,0 +1,13 @@
+#include "ctre/phoenix/MotorControl/CAN/VictorSPX.h"
+#include "HAL/HAL.h"
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol::can;
+/**
+ * Constructor
+ * @param deviceNumber [0,62]
+ */
+VictorSPX::VictorSPX(int deviceNumber) :
+    BaseMotorController(deviceNumber | 0x01040000) {
+		HAL_Report(HALUsageReporting::kResourceType_CTRE_future1, deviceNumber + 1);
+	}
diff --git a/cpp/src/MotorControl/CAN/WPI_TalonSRX.cpp b/cpp/src/MotorControl/CAN/WPI_TalonSRX.cpp
new file mode 100644
index 0000000..30a8bc9
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/WPI_TalonSRX.cpp
@@ -0,0 +1,157 @@
+/**
+ * WPI Compliant motor controller class.
+ * WPILIB's object model requires many interfaces to be implemented to use
+ * the various features.
+ * This includes...
+ * - Software PID loops running in the robot controller
+ * - LiveWindow/Test mode features
+ * - Motor Safety (auto-turn off of motor if Set stops getting called)
+ * - Single Parameter set that assumes a simple motor controller.
+ */
+#include "ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h"
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "HAL/HAL.h"
+#include <sstream>
+
+using namespace ctre::phoenix::motorcontrol::can;
+
+
+/** Constructor */
+WPI_TalonSRX::WPI_TalonSRX(int deviceNumber) :
+		BaseMotorController(deviceNumber | 0x02040000),
+		TalonSRX(deviceNumber),
+		_safetyHelper(this) {
+	HAL_Report(HALUsageReporting::kResourceType_CTRE_future2, deviceNumber + 1);
+	/* build string description */
+	std::stringstream work;
+	work << "Talon SRX " << deviceNumber;
+	_desc = work.str();
+	/* prep motor safety */
+	_safetyHelper.SetExpiration(0.0);
+	_safetyHelper.SetSafetyEnabled(false);
+	SetName("Talon SRX ", deviceNumber);
+}
+WPI_TalonSRX::~WPI_TalonSRX() {
+	/* MT */
+}
+
+//----------------------- set/get routines for WPILIB interfaces -------------------//
+/**
+ * Common interface for setting the speed of a simple speed controller.
+ *
+ * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+ * 									Value is also saved for Get().
+ */
+void WPI_TalonSRX::Set(double speed) {
+	_speed = speed;
+	TalonSRX::Set(ControlMode::PercentOutput, speed);
+	_safetyHelper.Feed();
+}
+void WPI_TalonSRX::PIDWrite(double output) {
+	Set(output);
+}
+
+/**
+ * Common interface for getting the current set speed of a speed controller.
+ *
+ * @return The current set speed.  Value is between -1.0 and 1.0.
+ */
+double WPI_TalonSRX::Get() const {
+	return _speed;
+}
+
+//----------------------- Intercept CTRE calls for motor safety -------------------//
+void WPI_TalonSRX::Set(ControlMode mode, double value) {
+	/* intercept the advanced Set and feed motor-safety */
+	TalonSRX::Set(mode, value);
+	_safetyHelper.Feed();
+}
+void WPI_TalonSRX::Set(ControlMode mode, double demand0, double demand1) {
+	/* intercept the advanced Set and feed motor-safety */
+	TalonSRX::Set(mode, demand0, demand1);
+	_safetyHelper.Feed();
+}
+//----------------------- Invert routines -------------------//
+/**
+ * Common interface for inverting direction of a speed controller.
+ *
+ * @param isInverted The state of inversion, true is inverted.
+ */
+void WPI_TalonSRX::SetInverted(bool isInverted) {
+	TalonSRX::SetInverted(isInverted);
+}
+/**
+ * Common interface for returning the inversion state of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ */
+bool WPI_TalonSRX::GetInverted() const {
+	return BaseMotorController::GetInverted();
+}
+//----------------------- turn-motor-off routines-------------------//
+/**
+ * Common interface for disabling a motor.
+ */
+void WPI_TalonSRX::Disable() {
+	NeutralOutput();
+}
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void WPI_TalonSRX::StopMotor() {
+	NeutralOutput();
+}
+
+//----------------------- Motor Safety-------------------//
+
+/**
+ * Set the safety expiration time.
+ *
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+void WPI_TalonSRX::SetExpiration(double timeout) {
+	_safetyHelper.SetExpiration(timeout);
+}
+
+/**
+ * Return the safety expiration time.
+ *
+ * @return The expiration time value.
+ */
+double WPI_TalonSRX::GetExpiration() const {
+	return _safetyHelper.GetExpiration();
+}
+
+/**
+ * Check if the motor is currently alive or stopped due to a timeout.
+ *
+ * @return a bool value that is true if the motor has NOT timed out and should
+ *         still be running.
+ */
+bool WPI_TalonSRX::IsAlive() const {
+	return _safetyHelper.IsAlive();
+}
+
+/**
+ * Check if motor safety is enabled.
+ *
+ * @return True if motor safety is enforced for this object
+ */
+bool WPI_TalonSRX::IsSafetyEnabled() const {
+	return _safetyHelper.IsSafetyEnabled();
+}
+
+void WPI_TalonSRX::SetSafetyEnabled(bool enabled) {
+	_safetyHelper.SetSafetyEnabled(enabled);
+}
+
+void WPI_TalonSRX::GetDescription(llvm::raw_ostream& desc) const {
+	desc << _desc.c_str();
+}
+
+void WPI_TalonSRX::InitSendable(frc::SendableBuilder& builder) {
+	builder.SetSmartDashboardType("Speed Controller");
+	builder.SetSafeState([=]() {StopMotor();});
+	builder.AddDoubleProperty("Value", [=]() {return Get();},
+			[=](double value) {Set(value);});
+}
diff --git a/cpp/src/MotorControl/CAN/WPI_VictorSPX.cpp b/cpp/src/MotorControl/CAN/WPI_VictorSPX.cpp
new file mode 100644
index 0000000..cc8ddbd
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/WPI_VictorSPX.cpp
@@ -0,0 +1,157 @@
+/**
+ * WPI Compliant motor controller class.
+ * WPILIB's object model requires many interfaces to be implemented to use
+ * the various features.
+ * This includes...
+ * - Software PID loops running in the robot controller
+ * - LiveWindow/Test mode features
+ * - Motor Safety (auto-turn off of motor if Set stops getting called)
+ * - Single Parameter set that assumes a simple motor controller.
+ */
+#include "ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h"
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "HAL/HAL.h"
+#include <sstream>
+
+using namespace ctre::phoenix::motorcontrol::can;
+
+
+/** Constructor */
+WPI_VictorSPX::WPI_VictorSPX(int deviceNumber) :
+		BaseMotorController(deviceNumber | 0x01040000),
+		VictorSPX(deviceNumber),
+		_safetyHelper(this) {
+	HAL_Report(HALUsageReporting::kResourceType_CTRE_future3, deviceNumber + 1);
+	/* build string description */
+	std::stringstream work;
+	work << "Victor SPX " << deviceNumber;
+	_desc = work.str();
+	/* prep motor safety */
+	_safetyHelper.SetExpiration(0.0);
+	_safetyHelper.SetSafetyEnabled(false);
+	SetName("Victor SPX ", deviceNumber);
+}
+WPI_VictorSPX::~WPI_VictorSPX() {
+	/* MT */
+}
+
+//----------------------- set/get routines for WPILIB interfaces -------------------//
+/**
+ * Common interface for setting the speed of a simple speed controller.
+ *
+ * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+ * 									Value is also saved for Get().
+ */
+void WPI_VictorSPX::Set(double speed) {
+	_speed = speed;
+	VictorSPX::Set(ControlMode::PercentOutput, speed);
+	_safetyHelper.Feed();
+}
+void WPI_VictorSPX::PIDWrite(double output) {
+	Set(output);
+}
+
+/**
+ * Common interface for getting the current set speed of a speed controller.
+ *
+ * @return The current set speed.  Value is between -1.0 and 1.0.
+ */
+double WPI_VictorSPX::Get() const {
+	return _speed;
+}
+
+//----------------------- Intercept CTRE calls for motor safety -------------------//
+void WPI_VictorSPX::Set(ControlMode mode, double value) {
+	/* intercept the advanced Set and feed motor-safety */
+	VictorSPX::Set(mode, value);
+	_safetyHelper.Feed();
+}
+void WPI_VictorSPX::Set(ControlMode mode, double demand0, double demand1) {
+	/* intercept the advanced Set and feed motor-safety */
+	VictorSPX::Set(mode, demand0, demand1);
+	_safetyHelper.Feed();
+}
+//----------------------- Invert routines -------------------//
+/**
+ * Common interface for inverting direction of a speed controller.
+ *
+ * @param isInverted The state of inversion, true is inverted.
+ */
+void WPI_VictorSPX::SetInverted(bool isInverted) {
+	VictorSPX::SetInverted(isInverted);
+}
+/**
+ * Common interface for returning the inversion state of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ */
+bool WPI_VictorSPX::GetInverted() const {
+	return BaseMotorController::GetInverted();
+}
+//----------------------- turn-motor-off routines-------------------//
+/**
+ * Common interface for disabling a motor.
+ */
+void WPI_VictorSPX::Disable() {
+	NeutralOutput();
+}
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void WPI_VictorSPX::StopMotor() {
+	NeutralOutput();
+}
+
+//----------------------- Motor Safety-------------------//
+
+/**
+ * Set the safety expiration time.
+ *
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+void WPI_VictorSPX::SetExpiration(double timeout) {
+	_safetyHelper.SetExpiration(timeout);
+}
+
+/**
+ * Return the safety expiration time.
+ *
+ * @return The expiration time value.
+ */
+double WPI_VictorSPX::GetExpiration() const {
+	return _safetyHelper.GetExpiration();
+}
+
+/**
+ * Check if the motor is currently alive or stopped due to a timeout.
+ *
+ * @return a bool value that is true if the motor has NOT timed out and should
+ *         still be running.
+ */
+bool WPI_VictorSPX::IsAlive() const {
+	return _safetyHelper.IsAlive();
+}
+
+/**
+ * Check if motor safety is enabled.
+ *
+ * @return True if motor safety is enforced for this object
+ */
+bool WPI_VictorSPX::IsSafetyEnabled() const {
+	return _safetyHelper.IsSafetyEnabled();
+}
+
+void WPI_VictorSPX::SetSafetyEnabled(bool enabled) {
+	_safetyHelper.SetSafetyEnabled(enabled);
+}
+
+void WPI_VictorSPX::GetDescription(llvm::raw_ostream& desc) const {
+	desc << _desc.c_str();
+}
+
+void WPI_VictorSPX::InitSendable(frc::SendableBuilder& builder) {
+	builder.SetSmartDashboardType("Speed Controller");
+	builder.SetSafeState([=]() {StopMotor();});
+	builder.AddDoubleProperty("Value", [=]() {return Get();},
+			[=](double value) {Set(value);});
+}
diff --git a/cpp/src/MotorControl/DeviceCatalog.cpp b/cpp/src/MotorControl/DeviceCatalog.cpp
new file mode 100644
index 0000000..c59f43a3
--- /dev/null
+++ b/cpp/src/MotorControl/DeviceCatalog.cpp
@@ -0,0 +1,6 @@
+#include <ctre/phoenix/MotorControl/DeviceCatalog.h>
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol;
+
+DeviceCatalog * DeviceCatalog::_instance = 0;
diff --git a/cpp/src/MotorControl/GroupMotorControllers.cpp b/cpp/src/MotorControl/GroupMotorControllers.cpp
new file mode 100644
index 0000000..d2e126a
--- /dev/null
+++ b/cpp/src/MotorControl/GroupMotorControllers.cpp
@@ -0,0 +1,25 @@
+#include "ctre/phoenix/MotorControl/GroupMotorControllers.h"
+
+using namespace ctre::phoenix;
+
+namespace ctre {
+namespace phoenix  {
+namespace motorcontrol {
+
+std::vector<IMotorController*> GroupMotorControllers::_mcs;
+
+void GroupMotorControllers::Register(IMotorController *motorController) {
+	_mcs.push_back(motorController);
+}
+
+int GroupMotorControllers::MotorControllerCount() {
+	return _mcs.size();
+}
+
+IMotorController* GroupMotorControllers::Get(int idx) {
+	return _mcs[idx];
+}
+
+}
+}
+}
diff --git a/cpp/src/MotorControl/SensorCollection.cpp b/cpp/src/MotorControl/SensorCollection.cpp
new file mode 100644
index 0000000..a1ff1b1
--- /dev/null
+++ b/cpp/src/MotorControl/SensorCollection.cpp
@@ -0,0 +1,241 @@
+#include "ctre/phoenix/MotorControl/SensorCollection.h"
+#include "ctre/phoenix/CCI/MotController_CCI.h"
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol;
+
+SensorCollection::SensorCollection(void * handle) {
+	_handle = handle;
+}
+
+/**
+ * Get the position of whatever is in the analog pin of the Talon, regardless of
+ *   whether it is actually being used for feedback.
+ *
+ * @return  the 24bit analog value.  The bottom ten bits is the ADC (0 - 1023)
+ *          on the analog pin of the Talon. The upper 14 bits tracks the overflows and underflows
+ *          (continuous sensor).
+ */
+
+int SensorCollection::GetAnalogIn() {
+	int retval = 0;
+	c_MotController_GetAnalogIn(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Sets analog position.
+ *
+ * @param   newPosition The new position.
+ * @param   timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ *
+ * @return  an ErrorCode.
+ */
+
+ErrorCode SensorCollection::SetAnalogPosition(int newPosition, int timeoutMs) {
+	return c_MotController_SetAnalogPosition(_handle, newPosition, timeoutMs);
+}
+
+/**
+ * Get the position of whatever is in the analog pin of the Talon, regardless of whether
+ *   it is actually being used for feedback.
+ *
+ * @return  the ADC (0 - 1023) on analog pin of the Talon.
+ */
+
+int SensorCollection::GetAnalogInRaw() {
+	int retval = 0;
+	c_MotController_GetAnalogInRaw(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Get the velocity of whatever is in the analog pin of the Talon, regardless of
+ *   whether it is actually being used for feedback.
+ *
+ * @return  the speed in units per 100ms where 1024 units is one rotation.
+ */
+
+int SensorCollection::GetAnalogInVel() {
+	int retval = 0;
+	c_MotController_GetAnalogInVel(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Get the quadrature position of the Talon, regardless of whether
+ *   it is actually being used for feedback.
+ *
+ * @return  the quadrature position.
+ */
+
+int SensorCollection::GetQuadraturePosition() {
+	int retval = 0;
+	c_MotController_GetQuadraturePosition(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Change the quadrature reported position.  Typically this is used to "zero" the
+ *   sensor. This only works with Quadrature sensor.  To set the selected sensor position
+ *   regardless of what type it is, see SetSelectedSensorPosition in the motor controller class.
+ *
+ * @param   newPosition The position value to apply to the sensor.
+ * @param   timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ *
+ * @return  error code.
+ */
+
+ErrorCode SensorCollection::SetQuadraturePosition(int newPosition,
+		int timeoutMs) {
+	return c_MotController_SetQuadraturePosition(_handle, newPosition,
+			timeoutMs);
+}
+
+/**
+ * Get the quadrature velocity, regardless of whether
+ *   it is actually being used for feedback.
+ *
+ * @return  the quadrature velocity in units per 100ms.
+ */
+
+int SensorCollection::GetQuadratureVelocity() {
+	int retval = 0;
+	c_MotController_GetQuadratureVelocity(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Gets pulse width position, regardless of whether
+ *   it is actually being used for feedback.
+ *
+ * @return  the pulse width position.
+ */
+
+int SensorCollection::GetPulseWidthPosition() {
+	int retval = 0;
+	c_MotController_GetPulseWidthPosition(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Sets pulse width position.
+ *
+ * @param   newPosition The position value to apply to the sensor.
+ * @param   timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ *
+ * @return  an ErrErrorCode
+ */
+ErrorCode SensorCollection::SetPulseWidthPosition(int newPosition,
+		int timeoutMs) {
+	return c_MotController_SetPulseWidthPosition(_handle, newPosition,
+			timeoutMs);
+}
+
+/**
+ * Gets pulse width velocity, regardless of whether
+ *   it is actually being used for feedback.
+ *
+ * @return  the pulse width velocity in units per 100ms (where 4096 units is 1 rotation).
+ */
+
+int SensorCollection::GetPulseWidthVelocity() {
+	int retval = 0;
+	c_MotController_GetPulseWidthVelocity(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Gets pulse width rise to fall time.
+ *
+ * @return  the pulse width rise to fall time in microseconds.
+ */
+
+int SensorCollection::GetPulseWidthRiseToFallUs() {
+	int retval = 0;
+	c_MotController_GetPulseWidthRiseToFallUs(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Gets pulse width rise to rise time.
+ *
+ * @return  the pulse width rise to rise time in microseconds.
+ */
+
+int SensorCollection::GetPulseWidthRiseToRiseUs() {
+	int retval = 0;
+	c_MotController_GetPulseWidthRiseToRiseUs(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Gets pin state quad a.
+ *
+ * @return  the pin state of quad a (1 if asserted, 0 if not asserted).
+ */
+
+int SensorCollection::GetPinStateQuadA() {
+	int retval = 0;
+	c_MotController_GetPinStateQuadA(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Gets pin state quad b.
+ *
+ * @return  Digital level of QUADB pin (1 if asserted, 0 if not asserted).
+ */
+
+int SensorCollection::GetPinStateQuadB() {
+	int retval = 0;
+	c_MotController_GetPinStateQuadB(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Gets pin state quad index.
+ *
+ * @return  Digital level of QUAD Index pin (1 if asserted, 0 if not asserted).
+ */
+
+int SensorCollection::GetPinStateQuadIdx() {
+	int retval = 0;
+	c_MotController_GetPinStateQuadIdx(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Is forward limit switch closed.
+ *
+ * @return  '1' iff forward limit switch is closed, 0 iff switch is open. This function works
+ *          regardless if limit switch feature is enabled.
+ */
+
+int SensorCollection::IsFwdLimitSwitchClosed() {
+	int retval = 0;
+	c_MotController_IsFwdLimitSwitchClosed(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Is reverse limit switch closed.
+ *
+ * @return  '1' iff reverse limit switch is closed, 0 iff switch is open. This function works
+ *          regardless if limit switch feature is enabled.
+ */
+
+int SensorCollection::IsRevLimitSwitchClosed() {
+	int retval = 0;
+	c_MotController_IsRevLimitSwitchClosed(_handle, &retval);
+	return retval;
+}
diff --git a/cpp/src/RCRadio3Ch.cpp b/cpp/src/RCRadio3Ch.cpp
new file mode 100644
index 0000000..2fea763
--- /dev/null
+++ b/cpp/src/RCRadio3Ch.cpp
@@ -0,0 +1,113 @@
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+#include "ctre/phoenix/RCRadio3Ch.h"
+#include <vector>
+
+namespace ctre {
+namespace phoenix {
+
+RCRadio3Ch::RCRadio3Ch(ctre::phoenix::CANifier *canifier) {
+	_canifier = canifier;
+}
+
+float RCRadio3Ch::GetDutyCycleUs(Channel channel) {
+	return _dutyCycleAndPeriods[(int) channel][0];
+}
+
+float RCRadio3Ch::GetDutyCyclePerc(Channel channel) {
+	float retval = RCRadio3Ch::GetDutyCycleUs(channel);
+
+	std::vector<double> xData = { 1000, 2000 };
+	std::vector<double> yData = { -1, 1 };
+
+	retval = RCRadio3Ch::Interpolate(xData, yData, retval, true);
+
+	if (retval < -1) {
+		retval = -1;
+	} else if (retval > +1) {
+		retval = +1;
+	}
+
+	return retval;
+}
+
+bool RCRadio3Ch::GetSwitchValue(Channel channel) {
+	float retval = RCRadio3Ch::GetDutyCyclePerc(channel);
+
+	return retval > 0.5f;
+}
+
+float RCRadio3Ch::GetPeriodUs(Channel channel) {
+	return _dutyCycleAndPeriods[(int) channel][1];
+}
+
+void RCRadio3Ch::Process() {
+	//Does some error code stuff, which we don't have...
+	_errorCodes[0] = _canifier->GetPWMInput(
+			ctre::phoenix::CANifier::PWMChannel::PWMChannel0, _dutyCycleAndPeriods[0]);
+	_errorCodes[1] = _canifier->GetPWMInput(
+			ctre::phoenix::CANifier::PWMChannel::PWMChannel1, _dutyCycleAndPeriods[1]);
+	_errorCodes[2] = _canifier->GetPWMInput(
+			ctre::phoenix::CANifier::PWMChannel::PWMChannel2, _dutyCycleAndPeriods[2]);
+	_errorCodes[3] = _canifier->GetPWMInput(
+			ctre::phoenix::CANifier::PWMChannel::PWMChannel3, _dutyCycleAndPeriods[3]);
+
+	Status health = Status::Okay;
+	if (health == Status::Okay) {
+		if (_errorCodes[0] < 0) {
+			health = Status::LossOfCAN;
+		}
+		if (_errorCodes[1] < 0) {
+			health = Status::LossOfCAN;
+		}
+		if (_errorCodes[2] < 0) {
+			health = Status::LossOfCAN;
+		}
+		if (_errorCodes[3] < 0) {
+			health = Status::LossOfCAN;
+		}
+	}
+
+	if (health == Status::Okay) {
+		if (RCRadio3Ch::GetPeriodUs(RCRadio3Ch::Channel1) == 0) {
+			health = Status::LossOfPwm;
+		}
+		if (RCRadio3Ch::GetPeriodUs(RCRadio3Ch::Channel2) == 0) {
+			health = Status::LossOfPwm;
+		}
+		if (RCRadio3Ch::GetPeriodUs(RCRadio3Ch::Channel3) == 0) {
+			health = Status::LossOfPwm;
+		}
+	}
+	CurrentStatus = health;	//Will have to change this to a getter and a setter
+}
+
+double RCRadio3Ch::Interpolate(std::vector<double> &xData,
+		std::vector<double> &yData, double x, bool extrapolate) {
+	int size = xData.size();
+
+	int i = 0;                    // find left end of interval for interpolation
+	if (x >= xData[size - 2])                  // special case: beyond right end
+			{
+		i = size - 2;
+	} else {
+		while (x > xData[i + 1])
+			i++;
+	}
+	double xL = xData[i], yL = yData[i], xR = xData[i + 1], yR = yData[i + 1]; // points on either side (unless beyond ends)
+	if (!extrapolate)           // if beyond ends of array and not extrapolating
+	{
+		if (x < xL)
+			yR = yL;
+		if (x > xR)
+			yL = yR;
+	}
+
+	double dydx = (yR - yL) / (xR - xL);                             // gradient
+
+	return yL + dydx * (x - xL);
+}
+
+}
+}
+#endif
diff --git a/cpp/src/Sensors/PigeonIMU.cpp b/cpp/src/Sensors/PigeonIMU.cpp
new file mode 100644
index 0000000..4755e43
--- /dev/null
+++ b/cpp/src/Sensors/PigeonIMU.cpp
@@ -0,0 +1,779 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ *
+ * Cross The Road Electronics (CTRE) licenses to you the right to
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ *
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+
+#include <memory>
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+#include "ctre/phoenix/Sensors/PigeonIMU.h"
+#include "ctre/phoenix/CTRLogger.h"
+#include "ctre/phoenix/CCI/Logger_CCI.h"
+#include "ctre/phoenix/CCI/PigeonIMU_CCI.h"
+#include "ctre/phoenix/MotorControl/CAN/TalonSRX.h"
+
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+
+#include "Utility.h"
+#include "HAL/HAL.h"
+
+using namespace ctre::phoenix::motorcontrol::can;
+
+namespace ctre {
+namespace phoenix {
+namespace sensors {
+
+/**
+ * Create a Pigeon object that communicates with Pigeon on CAN Bus.
+ * @param deviceNumber CAN Device Id of Pigeon [0,62]
+ */
+PigeonIMU::PigeonIMU(int deviceNumber) :
+		CANBusAddressable(deviceNumber) {
+	_handle = c_PigeonIMU_Create1(deviceNumber);
+	_deviceNumber = deviceNumber;
+	HAL_Report(HALUsageReporting::kResourceType_PigeonIMU, _deviceNumber + 1);
+}
+
+/**
+ * Create a Pigeon object that communciates with Pigeon through the Gadgeteer ribbon cable connected to a Talon on CAN Bus.
+ * @param talonSrx Object for the TalonSRX connected via ribbon cable.
+ */
+PigeonIMU::PigeonIMU(ctre::phoenix::motorcontrol::can::TalonSRX * talonSrx) :
+		CANBusAddressable(0) {
+	_handle = c_PigeonIMU_Create2(talonSrx->GetDeviceID());
+	_deviceNumber = talonSrx->GetDeviceID();
+	HAL_Report(HALUsageReporting::kResourceType_PigeonIMU, _deviceNumber + 1);
+	HAL_Report(HALUsageReporting::kResourceType_CTRE_future0, _deviceNumber + 1); //record as Pigeon-via-Uart
+}
+
+/**
+ * Sets the Yaw register to the specified value.
+ *
+ * @param angleDeg  Degree of Yaw [+/- 23040 degrees]
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetYaw(double angleDeg, int timeoutMs) {
+	int errCode = c_PigeonIMU_SetYaw(_handle, angleDeg, timeoutMs);
+	return errCode;
+}
+/**
+ * Atomically add to the Yaw register.
+ *
+ * @param angleDeg  Degrees to add to the Yaw register.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::AddYaw(double angleDeg, int timeoutMs) {
+	int errCode = c_PigeonIMU_AddYaw(_handle, angleDeg, timeoutMs);
+	return errCode;
+}
+/**
+ * Sets the Yaw register to match the current compass value.
+ *
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetYawToCompass(int timeoutMs) {
+	int errCode = c_PigeonIMU_SetYawToCompass(_handle, timeoutMs);
+	return errCode;
+}
+/**
+ * Sets the Fused Heading to the specified value.
+ *
+ * @param angleDeg  Degree of heading [+/- 23040 degrees]
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetFusedHeading(double angleDeg, int timeoutMs) {
+	int errCode = c_PigeonIMU_SetFusedHeading(_handle, angleDeg, timeoutMs);
+	return errCode;
+}
+/**
+ * Sets the AccumZAngle.
+ *
+ * @param angleDeg  Degrees to set AccumZAngle to.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetAccumZAngle(double angleDeg, int timeoutMs) {
+	int errCode = c_PigeonIMU_SetAccumZAngle(_handle, angleDeg, timeoutMs);
+	return errCode;
+}
+/**
+ * Enable/Disable Temp compensation. Pigeon defaults with this on at boot.
+ *
+ * @param bTempCompEnable Set to "True" to enable temperature compensation.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::ConfigTemperatureCompensationEnable(bool bTempCompEnable,
+		int timeoutMs) {
+	int errCode = c_PigeonIMU_ConfigTemperatureCompensationEnable(_handle,
+			bTempCompEnable, timeoutMs);
+	return errCode;
+}
+/**
+ * Atomically add to the Fused Heading register.
+ *
+ * @param angleDeg  Degrees to add to the Fused Heading register.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::AddFusedHeading(double angleDeg, int timeoutMs) {
+	int errCode = c_PigeonIMU_AddFusedHeading(_handle, angleDeg, timeoutMs);
+	return errCode;
+}
+/**
+ * Sets the Fused Heading register to match the current compass value.
+ *
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetFusedHeadingToCompass(int timeoutMs) {
+	int errCode = c_PigeonIMU_SetFusedHeadingToCompass(_handle, timeoutMs);
+	return errCode;
+}
+/**
+ * Set the declination for compass. Declination is the difference between
+ * Earth Magnetic north, and the geographic "True North".
+ *
+ * @param angleDegOffset  Degrees to set Compass Declination to.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetCompassDeclination(double angleDegOffset, int timeoutMs) {
+	int errCode = c_PigeonIMU_SetCompassDeclination(_handle, angleDegOffset,
+			timeoutMs);
+	return errCode;
+}
+/**
+ * Sets the compass angle. Although compass is absolute [0,360) degrees, the
+ * continuous compass register holds the wrap-arounds.
+ *
+ * @param angleDeg  Degrees to set continuous compass angle to.
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetCompassAngle(double angleDeg, int timeoutMs) {
+	int errCode = c_PigeonIMU_SetCompassAngle(_handle, angleDeg, timeoutMs);
+	return errCode;
+}
+//----------------------- Calibration routines -----------------------//
+/**
+ * Enters the Calbration mode.  See the Pigeon IMU documentation for More
+ * information on Calibration.
+ *
+ * @param calMode  Calibration to execute
+ * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::EnterCalibrationMode(CalibrationMode calMode, int timeoutMs) {
+	return c_PigeonIMU_EnterCalibrationMode(_handle, calMode, timeoutMs);
+}
+/**
+ * Get the status of the current (or previousley complete) calibration.
+ *
+ * @param statusToFill Container for the status information.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::GetGeneralStatus(PigeonIMU::GeneralStatus & statusToFill) {
+	int state;
+	int currentMode;
+	int calibrationError;
+	int bCalIsBooting;
+	double tempC;
+	int upTimeSec;
+	int noMotionBiasCount;
+	int tempCompensationCount;
+	int lastError;
+
+	int errCode = c_PigeonIMU_GetGeneralStatus(_handle, &state, &currentMode,
+			&calibrationError, &bCalIsBooting, &tempC, &upTimeSec,
+			&noMotionBiasCount, &tempCompensationCount, &lastError);
+
+	statusToFill.currentMode = (PigeonIMU::CalibrationMode) currentMode;
+	statusToFill.calibrationError = calibrationError;
+	statusToFill.bCalIsBooting = bCalIsBooting;
+	statusToFill.state = (PigeonIMU::PigeonState) state;
+	statusToFill.tempC = tempC;
+	statusToFill.noMotionBiasCount = noMotionBiasCount;
+	statusToFill.tempCompensationCount = tempCompensationCount;
+	statusToFill.upTimeSec = upTimeSec;
+	statusToFill.lastError = errCode;
+
+	/* build description string */
+	if (errCode != 0) { // same as NoComm
+		statusToFill.description =
+				"Status frame was not received, check wired connections and web-based config.";
+	} else if (statusToFill.bCalIsBooting) {
+		statusToFill.description =
+				"Pigeon is boot-caling to properly bias accel and gyro.  Do not move Pigeon.  When finished biasing, calibration mode will start.";
+	} else if (statusToFill.state == UserCalibration) {
+		/* mode specific descriptions */
+		switch (currentMode) {
+		case BootTareGyroAccel:
+			statusToFill.description =
+					"Boot-Calibration: Gyro and Accelerometer are being biased.";
+			break;
+		case Temperature:
+			statusToFill.description =
+					"Temperature-Calibration: Pigeon is collecting temp data and will finish when temp range is reached.  "
+							"Do not move Pigeon.";
+			break;
+		case Magnetometer12Pt:
+			statusToFill.description =
+					"Magnetometer Level 1 calibration: Orient the Pigeon PCB in the 12 positions documented in the User's Manual.";
+			break;
+		case Magnetometer360:
+			statusToFill.description =
+					"Magnetometer Level 2 calibration: Spin robot slowly in 360' fashion.  ";
+			break;
+		case Accelerometer:
+			statusToFill.description =
+					"Accelerometer Calibration: Pigeon PCB must be placed on a level source.  Follow User's Guide for how to level surfacee.  ";
+			break;
+		}
+	} else if (statusToFill.state == Ready) {
+		/* definitely not doing anything cal-related.  So just instrument the motion driver state */
+		statusToFill.description =
+				"Pigeon is running normally.  Last CAL error code was ";
+		statusToFill.description += std::to_string(calibrationError);
+		statusToFill.description += ".";
+	} else if (statusToFill.state == Initializing) {
+		/* definitely not doing anything cal-related.  So just instrument the motion driver state */
+		statusToFill.description =
+				"Pigeon is boot-caling to properly bias accel and gyro.  Do not move Pigeon.";
+	} else {
+		statusToFill.description = "Not enough data to determine status.";
+	}
+
+	return errCode;
+}
+//----------------------- General Error status  -----------------------//
+/**
+ * Call GetLastError() generated by this object.
+ * Not all functions return an error code but can
+ * potentially report errors.
+ *
+ * This function can be used to retrieve those error codes.
+ *
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetLastError() {
+	return c_PigeonIMU_GetLastError(_handle);
+}
+
+
+//----------------------- Strongly typed Signal decoders  -----------------------//
+/**
+ * Get 6d Quaternion data.
+ *
+ * @param wxyz Array to fill with quaternion data w[0], x[1], y[2], z[3]
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::Get6dQuaternion(double wxyz[4]) {
+	int errCode = c_PigeonIMU_Get6dQuaternion(_handle, wxyz);
+	return errCode;
+}
+/**
+ * Get Yaw, Pitch, and Roll data.
+ *
+ * @param ypr Array to fill with yaw[0], pitch[1], and roll[2] data
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetYawPitchRoll(double ypr[3]) {
+	int errCode = c_PigeonIMU_GetYawPitchRoll(_handle, ypr);
+	return errCode;
+}
+/**
+ * Get AccumGyro data.
+ * AccumGyro is the integrated gyro value on each axis.
+ *
+ * @param xyz_deg Array to fill with x[0], y[1], and z[2] AccumGyro data
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetAccumGyro(double xyz_deg[3]) {
+	int errCode = c_PigeonIMU_GetAccumGyro(_handle, xyz_deg);
+	return errCode;
+}
+/**
+ * Get the absolute compass heading.
+ * @return compass heading [0,360) degrees.
+ */
+double PigeonIMU::GetAbsoluteCompassHeading() {
+	double retval;
+	c_PigeonIMU_GetAbsoluteCompassHeading(_handle, &retval);
+	return retval;
+}
+/**
+ * Get the continuous compass heading.
+ * @return continuous compass heading [-23040, 23040) degrees. Use
+ *         SetCompassHeading to modify the wrap-around portion.
+ */
+double PigeonIMU::GetCompassHeading() {
+	double retval;
+	c_PigeonIMU_GetCompassHeading(_handle, &retval);
+	return retval;
+}
+/**
+ * Gets the compass' measured magnetic field strength.
+ * @return field strength in Microteslas (uT).
+ */
+double PigeonIMU::GetCompassFieldStrength() {
+	double retval;
+	c_PigeonIMU_GetCompassFieldStrength(_handle, &retval);
+	return retval;
+}
+/**
+ * Gets the temperature of the pigeon.
+ *
+ * @return Temperature in ('C)
+ */
+double PigeonIMU::GetTemp() {
+	double tempC;
+	c_PigeonIMU_GetTemp(_handle, &tempC);
+	return tempC;
+}
+/**
+ * Gets the current Pigeon state
+ *
+ * @return PigeonState enum
+ */
+PigeonIMU::PigeonState PigeonIMU::GetState() {
+	int retval;
+	c_PigeonIMU_GetState(_handle, &retval);
+	return (PigeonIMU::PigeonState) retval;
+}
+/**
+ * Gets the current Pigeon uptime.
+ *
+ * @return How long has Pigeon been running in whole seconds. Value caps at
+ *         255.
+ */
+uint32_t PigeonIMU::GetUpTime() {
+	int timeSec;
+	c_PigeonIMU_GetUpTime(_handle, &timeSec);
+	return timeSec;
+}
+/**
+ * Get Raw Magnetometer data.
+ *
+ * @param rm_xyz Array to fill with x[0], y[1], and z[2] data
+	 * 				Number is equal to 0.6 microTeslas per unit.
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetRawMagnetometer(int16_t rm_xyz[3]) {
+	int errCode = c_PigeonIMU_GetRawMagnetometer(_handle, rm_xyz);
+	return errCode;
+}
+/**
+ * Get Biased Magnetometer data.
+ *
+ * @param bm_xyz Array to fill with x[0], y[1], and z[2] data
+	 * 				Number is equal to 0.6 microTeslas per unit.
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetBiasedMagnetometer(int16_t bm_xyz[3]) {
+	int errCode = c_PigeonIMU_GetBiasedMagnetometer(_handle, bm_xyz);
+	return errCode;
+}
+/**
+ * Get Biased Accelerometer data.
+ *
+ * @param ba_xyz Array to fill with x[0], y[1], and z[2] data
+	 * 			These are in fixed point notation Q2.14.  eg. 16384 = 1G
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetBiasedAccelerometer(int16_t ba_xyz[3]) {
+	int errCode = c_PigeonIMU_GetBiasedAccelerometer(_handle, ba_xyz);
+	return errCode;
+}
+/**
+ * Get Raw Gyro data.
+ *
+ * @param xyz_dps Array to fill with x[0], y[1], and z[2] data in degrees per second.
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetRawGyro(double xyz_dps[3]) {
+	int errCode = c_PigeonIMU_GetRawGyro(_handle, xyz_dps);
+	return errCode;
+}
+/**
+ * Get Accelerometer tilt angles.
+ *
+ * @param tiltAngles Array to fill with x[0], y[1], and z[2] angles in degrees.
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetAccelerometerAngles(double tiltAngles[3]) {
+	int errCode = c_PigeonIMU_GetAccelerometerAngles(_handle, tiltAngles);
+	return errCode;
+}
+/**
+ * Get the current Fusion Status (including fused heading)
+ *
+ * @param status 	object reference to fill with fusion status flags.
+ *					Caller may pass null if flags are not needed.
+ * @return The fused heading in degrees.
+ */
+double PigeonIMU::GetFusedHeading(FusionStatus & status) {
+	int bIsFusing, bIsValid;
+	double fusedHeading;
+	int lastError;
+
+	int errCode = c_PigeonIMU_GetFusedHeading2(_handle, &bIsFusing, &bIsValid,
+			&fusedHeading, &lastError);
+
+	std::string description;
+
+	if (errCode != 0) {
+		bIsFusing = false;
+		bIsValid = false;
+		description =
+				"Could not receive status frame.  Check wiring and web-config.";
+	} else {
+		if (bIsValid == false) {
+			description = "Fused Heading is not valid.";
+		} else if (bIsFusing == false) {
+			description = "Fused Heading is valid.";
+		} else {
+			description = "Fused Heading is valid and is fusing compass.";
+		}
+	}
+
+	/* fill caller's struct */
+	status.heading = fusedHeading;
+	status.bIsFusing = (bool) bIsFusing;
+	status.bIsValid = (bool) bIsValid;
+	status.description = description;
+	status.lastError = errCode;
+
+	return fusedHeading;
+}
+/**
+ * Gets the Fused Heading
+ *
+ * @return The fused heading in degrees.
+ */
+double PigeonIMU::GetFusedHeading() {
+	double fusedHeading;
+	c_PigeonIMU_GetFusedHeading1(_handle, &fusedHeading);
+	return fusedHeading;
+}
+//----------------------- Startup/Reset status -----------------------//
+/**
+ * Use HasResetOccurred() instead.
+ */
+uint32_t PigeonIMU::GetResetCount() {
+	int retval;
+	c_PigeonIMU_GetResetCount(_handle, &retval);
+	return retval;
+}
+uint32_t PigeonIMU::GetResetFlags() {
+	int retval;
+	c_PigeonIMU_GetResetCount(_handle, &retval);
+	return (uint32_t) retval;
+}
+/**
+ * @return holds the version of the device.  Device must be powered cycled at least once.
+ */
+uint32_t PigeonIMU::GetFirmVers() {
+	int retval;
+	c_PigeonIMU_GetFirmwareVersion(_handle, &retval);
+	return retval;
+}
+/**
+ * @return true iff a reset has occurred since last call.
+ */
+bool PigeonIMU::HasResetOccurred() {
+	bool retval = false;
+	c_PigeonIMU_HasResetOccurred(_handle, &retval);
+	return retval;
+}
+
+/**
+ * Convert PigeonState to string.
+ * This function is static.
+ */
+std::string PigeonIMU::ToString(PigeonState state) {
+	std::string retval = "Unknown";
+	switch (state) {
+	case Initializing:
+		return "Initializing";
+	case Ready:
+		return "Ready";
+	case UserCalibration:
+		return "UserCalibration";
+	case NoComm:
+		return "NoComm";
+	}
+	return retval;
+}
+
+/**
+ * Convert CalibrationMode to string.
+ * This function is static.
+ */
+std::string PigeonIMU::ToString(CalibrationMode cm) {
+	std::string retval = "Unknown";
+	switch (cm) {
+	case BootTareGyroAccel:
+		return "BootTareGyroAccel";
+	case Temperature:
+		return "Temperature";
+	case Magnetometer12Pt:
+		return "Magnetometer12Pt";
+	case Magnetometer360:
+		return "Magnetometer360";
+	case Accelerometer:
+		return "Accelerometer";
+	}
+	return retval;
+}
+/**
+ * Sets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/declination/offset
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param newValue
+ *            Value for custom parameter.
+ * @param paramIndex
+ *            Index of custom parameter. [0-1]
+ * @param timeoutMs
+*            Timeout value in ms. If nonzero, function will wait for
+*            config success and report an error if it times out.
+*            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::ConfigSetCustomParam(int newValue, int paramIndex,
+		int timeoutMs) {
+	return c_PigeonIMU_ConfigSetCustomParam(_handle, newValue, paramIndex,
+			timeoutMs);
+}
+/**
+ * Gets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/declination/offset
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param paramIndex
+ *            Index of custom parameter. [0-1]
+ * @param timeoutMs
+*            Timeout value in ms. If nonzero, function will wait for
+*            config success and report an error if it times out.
+*            If zero, no blocking or checking is performed.
+ * @return Value of the custom param.
+ */
+int PigeonIMU::ConfigGetCustomParam(int paramIndex, int timeoutMs) {
+	int readValue;
+	c_PigeonIMU_ConfigGetCustomParam(_handle, &readValue, paramIndex,
+			timeoutMs);
+	return readValue;
+}
+/**
+ * Sets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ *            Parameter enumeration.
+ * @param value
+ *            Value of parameter.
+ * @param subValue
+ *            Subvalue for parameter. Maximum value of 255.
+ * @param ordinal
+ *            Ordinal of parameter.
+ * @param timeoutMs
+*            Timeout value in ms. If nonzero, function will wait for
+*            config success and report an error if it times out.
+*            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::ConfigSetParameter(ctre::phoenix::ParamEnum param, double value,
+		uint8_t subValue, int ordinal, int timeoutMs) {
+	return c_PigeonIMU_ConfigSetParameter(_handle, param, value, subValue,
+			ordinal, timeoutMs);
+
+}
+/**
+ * Gets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ *            Parameter enumeration.
+ * @param ordinal
+ *            Ordinal of parameter.
+ * @param timeoutMs
+*            Timeout value in ms. If nonzero, function will wait for
+*            config success and report an error if it times out.
+*            If zero, no blocking or checking is performed.
+ * @return Value of parameter.
+ */
+double PigeonIMU::ConfigGetParameter(ctre::phoenix::ParamEnum param, int ordinal,
+		int timeoutMs) {
+	double value = 0;
+	c_PigeonIMU_ConfigGetParameter(_handle, param, &value, ordinal, timeoutMs);
+	return value;
+}
+
+
+//------ Frames ----------//
+/**
+ * Sets the period of the given status frame.
+ *
+ * @param statusFrame
+ *            Frame whose period is to be changed.
+ * @param periodMs
+ *            Period in ms for the given frame.
+ * @param timeoutMs
+*            Timeout value in ms. If nonzero, function will wait for
+*            config success and report an error if it times out.
+*            If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::SetStatusFramePeriod(PigeonIMU_StatusFrame statusFrame,
+		int periodMs, int timeoutMs) {
+	return c_PigeonIMU_SetStatusFramePeriod(_handle, statusFrame, periodMs,
+			timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ *            Frame to get the period of.
+ * @param timeoutMs
+*            Timeout value in ms. If nonzero, function will wait for
+*            config success and report an error if it times out.
+*            If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int PigeonIMU::GetStatusFramePeriod(PigeonIMU_StatusFrame frame,
+		int timeoutMs) {
+	int periodMs = 0;
+	c_PigeonIMU_GetStatusFramePeriod(_handle, frame, &periodMs, timeoutMs);
+	return periodMs;
+}
+/**
+ * Sets the period of the given control frame.
+ *
+ * @param frame
+ *            Frame whose period is to be changed.
+ * @param periodMs
+ *            Period in ms for the given frame.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::SetControlFramePeriod(PigeonIMU_ControlFrame frame,
+		int periodMs) {
+	return c_PigeonIMU_SetControlFramePeriod(_handle, frame, periodMs);
+}
+//------ Firmware ----------//
+/**
+ * Gets the firmware version of the device.
+ *
+ * @return The firmware version of the device. Device must be powered
+ * cycled at least once.
+ */
+int PigeonIMU::GetFirmwareVersion() {
+	int retval = -1;
+	c_PigeonIMU_GetFirmwareVersion(_handle, &retval);
+	return retval;
+}
+//------ Faults ----------//
+/**
+ * Gets the fault status
+ *
+ * @param toFill
+ *            Container for fault statuses.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::GetFaults(PigeonIMU_Faults & toFill) {
+	int faultBits;
+	ErrorCode retval = c_PigeonIMU_GetFaults(_handle, &faultBits);
+	toFill = PigeonIMU_Faults(faultBits);
+	return retval;
+}
+/**
+ * Gets the sticky fault status
+ *
+ * @param toFill
+ *            Container for sticky fault statuses.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::GetStickyFaults(PigeonIMU_StickyFaults & toFill) {
+	int faultBits;
+	ErrorCode retval = c_PigeonIMU_GetFaults(_handle, &faultBits);
+	toFill = PigeonIMU_StickyFaults(faultBits);
+	return retval;
+}
+/**
+ * Clears the Sticky Faults
+ *
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::ClearStickyFaults(int timeoutMs) {
+	return c_PigeonIMU_ClearStickyFaults(_handle, timeoutMs);
+}
+
+} // namespace signals
+} // namespace phoenix
+} // namespace ctre
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/src/Stopwatch.cpp b/cpp/src/Stopwatch.cpp
new file mode 100644
index 0000000..522965b
--- /dev/null
+++ b/cpp/src/Stopwatch.cpp
@@ -0,0 +1,20 @@
+#include "ctre/phoenix/Stopwatch.h"
+
+namespace ctre {
+namespace phoenix {
+
+void Stopwatch::Start(){
+	_t0 = (float)clock();
+}
+unsigned int Stopwatch::DurationMs(){
+	return Stopwatch::Duration() * 1000;
+}
+float Stopwatch::Duration(){
+	_t1 = (float)clock();
+	long retval = _t1 - _t0;
+	if(retval < 0) retval = 0;
+	return retval * _scalar;
+}
+
+} // namespace phoenix
+}
diff --git a/cpp/src/Tasking/ButtonMonitor.cpp b/cpp/src/Tasking/ButtonMonitor.cpp
new file mode 100644
index 0000000..d420e34
--- /dev/null
+++ b/cpp/src/Tasking/ButtonMonitor.cpp
@@ -0,0 +1,46 @@
+#include "ctre/phoenix/Tasking/ButtonMonitor.h"
+#include <GenericHID.h> // WPILIB
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+
+ButtonMonitor::ButtonMonitor(frc::GenericHID * controller, int buttonIndex,
+		IButtonPressEventHandler * ButtonPressEventHandler) {
+	_gameCntrlr = controller;
+	_btnIdx = buttonIndex;
+	_handler = ButtonPressEventHandler;
+}
+ButtonMonitor::ButtonMonitor(const ButtonMonitor & rhs) {
+	_gameCntrlr = rhs._gameCntrlr;
+	_btnIdx = rhs._btnIdx;
+	_handler = rhs._handler;
+}
+
+void ButtonMonitor::Process() {
+	bool down = ((frc::GenericHID*)_gameCntrlr)->GetRawButton(_btnIdx);
+
+	if (!_isDown && down)
+		_handler->OnButtonPress(_btnIdx, down);
+
+	_isDown = down;
+}
+
+void ButtonMonitor::OnStart() {
+}
+void ButtonMonitor::OnLoop() {
+	Process();
+}
+bool ButtonMonitor::IsDone() {
+	return false;
+}
+void ButtonMonitor::OnStop() {
+}
+
+} // namespace tasking
+} // namespace phoenix
+} // namespace ctre
+
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/src/Tasking/Schedulers/ConcurrentScheduler.cpp b/cpp/src/Tasking/Schedulers/ConcurrentScheduler.cpp
new file mode 100644
index 0000000..c8d6773
--- /dev/null
+++ b/cpp/src/Tasking/Schedulers/ConcurrentScheduler.cpp
@@ -0,0 +1,88 @@
+#include "ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h"
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+namespace schedulers {
+
+ConcurrentScheduler::ConcurrentScheduler() {
+}
+ConcurrentScheduler::~ConcurrentScheduler() {
+}
+void ConcurrentScheduler::Add(ILoopable *aLoop, bool enable) {
+	_loops.push_back(aLoop);
+	_enabs.push_back(enable);
+}
+void ConcurrentScheduler::RemoveAll() {
+	_loops.clear();
+	_enabs.clear();
+}
+void ConcurrentScheduler::Start(ILoopable* toStart) {
+	for (int i = 0; i < (int) _loops.size(); ++i) {
+		ILoopable* lp = (ILoopable*) _loops[i];
+
+		if (lp == toStart) {
+			_enabs[i] = true;
+			lp->OnStart();
+			return;
+		}
+	}
+
+}
+void ConcurrentScheduler::Stop(ILoopable* toStop) {
+	for (int i = 0; i < (int) _loops.size(); ++i) {
+		ILoopable* lp = (ILoopable*) _loops[i];
+
+		if (lp == toStop) {
+			_enabs[i] = false;
+			lp->OnStop();
+			return;
+		}
+	}
+}
+void ConcurrentScheduler::StartAll() {	//All Loops
+	for (auto loop : _loops) {
+		loop->OnStart();
+	}
+	for (auto enable : _enabs) {
+		enable = true;
+	}
+}
+void ConcurrentScheduler::StopAll() {	//All Loops
+	for (auto loop : _loops) {
+		loop->OnStop();
+	}
+	for (auto enable : _enabs) {
+		enable = false;
+	}
+}
+void ConcurrentScheduler::Process() {
+	for (int i = 0; i < (int) _loops.size(); ++i) {
+		ILoopable* loop = (ILoopable*) _loops[i];
+		bool en = (bool) _enabs[i];
+		if (en) {
+			loop->OnLoop();
+		} else {
+			/* Current ILoopable is turned off, don't call OnLoop for it */
+		}
+	}
+}
+/* ILoopable */
+void ConcurrentScheduler::OnStart() {
+	ConcurrentScheduler::StartAll();
+}
+void ConcurrentScheduler::OnLoop() {
+	ConcurrentScheduler::Process();
+}
+void ConcurrentScheduler::OnStop() {
+	ConcurrentScheduler::StopAll();
+}
+bool ConcurrentScheduler::IsDone() {
+	return false;
+}
+
+} // namespace schedulers
+} // namespace tasking
+} // namespace phoenix
+} // namespace ctre
+
diff --git a/cpp/src/Tasking/Schedulers/SequentialScheduler.cpp b/cpp/src/Tasking/Schedulers/SequentialScheduler.cpp
new file mode 100644
index 0000000..edac7bd
--- /dev/null
+++ b/cpp/src/Tasking/Schedulers/SequentialScheduler.cpp
@@ -0,0 +1,86 @@
+#include "ctre/phoenix/Tasking/Schedulers/SequentialScheduler.h"
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+namespace schedulers {
+
+SequentialScheduler::SequentialScheduler() {
+}
+SequentialScheduler::~SequentialScheduler() {
+}
+void SequentialScheduler::Add(ILoopable *aLoop) {
+	_loops.push_back(aLoop);
+}
+ILoopable * SequentialScheduler::GetCurrent() {
+	ILoopable* retval = nullptr;
+	if (_idx < _loops.size()) {
+		retval = _loops[_idx];
+	}
+	return retval;
+}
+
+void SequentialScheduler::RemoveAll() {
+	_loops.clear();
+}
+void SequentialScheduler::Start() {
+	/* reset iterator regardless of loopable container */
+	_idx = 0;
+	/* do we have any to loop? */
+	if (_idx >= _loops.size()) {
+		/* there are no loopables */
+		_running = false;
+	} else {
+		/* start the first one */
+		_loops[_idx]->OnStart();
+		_running = true;
+	}
+
+}
+void SequentialScheduler::Stop() {
+	for (unsigned int i = 0; i < _loops.size(); i++) {
+		_loops[i]->OnStop();
+	}
+	_running = false;
+}
+void SequentialScheduler::Process() {
+	if (_idx < _loops.size()) {
+		if (_running) {
+			ILoopable* loop = _loops[_idx];
+			loop->OnLoop();
+			if (loop->IsDone()) {
+				/* iterate to next loopable */
+				++_idx;
+				if (_idx < _loops.size()) {
+					/* callback to start it */
+					_loops[_idx]->OnStart();
+				}
+			}
+		}
+	} else {
+		_running = false;
+	}
+}
+/* ILoopable */
+void SequentialScheduler::OnStart() {
+	SequentialScheduler::Start();
+}
+void SequentialScheduler::OnLoop() {
+	SequentialScheduler::Process();
+}
+void SequentialScheduler::OnStop() {
+	SequentialScheduler::Stop();
+}
+bool SequentialScheduler::IsDone() {
+	/* Have to return something to know if we are done */
+	if (_running == false)
+		return true;
+	else
+		return false;
+}
+
+} // namespace schedulers
+} // namespace tasking
+} // namespace phoenix
+} // namespace ctre
+
diff --git a/cpp/src/Utilities.cpp b/cpp/src/Utilities.cpp
new file mode 100644
index 0000000..5c749b2
--- /dev/null
+++ b/cpp/src/Utilities.cpp
@@ -0,0 +1,69 @@
+#include "ctre/phoenix/Utilities.h"
+
+namespace ctre {
+namespace phoenix {
+
+float Utilities::abs(float f) {
+	if (f >= 0)
+		return f;
+	return -f;
+}
+
+float Utilities::bound(float value, float capValue) {
+	if (value > capValue)
+		return capValue;
+	if (value < -capValue)
+		return -capValue;
+	return value;
+}
+
+float Utilities::cap(float value, float peak) {
+	if (value < -peak)
+		return -peak;
+	if (value > +peak)
+		return +peak;
+	return value;
+}
+
+bool Utilities::Contains(char array[], char item) {
+	//Not sure how to implement in c++ yet, made private
+	(void)array;
+	(void)item;
+	return false;
+}
+
+void Utilities::Deadband(float &value, float deadband) {
+	if (value < -deadband) {
+		/* outside of deadband */
+	} else if (value > +deadband) {
+		/* outside of deadband */
+	} else {
+		/* within 10% so zero it */
+		value = 0;
+	}
+}
+
+bool Utilities::IsWithin(float value, float compareTo, float allowDelta) {
+	float f = value - compareTo;
+	if (f < 0)
+		f *= -1;
+	return (f < allowDelta);
+}
+
+int Utilities::SmallerOf(int value_1, int value_2) {
+	if (value_1 > value_2)
+		return value_2;
+	else
+		return value_1;
+}
+
+void Utilities::Split_1(float forward, float turn, float *left, float *right) {
+	*left = forward + turn;
+	*right = forward - turn;
+}
+void Utilities::Split_2(float left, float right, float *forward, float *turn) {
+	*forward = (left + right) * 0.5f;
+	*turn = (left - right) * 0.5f;
+}
+}
+}
diff --git a/ctrlib.doxy b/ctrlib.doxy
new file mode 100644
index 0000000..f65a9ff
--- /dev/null
+++ b/ctrlib.doxy
@@ -0,0 +1,2492 @@
+# Doxyfile 1.8.13
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING      = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME           = CTRE_Phoenix
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER         = "2018_v14"
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF          = 
+
+# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
+# in the documentation. The maximum height of the logo should not exceed 55
+# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
+# the logo to the output directory.
+
+PROJECT_LOGO           = 
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY       = "release/cpp/docs/CTRE_Phoenix"
+
+# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS         = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES    = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE        = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC      = YES
+
+# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF           = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF       = "The $name class" \
+                         "The $name widget" \
+                         "The $name file" \
+                         is \
+                         provides \
+                         specifies \
+                         contains \
+                         represents \
+                         a \
+                         an \
+                         the
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC    = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB  = NO
+
+# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES        = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH        = 
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH    = 
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES            = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF      = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF           = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS           = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
+# page for each member. If set to NO, the documentation of a member will be part
+# of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES  = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE               = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES                = 
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST              = 
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C  = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA   = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN   = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL   = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note: For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING      = 
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT       = YES
+
+# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up
+# to that level are automatically included in the table of contents, even if
+# they do not have an id attribute.
+# Note: This feature currently applies only to Markdown headings.
+# Minimum value: 0, maximum value: 99, default value: 0.
+# This tag requires that the tag MARKDOWN_SUPPORT is set to YES.
+
+TOC_INCLUDE_HEADINGS   = 0
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by putting a % sign in front of the word or
+# globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT       = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT    = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT        = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT            = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT   = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC   = NO
+
+# If one adds a struct or class to a group and this option is enabled, then also
+# any nested class or struct is added to the same group. By default this option
+# is disabled and one has to add nested compounds explicitly via \ingroup.
+# The default value is: NO.
+
+GROUP_NESTED_COMPOUNDS = NO
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING            = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS  = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT   = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE      = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL            = YES
+
+# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE        = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE        = NO
+
+# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC         = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO,
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES  = YES
+
+# This flag is only useful for Objective-C code. If set to YES, local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO, only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS  = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES   = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS     = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO, these classes will be included in the various overviews. This option
+# has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES     = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO, these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS  = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO, these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS      = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS          = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES, upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES       = NO
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES, the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES       = NO
+
+# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
+# append additional text to a page's title, such as Class Reference. If set to
+# YES the compound reference will be hidden.
+# The default value is: NO.
+
+HIDE_COMPOUND_REFERENCE= NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES     = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC  = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES   = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO            = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS       = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS        = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES       = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME     = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING  = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
+# list. This list is created by putting \todo commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST      = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
+# list. This list is created by putting \test commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST      = YES
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST       = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS       = 
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES  = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES, the
+# list will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES        = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES             = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES        = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER    = 
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE            = 
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. See also \cite for info how to create references.
+
+CITE_BIB_FILES         = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET                  = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS               = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED   = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR      = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO, doxygen will only warn about wrong or incomplete
+# parameter documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC       = NO
+
+# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
+# a warning is encountered.
+# The default value is: NO.
+
+WARN_AS_ERROR          = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT            = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE           = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
+# Note: If this tag is empty the current directory is searched.
+
+INPUT                  = cpp libraries/driver
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING         = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# read by doxygen.
+#
+# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
+# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
+# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
+# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f95, *.f03, *.f08,
+# *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf and *.qsf.
+
+FILE_PATTERNS          = *.c \
+                         *.cc \
+                         *.cxx \
+                         *.cpp \
+                         *.c++ \
+                         *.java \
+                         *.ii \
+                         *.ixx \
+                         *.ipp \
+                         *.i++ \
+                         *.inl \
+                         *.idl \
+                         *.ddl \
+                         *.odl \
+                         *.h \
+                         *.hh \
+                         *.hxx \
+                         *.hpp \
+                         *.h++ \
+                         *.cs \
+                         *.d \
+                         *.php \
+                         *.php4 \
+                         *.php5 \
+                         *.phtml \
+                         *.inc \
+                         *.m \
+                         *.markdown \
+                         *.md \
+                         *.mm \
+                         *.dox \
+                         *.py \
+                         *.pyw \
+                         *.f90 \
+                         *.f95 \
+                         *.f03 \
+                         *.f08 \
+                         *.f \
+                         *.for \
+                         *.tcl \
+                         *.vhd \
+                         *.vhdl \
+                         *.ucf \
+                         *.qsf
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE              = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE                = 
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS       = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS       = 
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS        = 
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH           = 
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS       = *
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE      = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH             = 
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+INPUT_FILTER           = 
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+FILTER_PATTERNS        = 
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES    = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS = 
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER         = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES         = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS    = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION    = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS        = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS              = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS       = YES
+
+# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
+# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
+# cost of reduced performance. This can be particularly helpful with template
+# rich C++ code for which doxygen's built-in parser lacks the necessary type
+# information.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse-libclang=ON option for CMake.
+# The default value is: NO.
+
+CLANG_ASSISTED_PARSING = NO
+
+# If clang assisted parsing is enabled you can provide the compiler with command
+# line options that you would normally use when invoking the compiler. Note that
+# the include paths will already be set by doxygen for the files and directories
+# specified with INPUT and INCLUDE_PATH.
+# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
+
+CLANG_OPTIONS          = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX     = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX    = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX          = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML          = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT            = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION    = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER            = 
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER            = 
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET        = 
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# cascading style sheets that are included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefore more robust against future updates.
+# Doxygen will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list). For an example see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET  = 
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES       = 
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the style sheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE    = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT    = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA  = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to YES can help to show when doxygen was last run and thus if the
+# documentation is up to date.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP         = NO
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS  = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET        = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME        = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID       = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME  = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP      = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE               = 
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler (hhc.exe). If non-empty,
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION           = 
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated
+# (YES) or that it should be included in the master .chm file (NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI           = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING     = 
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated
+# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC             = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND             = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP           = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE               = 
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE          = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER     = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME   = 
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS  = 
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS  = 
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION           = 
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP   = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID         = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX          = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW      = YES
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE   = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH         = 250
+
+# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW    = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE       = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT    = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX            = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT         = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS     = 
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE       = 
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE           = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH    = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH        = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL       = 
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE        = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID     = 
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS  = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX         = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT           = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME         = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME     = makeindex
+
+# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX          = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE             = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. The package can be specified just
+# by its name or with the correct syntax as to be used with the LaTeX
+# \usepackage command. To get the times font for instance you can specify :
+# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
+# To use the option intlimits with the amsmath package you can specify:
+# EXTRA_PACKAGES=[intlimits]{amsmath}
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES         = 
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
+# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
+# string, for the replacement values of the other commands the user is referred
+# to HTML_HEADER.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER           = 
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer. See
+# LATEX_HEADER for more information on how to generate a default footer and what
+# special commands can be used inside the footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER           = 
+
+# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# LaTeX style sheets that are included after the standard style sheets created
+# by doxygen. Using this option one can overrule certain style aspects. Doxygen
+# will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_STYLESHEET = 
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES      = 
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS         = YES
+
+# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES, to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX           = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE        = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES     = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE      = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE        = plain
+
+# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_TIMESTAMP        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF           = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT             = rtf
+
+# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF            = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS         = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE    = 
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE    = 
+
+# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
+# with syntax highlighting in the RTF output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_SOURCE_CODE        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN           = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT             = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION          = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR             = 
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS              = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML           = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT             = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING     = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK       = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT         = docbook
+
+# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
+# program listings (including syntax highlighting and cross-referencing
+# information) to the DOCBOOK output. Note that enabling this will significantly
+# increase the size of the DOCBOOK output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_PROGRAMLISTING = NO
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
+# AutoGen Definitions (see http://autogen.sf.net) file that captures the
+# structure of the code including all documentation. Note that this feature is
+# still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF   = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD       = NO
+
+# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX          = NO
+
+# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO, the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY         = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING   = YES
+
+# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
+# in the source code. If set to NO, only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION        = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF     = NO
+
+# If the SEARCH_INCLUDES tag is set to YES, the include files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES        = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH           = 
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS  = 
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED             = 
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED      = 
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS   = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES               = 
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE       = 
+
+# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
+# the class index. If set to NO, only the inherited external classes will be
+# listed.
+# The default value is: NO.
+
+ALLEXTERNALS           = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
+# in the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS        = YES
+
+# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES         = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH              = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS         = NO
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH            = 
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH               = 
+
+# If set to YES the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS   = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: NO.
+
+HAVE_DOT               = NO
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS        = 0
+
+# When you want a differently looking font in the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME           = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE           = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH           = 
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH            = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH    = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS           = YES
+
+# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK               = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS   = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS     = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH          = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH      = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command. Disabling a call graph can be
+# accomplished by means of the command \hidecallgraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH             = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command. Disabling a caller graph can be
+# accomplished by means of the command \hidecallergraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH           = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY    = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH        = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot. For an explanation of the image formats see the section
+# output formats in the documentation of the dot tool (Graphviz (see:
+# http://www.graphviz.org/)).
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, jpg, gif, svg, png:gd, png:gd:gd, png:cairo,
+# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
+# png:gdiplus:gdiplus.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT       = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG        = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH               = 
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS           = 
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS           = 
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS           = 
+
+# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
+# path where java can find the plantuml.jar file. If left blank, it is assumed
+# PlantUML is not used or called during a preprocessing step. Doxygen will
+# generate a warning when it encounters a \startuml command in this case and
+# will not generate output for the diagram.
+
+PLANTUML_JAR_PATH      = 
+
+# When using plantuml, the PLANTUML_CFG_FILE tag can be used to specify a
+# configuration file for plantuml.
+
+PLANTUML_CFG_FILE      = 
+
+# When using plantuml, the specified paths are searched for files specified by
+# the !include statement in a plantuml block.
+
+PLANTUML_INCLUDE_PATH  = 
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES    = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH    = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT        = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS      = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND        = YES
+
+# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP            = YES
\ No newline at end of file
diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 0000000..512242e
--- /dev/null
+++ b/gradle/wrapper/gradle-wrapper.jar
Binary files differ
diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
new file mode 100644
index 0000000..b22a07f
--- /dev/null
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,6 @@
+#Tue Jul 25 08:32:31 EDT 2017
+distributionBase=GRADLE_USER_HOME
+distributionPath=wrapper/dists
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=wrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-3.3-bin.zip
diff --git a/gradlew b/gradlew
new file mode 100644
index 0000000..4453cce
--- /dev/null
+++ b/gradlew
@@ -0,0 +1,172 @@
+#!/usr/bin/env sh
+
+##############################################################################
+##
+##  Gradle start up script for UN*X
+##
+##############################################################################
+
+# Attempt to set APP_HOME
+# Resolve links: $0 may be a link
+PRG="$0"
+# Need this for relative symlinks.
+while [ -h "$PRG" ] ; do
+    ls=`ls -ld "$PRG"`
+    link=`expr "$ls" : '.*-> \(.*\)$'`
+    if expr "$link" : '/.*' > /dev/null; then
+        PRG="$link"
+    else
+        PRG=`dirname "$PRG"`"/$link"
+    fi
+done
+SAVED="`pwd`"
+cd "`dirname \"$PRG\"`/" >/dev/null
+APP_HOME="`pwd -P`"
+cd "$SAVED" >/dev/null
+
+APP_NAME="Gradle"
+APP_BASE_NAME=`basename "$0"`
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS=""
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD="maximum"
+
+warn ( ) {
+    echo "$*"
+}
+
+die ( ) {
+    echo
+    echo "$*"
+    echo
+    exit 1
+}
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "`uname`" in
+  CYGWIN* )
+    cygwin=true
+    ;;
+  Darwin* )
+    darwin=true
+    ;;
+  MINGW* )
+    msys=true
+    ;;
+  NONSTOP* )
+    nonstop=true
+    ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD="$JAVA_HOME/jre/sh/java"
+    else
+        JAVACMD="$JAVA_HOME/bin/java"
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD="java"
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then
+    MAX_FD_LIMIT=`ulimit -H -n`
+    if [ $? -eq 0 ] ; then
+        if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
+            MAX_FD="$MAX_FD_LIMIT"
+        fi
+        ulimit -n $MAX_FD
+        if [ $? -ne 0 ] ; then
+            warn "Could not set maximum file descriptor limit: $MAX_FD"
+        fi
+    else
+        warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
+    fi
+fi
+
+# For Darwin, add options to specify how the application appears in the dock
+if $darwin; then
+    GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
+fi
+
+# For Cygwin, switch paths to Windows format before running java
+if $cygwin ; then
+    APP_HOME=`cygpath --path --mixed "$APP_HOME"`
+    CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
+    JAVACMD=`cygpath --unix "$JAVACMD"`
+
+    # We build the pattern for arguments to be converted via cygpath
+    ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
+    SEP=""
+    for dir in $ROOTDIRSRAW ; do
+        ROOTDIRS="$ROOTDIRS$SEP$dir"
+        SEP="|"
+    done
+    OURCYGPATTERN="(^($ROOTDIRS))"
+    # Add a user-defined pattern to the cygpath arguments
+    if [ "$GRADLE_CYGPATTERN" != "" ] ; then
+        OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
+    fi
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    i=0
+    for arg in "$@" ; do
+        CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
+        CHECK2=`echo "$arg"|egrep -c "^-"`                                 ### Determine if an option
+
+        if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then                    ### Added a condition
+            eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
+        else
+            eval `echo args$i`="\"$arg\""
+        fi
+        i=$((i+1))
+    done
+    case $i in
+        (0) set -- ;;
+        (1) set -- "$args0" ;;
+        (2) set -- "$args0" "$args1" ;;
+        (3) set -- "$args0" "$args1" "$args2" ;;
+        (4) set -- "$args0" "$args1" "$args2" "$args3" ;;
+        (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
+        (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
+        (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
+        (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
+        (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
+    esac
+fi
+
+# Escape application args
+save ( ) {
+    for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
+    echo " "
+}
+APP_ARGS=$(save "$@")
+
+# Collect all arguments for the java command, following the shell quoting and substitution rules
+eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
+
+# by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong
+if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then
+  cd "$(dirname "$0")"
+fi
+
+exec "$JAVACMD" "$@"
diff --git a/gradlew.bat b/gradlew.bat
new file mode 100644
index 0000000..f955316
--- /dev/null
+++ b/gradlew.bat
@@ -0,0 +1,84 @@
+@if "%DEBUG%" == "" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%" == "" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS=
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if "%ERRORLEVEL%" == "0" goto init
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto init
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:init
+@rem Get command-line arguments, handling Windows variants
+
+if not "%OS%" == "Windows_NT" goto win9xME_args
+
+:win9xME_args
+@rem Slurp the command line arguments.
+set CMD_LINE_ARGS=
+set _SKIP=2
+
+:win9xME_args_slurp
+if "x%~1" == "x" goto execute
+
+set CMD_LINE_ARGS=%*
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
+
+:end
+@rem End local scope for the variables with windows NT shell
+if "%ERRORLEVEL%"=="0" goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+exit /b 1
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/java/.classpath b/java/.classpath
new file mode 100644
index 0000000..94c6baa
--- /dev/null
+++ b/java/.classpath
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="var" path="wpilib" sourcepath="wpilib.sources"/>
+	<classpathentry kind="var" path="networktables" sourcepath="networktables.sources"/>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.8"/>
+	<classpathentry kind="var" path="opencv" sourcepath="opencv.sources"/>
+	<classpathentry kind="var" path="cscore" sourcepath="cscore.sources"/>
+	<classpathentry kind="output" path="bin"/>
+</classpath>
diff --git a/java/.project b/java/.project
new file mode 100644
index 0000000..bdfcd7e
--- /dev/null
+++ b/java/.project
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>CTRE_PhoenixJAR</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>
diff --git a/java/arm-linux/LICENSE b/java/arm-linux/LICENSE
new file mode 100644
index 0000000..b40a0f4
--- /dev/null
+++ b/java/arm-linux/LICENSE
@@ -0,0 +1,347 @@
+The GNU General Public License (GPL)
+
+Version 2, June 1991
+
+Copyright (C) 1989, 1991 Free Software Foundation, Inc.
+59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+
+Everyone is permitted to copy and distribute verbatim copies of this license
+document, but changing it is not allowed.
+
+Preamble
+
+The licenses for most software are designed to take away your freedom to share
+and change it.  By contrast, the GNU General Public License is intended to
+guarantee your freedom to share and change free software--to make sure the
+software is free for all its users.  This General Public License applies to
+most of the Free Software Foundation's software and to any other program whose
+authors commit to using it.  (Some other Free Software Foundation software is
+covered by the GNU Library General Public License instead.) You can apply it to
+your programs, too.
+
+When we speak of free software, we are referring to freedom, not price.  Our
+General Public Licenses are designed to make sure that you have the freedom to
+distribute copies of free software (and charge for this service if you wish),
+that you receive source code or can get it if you want it, that you can change
+the software or use pieces of it in new free programs; and that you know you
+can do these things.
+
+To protect your rights, we need to make restrictions that forbid anyone to deny
+you these rights or to ask you to surrender the rights.  These restrictions
+translate to certain responsibilities for you if you distribute copies of the
+software, or if you modify it.
+
+For example, if you distribute copies of such a program, whether gratis or for
+a fee, you must give the recipients all the rights that you have.  You must
+make sure that they, too, receive or can get the source code.  And you must
+show them these terms so they know their rights.
+
+We protect your rights with two steps: (1) copyright the software, and (2)
+offer you this license which gives you legal permission to copy, distribute
+and/or modify the software.
+
+Also, for each author's protection and ours, we want to make certain that
+everyone understands that there is no warranty for this free software.  If the
+software is modified by someone else and passed on, we want its recipients to
+know that what they have is not the original, so that any problems introduced
+by others will not reflect on the original authors' reputations.
+
+Finally, any free program is threatened constantly by software patents.  We
+wish to avoid the danger that redistributors of a free program will
+individually obtain patent licenses, in effect making the program proprietary.
+To prevent this, we have made it clear that any patent must be licensed for
+everyone's free use or not licensed at all.
+
+The precise terms and conditions for copying, distribution and modification
+follow.
+
+TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+0. This License applies to any program or other work which contains a notice
+placed by the copyright holder saying it may be distributed under the terms of
+this General Public License.  The "Program", below, refers to any such program
+or work, and a "work based on the Program" means either the Program or any
+derivative work under copyright law: that is to say, a work containing the
+Program or a portion of it, either verbatim or with modifications and/or
+translated into another language.  (Hereinafter, translation is included
+without limitation in the term "modification".) Each licensee is addressed as
+"you".
+
+Activities other than copying, distribution and modification are not covered by
+this License; they are outside its scope.  The act of running the Program is
+not restricted, and the output from the Program is covered only if its contents
+constitute a work based on the Program (independent of having been made by
+running the Program).  Whether that is true depends on what the Program does.
+
+1. You may copy and distribute verbatim copies of the Program's source code as
+you receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice and
+disclaimer of warranty; keep intact all the notices that refer to this License
+and to the absence of any warranty; and give any other recipients of the
+Program a copy of this License along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and you may
+at your option offer warranty protection in exchange for a fee.
+
+2. You may modify your copy or copies of the Program or any portion of it, thus
+forming a work based on the Program, and copy and distribute such modifications
+or work under the terms of Section 1 above, provided that you also meet all of
+these conditions:
+
+    a) You must cause the modified files to carry prominent notices stating
+    that you changed the files and the date of any change.
+
+    b) You must cause any work that you distribute or publish, that in whole or
+    in part contains or is derived from the Program or any part thereof, to be
+    licensed as a whole at no charge to all third parties under the terms of
+    this License.
+
+    c) If the modified program normally reads commands interactively when run,
+    you must cause it, when started running for such interactive use in the
+    most ordinary way, to print or display an announcement including an
+    appropriate copyright notice and a notice that there is no warranty (or
+    else, saying that you provide a warranty) and that users may redistribute
+    the program under these conditions, and telling the user how to view a copy
+    of this License.  (Exception: if the Program itself is interactive but does
+    not normally print such an announcement, your work based on the Program is
+    not required to print an announcement.)
+
+These requirements apply to the modified work as a whole.  If identifiable
+sections of that work are not derived from the Program, and can be reasonably
+considered independent and separate works in themselves, then this License, and
+its terms, do not apply to those sections when you distribute them as separate
+works.  But when you distribute the same sections as part of a whole which is a
+work based on the Program, the distribution of the whole must be on the terms
+of this License, whose permissions for other licensees extend to the entire
+whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest your
+rights to work written entirely by you; rather, the intent is to exercise the
+right to control the distribution of derivative or collective works based on
+the Program.
+
+In addition, mere aggregation of another work not based on the Program with the
+Program (or with a work based on the Program) on a volume of a storage or
+distribution medium does not bring the other work under the scope of this
+License.
+
+3. You may copy and distribute the Program (or a work based on it, under
+Section 2) in object code or executable form under the terms of Sections 1 and
+2 above provided that you also do one of the following:
+
+    a) Accompany it with the complete corresponding machine-readable source
+    code, which must be distributed under the terms of Sections 1 and 2 above
+    on a medium customarily used for software interchange; or,
+
+    b) Accompany it with a written offer, valid for at least three years, to
+    give any third party, for a charge no more than your cost of physically
+    performing source distribution, a complete machine-readable copy of the
+    corresponding source code, to be distributed under the terms of Sections 1
+    and 2 above on a medium customarily used for software interchange; or,
+
+    c) Accompany it with the information you received as to the offer to
+    distribute corresponding source code.  (This alternative is allowed only
+    for noncommercial distribution and only if you received the program in
+    object code or executable form with such an offer, in accord with
+    Subsection b above.)
+
+The source code for a work means the preferred form of the work for making
+modifications to it.  For an executable work, complete source code means all
+the source code for all modules it contains, plus any associated interface
+definition files, plus the scripts used to control compilation and installation
+of the executable.  However, as a special exception, the source code
+distributed need not include anything that is normally distributed (in either
+source or binary form) with the major components (compiler, kernel, and so on)
+of the operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the source
+code from the same place counts as distribution of the source code, even though
+third parties are not compelled to copy the source along with the object code.
+
+4. You may not copy, modify, sublicense, or distribute the Program except as
+expressly provided under this License.  Any attempt otherwise to copy, modify,
+sublicense or distribute the Program is void, and will automatically terminate
+your rights under this License.  However, parties who have received copies, or
+rights, from you under this License will not have their licenses terminated so
+long as such parties remain in full compliance.
+
+5. You are not required to accept this License, since you have not signed it.
+However, nothing else grants you permission to modify or distribute the Program
+or its derivative works.  These actions are prohibited by law if you do not
+accept this License.  Therefore, by modifying or distributing the Program (or
+any work based on the Program), you indicate your acceptance of this License to
+do so, and all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+6. Each time you redistribute the Program (or any work based on the Program),
+the recipient automatically receives a license from the original licensor to
+copy, distribute or modify the Program subject to these terms and conditions.
+You may not impose any further restrictions on the recipients' exercise of the
+rights granted herein.  You are not responsible for enforcing compliance by
+third parties to this License.
+
+7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues), conditions
+are imposed on you (whether by court order, agreement or otherwise) that
+contradict the conditions of this License, they do not excuse you from the
+conditions of this License.  If you cannot distribute so as to satisfy
+simultaneously your obligations under this License and any other pertinent
+obligations, then as a consequence you may not distribute the Program at all.
+For example, if a patent license would not permit royalty-free redistribution
+of the Program by all those who receive copies directly or indirectly through
+you, then the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply and
+the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any patents or
+other property right claims or to contest validity of any such claims; this
+section has the sole purpose of protecting the integrity of the free software
+distribution system, which is implemented by public license practices.  Many
+people have made generous contributions to the wide range of software
+distributed through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing to
+distribute software through any other system and a licensee cannot impose that
+choice.
+
+This section is intended to make thoroughly clear what is believed to be a
+consequence of the rest of this License.
+
+8. If the distribution and/or use of the Program is restricted in certain
+countries either by patents or by copyrighted interfaces, the original
+copyright holder who places the Program under this License may add an explicit
+geographical distribution limitation excluding those countries, so that
+distribution is permitted only in or among countries not thus excluded.  In
+such case, this License incorporates the limitation as if written in the body
+of this License.
+
+9. The Free Software Foundation may publish revised and/or new versions of the
+General Public License from time to time.  Such new versions will be similar in
+spirit to the present version, but may differ in detail to address new problems
+or concerns.
+
+Each version is given a distinguishing version number.  If the Program
+specifies a version number of this License which applies to it and "any later
+version", you have the option of following the terms and conditions either of
+that version or of any later version published by the Free Software Foundation.
+If the Program does not specify a version number of this License, you may
+choose any version ever published by the Free Software Foundation.
+
+10. If you wish to incorporate parts of the Program into other free programs
+whose distribution conditions are different, write to the author to ask for
+permission.  For software which is copyrighted by the Free Software Foundation,
+write to the Free Software Foundation; we sometimes make exceptions for this.
+Our decision will be guided by the two goals of preserving the free status of
+all derivatives of our free software and of promoting the sharing and reuse of
+software generally.
+
+NO WARRANTY
+
+11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY FOR
+THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN OTHERWISE
+STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE
+PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED,
+INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND
+PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE,
+YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL
+ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR REDISTRIBUTE THE
+PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR
+INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA
+BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER
+OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
+
+END OF TERMS AND CONDITIONS
+
+How to Apply These Terms to Your New Programs
+
+If you develop a new program, and you want it to be of the greatest possible
+use to the public, the best way to achieve this is to make it free software
+which everyone can redistribute and change under these terms.
+
+To do so, attach the following notices to the program.  It is safest to attach
+them to the start of each source file to most effectively convey the exclusion
+of warranty; and each file should have at least the "copyright" line and a
+pointer to where the full notice is found.
+
+    One line to give the program's name and a brief idea of what it does.
+
+    Copyright (C) <year> <name of author>
+
+    This program is free software; you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by the Free
+    Software Foundation; either version 2 of the License, or (at your option)
+    any later version.
+
+    This program is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+    more details.
+
+    You should have received a copy of the GNU General Public License along
+    with this program; if not, write to the Free Software Foundation, Inc., 59
+    Temple Place, Suite 330, Boston, MA 02111-1307 USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program is interactive, make it output a short notice like this when it
+starts in an interactive mode:
+
+    Gnomovision version 69, Copyright (C) year name of author Gnomovision comes
+    with ABSOLUTELY NO WARRANTY; for details type 'show w'.  This is free
+    software, and you are welcome to redistribute it under certain conditions;
+    type 'show c' for details.
+
+The hypothetical commands 'show w' and 'show c' should show the appropriate
+parts of the General Public License.  Of course, the commands you use may be
+called something other than 'show w' and 'show c'; they could even be
+mouse-clicks or menu items--whatever suits your program.
+
+You should also get your employer (if you work as a programmer) or your school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.  Here
+is a sample; alter the names:
+
+    Yoyodyne, Inc., hereby disclaims all copyright interest in the program
+    'Gnomovision' (which makes passes at compilers) written by James Hacker.
+
+    signature of Ty Coon, 1 April 1989
+
+    Ty Coon, President of Vice
+
+This General Public License does not permit incorporating your program into
+proprietary programs.  If your program is a subroutine library, you may
+consider it more useful to permit linking proprietary applications with the
+library.  If this is what you want to do, use the GNU Library General Public
+License instead of this License.
+
+
+"CLASSPATH" EXCEPTION TO THE GPL
+
+Certain source files distributed by Oracle America and/or its affiliates are
+subject to the following clarification and special exception to the GPL, but
+only where Oracle has expressly included in the particular source file's header
+the words "Oracle designates this particular file as subject to the "Classpath"
+exception as provided by Oracle in the LICENSE file that accompanied this code."
+
+    Linking this library statically or dynamically with other modules is making
+    a combined work based on this library.  Thus, the terms and conditions of
+    the GNU General Public License cover the whole combination.
+
+    As a special exception, the copyright holders of this library give you
+    permission to link this library with independent modules to produce an
+    executable, regardless of the license terms of these independent modules,
+    and to copy and distribute the resulting executable under terms of your
+    choice, provided that you also meet, for each linked independent module,
+    the terms and conditions of the license of that module.  An independent
+    module is a module which is not derived from or based on this library.  If
+    you modify this library, you may extend this exception to your version of
+    the library, but you are not obligated to do so.  If you do not wish to do
+    so, delete this exception statement from your version.
diff --git a/java/arm-linux/jni.h b/java/arm-linux/jni.h
new file mode 100644
index 0000000..2e83cb7
--- /dev/null
+++ b/java/arm-linux/jni.h
@@ -0,0 +1,1960 @@
+/*
+ * Copyright (c) 1996, 2013, Oracle and/or its affiliates. All rights reserved.
+ * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
+ *
+ * This code is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 only, as
+ * published by the Free Software Foundation.  Oracle designates this
+ * particular file as subject to the "Classpath" exception as provided
+ * by Oracle in the LICENSE file that accompanied this code.
+ *
+ * This code 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
+ * version 2 for more details (a copy is included in the LICENSE file that
+ * accompanied this code).
+ *
+ * You should have received a copy of the GNU General Public License version
+ * 2 along with this work; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Please contact Oracle, 500 Oracle Parkway, Redwood Shores, CA 94065 USA
+ * or visit www.oracle.com if you need additional information or have any
+ * questions.
+ */
+
+/*
+ * We used part of Netscape's Java Runtime Interface (JRI) as the starting
+ * point of our design and implementation.
+ */
+
+/******************************************************************************
+ * Java Runtime Interface
+ * Copyright (c) 1996 Netscape Communications Corporation. All rights reserved.
+ *****************************************************************************/
+
+#ifndef _JAVASOFT_JNI_H_
+#define _JAVASOFT_JNI_H_
+
+#include <stdio.h>
+#include <stdarg.h>
+
+/* jni_md.h contains the machine-dependent typedefs for jbyte, jint
+   and jlong */
+
+#include "jni_md.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * JNI Types
+ */
+
+#ifndef JNI_TYPES_ALREADY_DEFINED_IN_JNI_MD_H
+
+typedef unsigned char   jboolean;
+typedef unsigned short  jchar;
+typedef short           jshort;
+typedef float           jfloat;
+typedef double          jdouble;
+
+typedef jint            jsize;
+
+#ifdef __cplusplus
+
+class _jobject {};
+class _jclass : public _jobject {};
+class _jthrowable : public _jobject {};
+class _jstring : public _jobject {};
+class _jarray : public _jobject {};
+class _jbooleanArray : public _jarray {};
+class _jbyteArray : public _jarray {};
+class _jcharArray : public _jarray {};
+class _jshortArray : public _jarray {};
+class _jintArray : public _jarray {};
+class _jlongArray : public _jarray {};
+class _jfloatArray : public _jarray {};
+class _jdoubleArray : public _jarray {};
+class _jobjectArray : public _jarray {};
+
+typedef _jobject *jobject;
+typedef _jclass *jclass;
+typedef _jthrowable *jthrowable;
+typedef _jstring *jstring;
+typedef _jarray *jarray;
+typedef _jbooleanArray *jbooleanArray;
+typedef _jbyteArray *jbyteArray;
+typedef _jcharArray *jcharArray;
+typedef _jshortArray *jshortArray;
+typedef _jintArray *jintArray;
+typedef _jlongArray *jlongArray;
+typedef _jfloatArray *jfloatArray;
+typedef _jdoubleArray *jdoubleArray;
+typedef _jobjectArray *jobjectArray;
+
+#else
+
+struct _jobject;
+
+typedef struct _jobject *jobject;
+typedef jobject jclass;
+typedef jobject jthrowable;
+typedef jobject jstring;
+typedef jobject jarray;
+typedef jarray jbooleanArray;
+typedef jarray jbyteArray;
+typedef jarray jcharArray;
+typedef jarray jshortArray;
+typedef jarray jintArray;
+typedef jarray jlongArray;
+typedef jarray jfloatArray;
+typedef jarray jdoubleArray;
+typedef jarray jobjectArray;
+
+#endif
+
+typedef jobject jweak;
+
+typedef union jvalue {
+    jboolean z;
+    jbyte    b;
+    jchar    c;
+    jshort   s;
+    jint     i;
+    jlong    j;
+    jfloat   f;
+    jdouble  d;
+    jobject  l;
+} jvalue;
+
+struct _jfieldID;
+typedef struct _jfieldID *jfieldID;
+
+struct _jmethodID;
+typedef struct _jmethodID *jmethodID;
+
+/* Return values from jobjectRefType */
+typedef enum _jobjectType {
+     JNIInvalidRefType    = 0,
+     JNILocalRefType      = 1,
+     JNIGlobalRefType     = 2,
+     JNIWeakGlobalRefType = 3
+} jobjectRefType;
+
+
+#endif /* JNI_TYPES_ALREADY_DEFINED_IN_JNI_MD_H */
+
+/*
+ * jboolean constants
+ */
+
+#define JNI_FALSE 0
+#define JNI_TRUE 1
+
+/*
+ * possible return values for JNI functions.
+ */
+
+#define JNI_OK           0                 /* success */
+#define JNI_ERR          (-1)              /* unknown error */
+#define JNI_EDETACHED    (-2)              /* thread detached from the VM */
+#define JNI_EVERSION     (-3)              /* JNI version error */
+#define JNI_ENOMEM       (-4)              /* not enough memory */
+#define JNI_EEXIST       (-5)              /* VM already created */
+#define JNI_EINVAL       (-6)              /* invalid arguments */
+
+/*
+ * used in ReleaseScalarArrayElements
+ */
+
+#define JNI_COMMIT 1
+#define JNI_ABORT 2
+
+/*
+ * used in RegisterNatives to describe native method name, signature,
+ * and function pointer.
+ */
+
+typedef struct {
+    char *name;
+    char *signature;
+    void *fnPtr;
+} JNINativeMethod;
+
+/*
+ * JNI Native Method Interface.
+ */
+
+struct JNINativeInterface_;
+
+struct JNIEnv_;
+
+#ifdef __cplusplus
+typedef JNIEnv_ JNIEnv;
+#else
+typedef const struct JNINativeInterface_ *JNIEnv;
+#endif
+
+/*
+ * JNI Invocation Interface.
+ */
+
+struct JNIInvokeInterface_;
+
+struct JavaVM_;
+
+#ifdef __cplusplus
+typedef JavaVM_ JavaVM;
+#else
+typedef const struct JNIInvokeInterface_ *JavaVM;
+#endif
+
+struct JNINativeInterface_ {
+    void *reserved0;
+    void *reserved1;
+    void *reserved2;
+
+    void *reserved3;
+    jint (JNICALL *GetVersion)(JNIEnv *env);
+
+    jclass (JNICALL *DefineClass)
+      (JNIEnv *env, const char *name, jobject loader, const jbyte *buf,
+       jsize len);
+    jclass (JNICALL *FindClass)
+      (JNIEnv *env, const char *name);
+
+    jmethodID (JNICALL *FromReflectedMethod)
+      (JNIEnv *env, jobject method);
+    jfieldID (JNICALL *FromReflectedField)
+      (JNIEnv *env, jobject field);
+
+    jobject (JNICALL *ToReflectedMethod)
+      (JNIEnv *env, jclass cls, jmethodID methodID, jboolean isStatic);
+
+    jclass (JNICALL *GetSuperclass)
+      (JNIEnv *env, jclass sub);
+    jboolean (JNICALL *IsAssignableFrom)
+      (JNIEnv *env, jclass sub, jclass sup);
+
+    jobject (JNICALL *ToReflectedField)
+      (JNIEnv *env, jclass cls, jfieldID fieldID, jboolean isStatic);
+
+    jint (JNICALL *Throw)
+      (JNIEnv *env, jthrowable obj);
+    jint (JNICALL *ThrowNew)
+      (JNIEnv *env, jclass clazz, const char *msg);
+    jthrowable (JNICALL *ExceptionOccurred)
+      (JNIEnv *env);
+    void (JNICALL *ExceptionDescribe)
+      (JNIEnv *env);
+    void (JNICALL *ExceptionClear)
+      (JNIEnv *env);
+    void (JNICALL *FatalError)
+      (JNIEnv *env, const char *msg);
+
+    jint (JNICALL *PushLocalFrame)
+      (JNIEnv *env, jint capacity);
+    jobject (JNICALL *PopLocalFrame)
+      (JNIEnv *env, jobject result);
+
+    jobject (JNICALL *NewGlobalRef)
+      (JNIEnv *env, jobject lobj);
+    void (JNICALL *DeleteGlobalRef)
+      (JNIEnv *env, jobject gref);
+    void (JNICALL *DeleteLocalRef)
+      (JNIEnv *env, jobject obj);
+    jboolean (JNICALL *IsSameObject)
+      (JNIEnv *env, jobject obj1, jobject obj2);
+    jobject (JNICALL *NewLocalRef)
+      (JNIEnv *env, jobject ref);
+    jint (JNICALL *EnsureLocalCapacity)
+      (JNIEnv *env, jint capacity);
+
+    jobject (JNICALL *AllocObject)
+      (JNIEnv *env, jclass clazz);
+    jobject (JNICALL *NewObject)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jobject (JNICALL *NewObjectV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jobject (JNICALL *NewObjectA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    jclass (JNICALL *GetObjectClass)
+      (JNIEnv *env, jobject obj);
+    jboolean (JNICALL *IsInstanceOf)
+      (JNIEnv *env, jobject obj, jclass clazz);
+
+    jmethodID (JNICALL *GetMethodID)
+      (JNIEnv *env, jclass clazz, const char *name, const char *sig);
+
+    jobject (JNICALL *CallObjectMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    jobject (JNICALL *CallObjectMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    jobject (JNICALL *CallObjectMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue * args);
+
+    jboolean (JNICALL *CallBooleanMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    jboolean (JNICALL *CallBooleanMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    jboolean (JNICALL *CallBooleanMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue * args);
+
+    jbyte (JNICALL *CallByteMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    jbyte (JNICALL *CallByteMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    jbyte (JNICALL *CallByteMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue *args);
+
+    jchar (JNICALL *CallCharMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    jchar (JNICALL *CallCharMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    jchar (JNICALL *CallCharMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue *args);
+
+    jshort (JNICALL *CallShortMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    jshort (JNICALL *CallShortMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    jshort (JNICALL *CallShortMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue *args);
+
+    jint (JNICALL *CallIntMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    jint (JNICALL *CallIntMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    jint (JNICALL *CallIntMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue *args);
+
+    jlong (JNICALL *CallLongMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    jlong (JNICALL *CallLongMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    jlong (JNICALL *CallLongMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue *args);
+
+    jfloat (JNICALL *CallFloatMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    jfloat (JNICALL *CallFloatMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    jfloat (JNICALL *CallFloatMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue *args);
+
+    jdouble (JNICALL *CallDoubleMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    jdouble (JNICALL *CallDoubleMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    jdouble (JNICALL *CallDoubleMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue *args);
+
+    void (JNICALL *CallVoidMethod)
+      (JNIEnv *env, jobject obj, jmethodID methodID, ...);
+    void (JNICALL *CallVoidMethodV)
+      (JNIEnv *env, jobject obj, jmethodID methodID, va_list args);
+    void (JNICALL *CallVoidMethodA)
+      (JNIEnv *env, jobject obj, jmethodID methodID, const jvalue * args);
+
+    jobject (JNICALL *CallNonvirtualObjectMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    jobject (JNICALL *CallNonvirtualObjectMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    jobject (JNICALL *CallNonvirtualObjectMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue * args);
+
+    jboolean (JNICALL *CallNonvirtualBooleanMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    jboolean (JNICALL *CallNonvirtualBooleanMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    jboolean (JNICALL *CallNonvirtualBooleanMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue * args);
+
+    jbyte (JNICALL *CallNonvirtualByteMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    jbyte (JNICALL *CallNonvirtualByteMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    jbyte (JNICALL *CallNonvirtualByteMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue *args);
+
+    jchar (JNICALL *CallNonvirtualCharMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    jchar (JNICALL *CallNonvirtualCharMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    jchar (JNICALL *CallNonvirtualCharMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue *args);
+
+    jshort (JNICALL *CallNonvirtualShortMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    jshort (JNICALL *CallNonvirtualShortMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    jshort (JNICALL *CallNonvirtualShortMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue *args);
+
+    jint (JNICALL *CallNonvirtualIntMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    jint (JNICALL *CallNonvirtualIntMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    jint (JNICALL *CallNonvirtualIntMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue *args);
+
+    jlong (JNICALL *CallNonvirtualLongMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    jlong (JNICALL *CallNonvirtualLongMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    jlong (JNICALL *CallNonvirtualLongMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue *args);
+
+    jfloat (JNICALL *CallNonvirtualFloatMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    jfloat (JNICALL *CallNonvirtualFloatMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    jfloat (JNICALL *CallNonvirtualFloatMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue *args);
+
+    jdouble (JNICALL *CallNonvirtualDoubleMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    jdouble (JNICALL *CallNonvirtualDoubleMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    jdouble (JNICALL *CallNonvirtualDoubleMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue *args);
+
+    void (JNICALL *CallNonvirtualVoidMethod)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID, ...);
+    void (JNICALL *CallNonvirtualVoidMethodV)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       va_list args);
+    void (JNICALL *CallNonvirtualVoidMethodA)
+      (JNIEnv *env, jobject obj, jclass clazz, jmethodID methodID,
+       const jvalue * args);
+
+    jfieldID (JNICALL *GetFieldID)
+      (JNIEnv *env, jclass clazz, const char *name, const char *sig);
+
+    jobject (JNICALL *GetObjectField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID);
+    jboolean (JNICALL *GetBooleanField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID);
+    jbyte (JNICALL *GetByteField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID);
+    jchar (JNICALL *GetCharField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID);
+    jshort (JNICALL *GetShortField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID);
+    jint (JNICALL *GetIntField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID);
+    jlong (JNICALL *GetLongField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID);
+    jfloat (JNICALL *GetFloatField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID);
+    jdouble (JNICALL *GetDoubleField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID);
+
+    void (JNICALL *SetObjectField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID, jobject val);
+    void (JNICALL *SetBooleanField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID, jboolean val);
+    void (JNICALL *SetByteField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID, jbyte val);
+    void (JNICALL *SetCharField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID, jchar val);
+    void (JNICALL *SetShortField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID, jshort val);
+    void (JNICALL *SetIntField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID, jint val);
+    void (JNICALL *SetLongField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID, jlong val);
+    void (JNICALL *SetFloatField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID, jfloat val);
+    void (JNICALL *SetDoubleField)
+      (JNIEnv *env, jobject obj, jfieldID fieldID, jdouble val);
+
+    jmethodID (JNICALL *GetStaticMethodID)
+      (JNIEnv *env, jclass clazz, const char *name, const char *sig);
+
+    jobject (JNICALL *CallStaticObjectMethod)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jobject (JNICALL *CallStaticObjectMethodV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jobject (JNICALL *CallStaticObjectMethodA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    jboolean (JNICALL *CallStaticBooleanMethod)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jboolean (JNICALL *CallStaticBooleanMethodV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jboolean (JNICALL *CallStaticBooleanMethodA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    jbyte (JNICALL *CallStaticByteMethod)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jbyte (JNICALL *CallStaticByteMethodV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jbyte (JNICALL *CallStaticByteMethodA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    jchar (JNICALL *CallStaticCharMethod)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jchar (JNICALL *CallStaticCharMethodV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jchar (JNICALL *CallStaticCharMethodA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    jshort (JNICALL *CallStaticShortMethod)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jshort (JNICALL *CallStaticShortMethodV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jshort (JNICALL *CallStaticShortMethodA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    jint (JNICALL *CallStaticIntMethod)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jint (JNICALL *CallStaticIntMethodV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jint (JNICALL *CallStaticIntMethodA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    jlong (JNICALL *CallStaticLongMethod)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jlong (JNICALL *CallStaticLongMethodV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jlong (JNICALL *CallStaticLongMethodA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    jfloat (JNICALL *CallStaticFloatMethod)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jfloat (JNICALL *CallStaticFloatMethodV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jfloat (JNICALL *CallStaticFloatMethodA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    jdouble (JNICALL *CallStaticDoubleMethod)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, ...);
+    jdouble (JNICALL *CallStaticDoubleMethodV)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, va_list args);
+    jdouble (JNICALL *CallStaticDoubleMethodA)
+      (JNIEnv *env, jclass clazz, jmethodID methodID, const jvalue *args);
+
+    void (JNICALL *CallStaticVoidMethod)
+      (JNIEnv *env, jclass cls, jmethodID methodID, ...);
+    void (JNICALL *CallStaticVoidMethodV)
+      (JNIEnv *env, jclass cls, jmethodID methodID, va_list args);
+    void (JNICALL *CallStaticVoidMethodA)
+      (JNIEnv *env, jclass cls, jmethodID methodID, const jvalue * args);
+
+    jfieldID (JNICALL *GetStaticFieldID)
+      (JNIEnv *env, jclass clazz, const char *name, const char *sig);
+    jobject (JNICALL *GetStaticObjectField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID);
+    jboolean (JNICALL *GetStaticBooleanField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID);
+    jbyte (JNICALL *GetStaticByteField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID);
+    jchar (JNICALL *GetStaticCharField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID);
+    jshort (JNICALL *GetStaticShortField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID);
+    jint (JNICALL *GetStaticIntField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID);
+    jlong (JNICALL *GetStaticLongField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID);
+    jfloat (JNICALL *GetStaticFloatField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID);
+    jdouble (JNICALL *GetStaticDoubleField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID);
+
+    void (JNICALL *SetStaticObjectField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID, jobject value);
+    void (JNICALL *SetStaticBooleanField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID, jboolean value);
+    void (JNICALL *SetStaticByteField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID, jbyte value);
+    void (JNICALL *SetStaticCharField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID, jchar value);
+    void (JNICALL *SetStaticShortField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID, jshort value);
+    void (JNICALL *SetStaticIntField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID, jint value);
+    void (JNICALL *SetStaticLongField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID, jlong value);
+    void (JNICALL *SetStaticFloatField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID, jfloat value);
+    void (JNICALL *SetStaticDoubleField)
+      (JNIEnv *env, jclass clazz, jfieldID fieldID, jdouble value);
+
+    jstring (JNICALL *NewString)
+      (JNIEnv *env, const jchar *unicode, jsize len);
+    jsize (JNICALL *GetStringLength)
+      (JNIEnv *env, jstring str);
+    const jchar *(JNICALL *GetStringChars)
+      (JNIEnv *env, jstring str, jboolean *isCopy);
+    void (JNICALL *ReleaseStringChars)
+      (JNIEnv *env, jstring str, const jchar *chars);
+
+    jstring (JNICALL *NewStringUTF)
+      (JNIEnv *env, const char *utf);
+    jsize (JNICALL *GetStringUTFLength)
+      (JNIEnv *env, jstring str);
+    const char* (JNICALL *GetStringUTFChars)
+      (JNIEnv *env, jstring str, jboolean *isCopy);
+    void (JNICALL *ReleaseStringUTFChars)
+      (JNIEnv *env, jstring str, const char* chars);
+
+
+    jsize (JNICALL *GetArrayLength)
+      (JNIEnv *env, jarray array);
+
+    jobjectArray (JNICALL *NewObjectArray)
+      (JNIEnv *env, jsize len, jclass clazz, jobject init);
+    jobject (JNICALL *GetObjectArrayElement)
+      (JNIEnv *env, jobjectArray array, jsize index);
+    void (JNICALL *SetObjectArrayElement)
+      (JNIEnv *env, jobjectArray array, jsize index, jobject val);
+
+    jbooleanArray (JNICALL *NewBooleanArray)
+      (JNIEnv *env, jsize len);
+    jbyteArray (JNICALL *NewByteArray)
+      (JNIEnv *env, jsize len);
+    jcharArray (JNICALL *NewCharArray)
+      (JNIEnv *env, jsize len);
+    jshortArray (JNICALL *NewShortArray)
+      (JNIEnv *env, jsize len);
+    jintArray (JNICALL *NewIntArray)
+      (JNIEnv *env, jsize len);
+    jlongArray (JNICALL *NewLongArray)
+      (JNIEnv *env, jsize len);
+    jfloatArray (JNICALL *NewFloatArray)
+      (JNIEnv *env, jsize len);
+    jdoubleArray (JNICALL *NewDoubleArray)
+      (JNIEnv *env, jsize len);
+
+    jboolean * (JNICALL *GetBooleanArrayElements)
+      (JNIEnv *env, jbooleanArray array, jboolean *isCopy);
+    jbyte * (JNICALL *GetByteArrayElements)
+      (JNIEnv *env, jbyteArray array, jboolean *isCopy);
+    jchar * (JNICALL *GetCharArrayElements)
+      (JNIEnv *env, jcharArray array, jboolean *isCopy);
+    jshort * (JNICALL *GetShortArrayElements)
+      (JNIEnv *env, jshortArray array, jboolean *isCopy);
+    jint * (JNICALL *GetIntArrayElements)
+      (JNIEnv *env, jintArray array, jboolean *isCopy);
+    jlong * (JNICALL *GetLongArrayElements)
+      (JNIEnv *env, jlongArray array, jboolean *isCopy);
+    jfloat * (JNICALL *GetFloatArrayElements)
+      (JNIEnv *env, jfloatArray array, jboolean *isCopy);
+    jdouble * (JNICALL *GetDoubleArrayElements)
+      (JNIEnv *env, jdoubleArray array, jboolean *isCopy);
+
+    void (JNICALL *ReleaseBooleanArrayElements)
+      (JNIEnv *env, jbooleanArray array, jboolean *elems, jint mode);
+    void (JNICALL *ReleaseByteArrayElements)
+      (JNIEnv *env, jbyteArray array, jbyte *elems, jint mode);
+    void (JNICALL *ReleaseCharArrayElements)
+      (JNIEnv *env, jcharArray array, jchar *elems, jint mode);
+    void (JNICALL *ReleaseShortArrayElements)
+      (JNIEnv *env, jshortArray array, jshort *elems, jint mode);
+    void (JNICALL *ReleaseIntArrayElements)
+      (JNIEnv *env, jintArray array, jint *elems, jint mode);
+    void (JNICALL *ReleaseLongArrayElements)
+      (JNIEnv *env, jlongArray array, jlong *elems, jint mode);
+    void (JNICALL *ReleaseFloatArrayElements)
+      (JNIEnv *env, jfloatArray array, jfloat *elems, jint mode);
+    void (JNICALL *ReleaseDoubleArrayElements)
+      (JNIEnv *env, jdoubleArray array, jdouble *elems, jint mode);
+
+    void (JNICALL *GetBooleanArrayRegion)
+      (JNIEnv *env, jbooleanArray array, jsize start, jsize l, jboolean *buf);
+    void (JNICALL *GetByteArrayRegion)
+      (JNIEnv *env, jbyteArray array, jsize start, jsize len, jbyte *buf);
+    void (JNICALL *GetCharArrayRegion)
+      (JNIEnv *env, jcharArray array, jsize start, jsize len, jchar *buf);
+    void (JNICALL *GetShortArrayRegion)
+      (JNIEnv *env, jshortArray array, jsize start, jsize len, jshort *buf);
+    void (JNICALL *GetIntArrayRegion)
+      (JNIEnv *env, jintArray array, jsize start, jsize len, jint *buf);
+    void (JNICALL *GetLongArrayRegion)
+      (JNIEnv *env, jlongArray array, jsize start, jsize len, jlong *buf);
+    void (JNICALL *GetFloatArrayRegion)
+      (JNIEnv *env, jfloatArray array, jsize start, jsize len, jfloat *buf);
+    void (JNICALL *GetDoubleArrayRegion)
+      (JNIEnv *env, jdoubleArray array, jsize start, jsize len, jdouble *buf);
+
+    void (JNICALL *SetBooleanArrayRegion)
+      (JNIEnv *env, jbooleanArray array, jsize start, jsize l, const jboolean *buf);
+    void (JNICALL *SetByteArrayRegion)
+      (JNIEnv *env, jbyteArray array, jsize start, jsize len, const jbyte *buf);
+    void (JNICALL *SetCharArrayRegion)
+      (JNIEnv *env, jcharArray array, jsize start, jsize len, const jchar *buf);
+    void (JNICALL *SetShortArrayRegion)
+      (JNIEnv *env, jshortArray array, jsize start, jsize len, const jshort *buf);
+    void (JNICALL *SetIntArrayRegion)
+      (JNIEnv *env, jintArray array, jsize start, jsize len, const jint *buf);
+    void (JNICALL *SetLongArrayRegion)
+      (JNIEnv *env, jlongArray array, jsize start, jsize len, const jlong *buf);
+    void (JNICALL *SetFloatArrayRegion)
+      (JNIEnv *env, jfloatArray array, jsize start, jsize len, const jfloat *buf);
+    void (JNICALL *SetDoubleArrayRegion)
+      (JNIEnv *env, jdoubleArray array, jsize start, jsize len, const jdouble *buf);
+
+    jint (JNICALL *RegisterNatives)
+      (JNIEnv *env, jclass clazz, const JNINativeMethod *methods,
+       jint nMethods);
+    jint (JNICALL *UnregisterNatives)
+      (JNIEnv *env, jclass clazz);
+
+    jint (JNICALL *MonitorEnter)
+      (JNIEnv *env, jobject obj);
+    jint (JNICALL *MonitorExit)
+      (JNIEnv *env, jobject obj);
+
+    jint (JNICALL *GetJavaVM)
+      (JNIEnv *env, JavaVM **vm);
+
+    void (JNICALL *GetStringRegion)
+      (JNIEnv *env, jstring str, jsize start, jsize len, jchar *buf);
+    void (JNICALL *GetStringUTFRegion)
+      (JNIEnv *env, jstring str, jsize start, jsize len, char *buf);
+
+    void * (JNICALL *GetPrimitiveArrayCritical)
+      (JNIEnv *env, jarray array, jboolean *isCopy);
+    void (JNICALL *ReleasePrimitiveArrayCritical)
+      (JNIEnv *env, jarray array, void *carray, jint mode);
+
+    const jchar * (JNICALL *GetStringCritical)
+      (JNIEnv *env, jstring string, jboolean *isCopy);
+    void (JNICALL *ReleaseStringCritical)
+      (JNIEnv *env, jstring string, const jchar *cstring);
+
+    jweak (JNICALL *NewWeakGlobalRef)
+       (JNIEnv *env, jobject obj);
+    void (JNICALL *DeleteWeakGlobalRef)
+       (JNIEnv *env, jweak ref);
+
+    jboolean (JNICALL *ExceptionCheck)
+       (JNIEnv *env);
+
+    jobject (JNICALL *NewDirectByteBuffer)
+       (JNIEnv* env, void* address, jlong capacity);
+    void* (JNICALL *GetDirectBufferAddress)
+       (JNIEnv* env, jobject buf);
+    jlong (JNICALL *GetDirectBufferCapacity)
+       (JNIEnv* env, jobject buf);
+
+    /* New JNI 1.6 Features */
+
+    jobjectRefType (JNICALL *GetObjectRefType)
+        (JNIEnv* env, jobject obj);
+};
+
+/*
+ * We use inlined functions for C++ so that programmers can write:
+ *
+ *    env->FindClass("java/lang/String")
+ *
+ * in C++ rather than:
+ *
+ *    (*env)->FindClass(env, "java/lang/String")
+ *
+ * in C.
+ */
+
+struct JNIEnv_ {
+    const struct JNINativeInterface_ *functions;
+#ifdef __cplusplus
+
+    jint GetVersion() {
+        return functions->GetVersion(this);
+    }
+    jclass DefineClass(const char *name, jobject loader, const jbyte *buf,
+                       jsize len) {
+        return functions->DefineClass(this, name, loader, buf, len);
+    }
+    jclass FindClass(const char *name) {
+        return functions->FindClass(this, name);
+    }
+    jmethodID FromReflectedMethod(jobject method) {
+        return functions->FromReflectedMethod(this,method);
+    }
+    jfieldID FromReflectedField(jobject field) {
+        return functions->FromReflectedField(this,field);
+    }
+
+    jobject ToReflectedMethod(jclass cls, jmethodID methodID, jboolean isStatic) {
+        return functions->ToReflectedMethod(this, cls, methodID, isStatic);
+    }
+
+    jclass GetSuperclass(jclass sub) {
+        return functions->GetSuperclass(this, sub);
+    }
+    jboolean IsAssignableFrom(jclass sub, jclass sup) {
+        return functions->IsAssignableFrom(this, sub, sup);
+    }
+
+    jobject ToReflectedField(jclass cls, jfieldID fieldID, jboolean isStatic) {
+        return functions->ToReflectedField(this,cls,fieldID,isStatic);
+    }
+
+    jint Throw(jthrowable obj) {
+        return functions->Throw(this, obj);
+    }
+    jint ThrowNew(jclass clazz, const char *msg) {
+        return functions->ThrowNew(this, clazz, msg);
+    }
+    jthrowable ExceptionOccurred() {
+        return functions->ExceptionOccurred(this);
+    }
+    void ExceptionDescribe() {
+        functions->ExceptionDescribe(this);
+    }
+    void ExceptionClear() {
+        functions->ExceptionClear(this);
+    }
+    void FatalError(const char *msg) {
+        functions->FatalError(this, msg);
+    }
+
+    jint PushLocalFrame(jint capacity) {
+        return functions->PushLocalFrame(this,capacity);
+    }
+    jobject PopLocalFrame(jobject result) {
+        return functions->PopLocalFrame(this,result);
+    }
+
+    jobject NewGlobalRef(jobject lobj) {
+        return functions->NewGlobalRef(this,lobj);
+    }
+    void DeleteGlobalRef(jobject gref) {
+        functions->DeleteGlobalRef(this,gref);
+    }
+    void DeleteLocalRef(jobject obj) {
+        functions->DeleteLocalRef(this, obj);
+    }
+
+    jboolean IsSameObject(jobject obj1, jobject obj2) {
+        return functions->IsSameObject(this,obj1,obj2);
+    }
+
+    jobject NewLocalRef(jobject ref) {
+        return functions->NewLocalRef(this,ref);
+    }
+    jint EnsureLocalCapacity(jint capacity) {
+        return functions->EnsureLocalCapacity(this,capacity);
+    }
+
+    jobject AllocObject(jclass clazz) {
+        return functions->AllocObject(this,clazz);
+    }
+    jobject NewObject(jclass clazz, jmethodID methodID, ...) {
+        va_list args;
+        jobject result;
+        va_start(args, methodID);
+        result = functions->NewObjectV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jobject NewObjectV(jclass clazz, jmethodID methodID,
+                       va_list args) {
+        return functions->NewObjectV(this,clazz,methodID,args);
+    }
+    jobject NewObjectA(jclass clazz, jmethodID methodID,
+                       const jvalue *args) {
+        return functions->NewObjectA(this,clazz,methodID,args);
+    }
+
+    jclass GetObjectClass(jobject obj) {
+        return functions->GetObjectClass(this,obj);
+    }
+    jboolean IsInstanceOf(jobject obj, jclass clazz) {
+        return functions->IsInstanceOf(this,obj,clazz);
+    }
+
+    jmethodID GetMethodID(jclass clazz, const char *name,
+                          const char *sig) {
+        return functions->GetMethodID(this,clazz,name,sig);
+    }
+
+    jobject CallObjectMethod(jobject obj, jmethodID methodID, ...) {
+        va_list args;
+        jobject result;
+        va_start(args,methodID);
+        result = functions->CallObjectMethodV(this,obj,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jobject CallObjectMethodV(jobject obj, jmethodID methodID,
+                        va_list args) {
+        return functions->CallObjectMethodV(this,obj,methodID,args);
+    }
+    jobject CallObjectMethodA(jobject obj, jmethodID methodID,
+                        const jvalue * args) {
+        return functions->CallObjectMethodA(this,obj,methodID,args);
+    }
+
+    jboolean CallBooleanMethod(jobject obj,
+                               jmethodID methodID, ...) {
+        va_list args;
+        jboolean result;
+        va_start(args,methodID);
+        result = functions->CallBooleanMethodV(this,obj,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jboolean CallBooleanMethodV(jobject obj, jmethodID methodID,
+                                va_list args) {
+        return functions->CallBooleanMethodV(this,obj,methodID,args);
+    }
+    jboolean CallBooleanMethodA(jobject obj, jmethodID methodID,
+                                const jvalue * args) {
+        return functions->CallBooleanMethodA(this,obj,methodID, args);
+    }
+
+    jbyte CallByteMethod(jobject obj, jmethodID methodID, ...) {
+        va_list args;
+        jbyte result;
+        va_start(args,methodID);
+        result = functions->CallByteMethodV(this,obj,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jbyte CallByteMethodV(jobject obj, jmethodID methodID,
+                          va_list args) {
+        return functions->CallByteMethodV(this,obj,methodID,args);
+    }
+    jbyte CallByteMethodA(jobject obj, jmethodID methodID,
+                          const jvalue * args) {
+        return functions->CallByteMethodA(this,obj,methodID,args);
+    }
+
+    jchar CallCharMethod(jobject obj, jmethodID methodID, ...) {
+        va_list args;
+        jchar result;
+        va_start(args,methodID);
+        result = functions->CallCharMethodV(this,obj,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jchar CallCharMethodV(jobject obj, jmethodID methodID,
+                          va_list args) {
+        return functions->CallCharMethodV(this,obj,methodID,args);
+    }
+    jchar CallCharMethodA(jobject obj, jmethodID methodID,
+                          const jvalue * args) {
+        return functions->CallCharMethodA(this,obj,methodID,args);
+    }
+
+    jshort CallShortMethod(jobject obj, jmethodID methodID, ...) {
+        va_list args;
+        jshort result;
+        va_start(args,methodID);
+        result = functions->CallShortMethodV(this,obj,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jshort CallShortMethodV(jobject obj, jmethodID methodID,
+                            va_list args) {
+        return functions->CallShortMethodV(this,obj,methodID,args);
+    }
+    jshort CallShortMethodA(jobject obj, jmethodID methodID,
+                            const jvalue * args) {
+        return functions->CallShortMethodA(this,obj,methodID,args);
+    }
+
+    jint CallIntMethod(jobject obj, jmethodID methodID, ...) {
+        va_list args;
+        jint result;
+        va_start(args,methodID);
+        result = functions->CallIntMethodV(this,obj,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jint CallIntMethodV(jobject obj, jmethodID methodID,
+                        va_list args) {
+        return functions->CallIntMethodV(this,obj,methodID,args);
+    }
+    jint CallIntMethodA(jobject obj, jmethodID methodID,
+                        const jvalue * args) {
+        return functions->CallIntMethodA(this,obj,methodID,args);
+    }
+
+    jlong CallLongMethod(jobject obj, jmethodID methodID, ...) {
+        va_list args;
+        jlong result;
+        va_start(args,methodID);
+        result = functions->CallLongMethodV(this,obj,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jlong CallLongMethodV(jobject obj, jmethodID methodID,
+                          va_list args) {
+        return functions->CallLongMethodV(this,obj,methodID,args);
+    }
+    jlong CallLongMethodA(jobject obj, jmethodID methodID,
+                          const jvalue * args) {
+        return functions->CallLongMethodA(this,obj,methodID,args);
+    }
+
+    jfloat CallFloatMethod(jobject obj, jmethodID methodID, ...) {
+        va_list args;
+        jfloat result;
+        va_start(args,methodID);
+        result = functions->CallFloatMethodV(this,obj,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jfloat CallFloatMethodV(jobject obj, jmethodID methodID,
+                            va_list args) {
+        return functions->CallFloatMethodV(this,obj,methodID,args);
+    }
+    jfloat CallFloatMethodA(jobject obj, jmethodID methodID,
+                            const jvalue * args) {
+        return functions->CallFloatMethodA(this,obj,methodID,args);
+    }
+
+    jdouble CallDoubleMethod(jobject obj, jmethodID methodID, ...) {
+        va_list args;
+        jdouble result;
+        va_start(args,methodID);
+        result = functions->CallDoubleMethodV(this,obj,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jdouble CallDoubleMethodV(jobject obj, jmethodID methodID,
+                        va_list args) {
+        return functions->CallDoubleMethodV(this,obj,methodID,args);
+    }
+    jdouble CallDoubleMethodA(jobject obj, jmethodID methodID,
+                        const jvalue * args) {
+        return functions->CallDoubleMethodA(this,obj,methodID,args);
+    }
+
+    void CallVoidMethod(jobject obj, jmethodID methodID, ...) {
+        va_list args;
+        va_start(args,methodID);
+        functions->CallVoidMethodV(this,obj,methodID,args);
+        va_end(args);
+    }
+    void CallVoidMethodV(jobject obj, jmethodID methodID,
+                         va_list args) {
+        functions->CallVoidMethodV(this,obj,methodID,args);
+    }
+    void CallVoidMethodA(jobject obj, jmethodID methodID,
+                         const jvalue * args) {
+        functions->CallVoidMethodA(this,obj,methodID,args);
+    }
+
+    jobject CallNonvirtualObjectMethod(jobject obj, jclass clazz,
+                                       jmethodID methodID, ...) {
+        va_list args;
+        jobject result;
+        va_start(args,methodID);
+        result = functions->CallNonvirtualObjectMethodV(this,obj,clazz,
+                                                        methodID,args);
+        va_end(args);
+        return result;
+    }
+    jobject CallNonvirtualObjectMethodV(jobject obj, jclass clazz,
+                                        jmethodID methodID, va_list args) {
+        return functions->CallNonvirtualObjectMethodV(this,obj,clazz,
+                                                      methodID,args);
+    }
+    jobject CallNonvirtualObjectMethodA(jobject obj, jclass clazz,
+                                        jmethodID methodID, const jvalue * args) {
+        return functions->CallNonvirtualObjectMethodA(this,obj,clazz,
+                                                      methodID,args);
+    }
+
+    jboolean CallNonvirtualBooleanMethod(jobject obj, jclass clazz,
+                                         jmethodID methodID, ...) {
+        va_list args;
+        jboolean result;
+        va_start(args,methodID);
+        result = functions->CallNonvirtualBooleanMethodV(this,obj,clazz,
+                                                         methodID,args);
+        va_end(args);
+        return result;
+    }
+    jboolean CallNonvirtualBooleanMethodV(jobject obj, jclass clazz,
+                                          jmethodID methodID, va_list args) {
+        return functions->CallNonvirtualBooleanMethodV(this,obj,clazz,
+                                                       methodID,args);
+    }
+    jboolean CallNonvirtualBooleanMethodA(jobject obj, jclass clazz,
+                                          jmethodID methodID, const jvalue * args) {
+        return functions->CallNonvirtualBooleanMethodA(this,obj,clazz,
+                                                       methodID, args);
+    }
+
+    jbyte CallNonvirtualByteMethod(jobject obj, jclass clazz,
+                                   jmethodID methodID, ...) {
+        va_list args;
+        jbyte result;
+        va_start(args,methodID);
+        result = functions->CallNonvirtualByteMethodV(this,obj,clazz,
+                                                      methodID,args);
+        va_end(args);
+        return result;
+    }
+    jbyte CallNonvirtualByteMethodV(jobject obj, jclass clazz,
+                                    jmethodID methodID, va_list args) {
+        return functions->CallNonvirtualByteMethodV(this,obj,clazz,
+                                                    methodID,args);
+    }
+    jbyte CallNonvirtualByteMethodA(jobject obj, jclass clazz,
+                                    jmethodID methodID, const jvalue * args) {
+        return functions->CallNonvirtualByteMethodA(this,obj,clazz,
+                                                    methodID,args);
+    }
+
+    jchar CallNonvirtualCharMethod(jobject obj, jclass clazz,
+                                   jmethodID methodID, ...) {
+        va_list args;
+        jchar result;
+        va_start(args,methodID);
+        result = functions->CallNonvirtualCharMethodV(this,obj,clazz,
+                                                      methodID,args);
+        va_end(args);
+        return result;
+    }
+    jchar CallNonvirtualCharMethodV(jobject obj, jclass clazz,
+                                    jmethodID methodID, va_list args) {
+        return functions->CallNonvirtualCharMethodV(this,obj,clazz,
+                                                    methodID,args);
+    }
+    jchar CallNonvirtualCharMethodA(jobject obj, jclass clazz,
+                                    jmethodID methodID, const jvalue * args) {
+        return functions->CallNonvirtualCharMethodA(this,obj,clazz,
+                                                    methodID,args);
+    }
+
+    jshort CallNonvirtualShortMethod(jobject obj, jclass clazz,
+                                     jmethodID methodID, ...) {
+        va_list args;
+        jshort result;
+        va_start(args,methodID);
+        result = functions->CallNonvirtualShortMethodV(this,obj,clazz,
+                                                       methodID,args);
+        va_end(args);
+        return result;
+    }
+    jshort CallNonvirtualShortMethodV(jobject obj, jclass clazz,
+                                      jmethodID methodID, va_list args) {
+        return functions->CallNonvirtualShortMethodV(this,obj,clazz,
+                                                     methodID,args);
+    }
+    jshort CallNonvirtualShortMethodA(jobject obj, jclass clazz,
+                                      jmethodID methodID, const jvalue * args) {
+        return functions->CallNonvirtualShortMethodA(this,obj,clazz,
+                                                     methodID,args);
+    }
+
+    jint CallNonvirtualIntMethod(jobject obj, jclass clazz,
+                                 jmethodID methodID, ...) {
+        va_list args;
+        jint result;
+        va_start(args,methodID);
+        result = functions->CallNonvirtualIntMethodV(this,obj,clazz,
+                                                     methodID,args);
+        va_end(args);
+        return result;
+    }
+    jint CallNonvirtualIntMethodV(jobject obj, jclass clazz,
+                                  jmethodID methodID, va_list args) {
+        return functions->CallNonvirtualIntMethodV(this,obj,clazz,
+                                                   methodID,args);
+    }
+    jint CallNonvirtualIntMethodA(jobject obj, jclass clazz,
+                                  jmethodID methodID, const jvalue * args) {
+        return functions->CallNonvirtualIntMethodA(this,obj,clazz,
+                                                   methodID,args);
+    }
+
+    jlong CallNonvirtualLongMethod(jobject obj, jclass clazz,
+                                   jmethodID methodID, ...) {
+        va_list args;
+        jlong result;
+        va_start(args,methodID);
+        result = functions->CallNonvirtualLongMethodV(this,obj,clazz,
+                                                      methodID,args);
+        va_end(args);
+        return result;
+    }
+    jlong CallNonvirtualLongMethodV(jobject obj, jclass clazz,
+                                    jmethodID methodID, va_list args) {
+        return functions->CallNonvirtualLongMethodV(this,obj,clazz,
+                                                    methodID,args);
+    }
+    jlong CallNonvirtualLongMethodA(jobject obj, jclass clazz,
+                                    jmethodID methodID, const jvalue * args) {
+        return functions->CallNonvirtualLongMethodA(this,obj,clazz,
+                                                    methodID,args);
+    }
+
+    jfloat CallNonvirtualFloatMethod(jobject obj, jclass clazz,
+                                     jmethodID methodID, ...) {
+        va_list args;
+        jfloat result;
+        va_start(args,methodID);
+        result = functions->CallNonvirtualFloatMethodV(this,obj,clazz,
+                                                       methodID,args);
+        va_end(args);
+        return result;
+    }
+    jfloat CallNonvirtualFloatMethodV(jobject obj, jclass clazz,
+                                      jmethodID methodID,
+                                      va_list args) {
+        return functions->CallNonvirtualFloatMethodV(this,obj,clazz,
+                                                     methodID,args);
+    }
+    jfloat CallNonvirtualFloatMethodA(jobject obj, jclass clazz,
+                                      jmethodID methodID,
+                                      const jvalue * args) {
+        return functions->CallNonvirtualFloatMethodA(this,obj,clazz,
+                                                     methodID,args);
+    }
+
+    jdouble CallNonvirtualDoubleMethod(jobject obj, jclass clazz,
+                                       jmethodID methodID, ...) {
+        va_list args;
+        jdouble result;
+        va_start(args,methodID);
+        result = functions->CallNonvirtualDoubleMethodV(this,obj,clazz,
+                                                        methodID,args);
+        va_end(args);
+        return result;
+    }
+    jdouble CallNonvirtualDoubleMethodV(jobject obj, jclass clazz,
+                                        jmethodID methodID,
+                                        va_list args) {
+        return functions->CallNonvirtualDoubleMethodV(this,obj,clazz,
+                                                      methodID,args);
+    }
+    jdouble CallNonvirtualDoubleMethodA(jobject obj, jclass clazz,
+                                        jmethodID methodID,
+                                        const jvalue * args) {
+        return functions->CallNonvirtualDoubleMethodA(this,obj,clazz,
+                                                      methodID,args);
+    }
+
+    void CallNonvirtualVoidMethod(jobject obj, jclass clazz,
+                                  jmethodID methodID, ...) {
+        va_list args;
+        va_start(args,methodID);
+        functions->CallNonvirtualVoidMethodV(this,obj,clazz,methodID,args);
+        va_end(args);
+    }
+    void CallNonvirtualVoidMethodV(jobject obj, jclass clazz,
+                                   jmethodID methodID,
+                                   va_list args) {
+        functions->CallNonvirtualVoidMethodV(this,obj,clazz,methodID,args);
+    }
+    void CallNonvirtualVoidMethodA(jobject obj, jclass clazz,
+                                   jmethodID methodID,
+                                   const jvalue * args) {
+        functions->CallNonvirtualVoidMethodA(this,obj,clazz,methodID,args);
+    }
+
+    jfieldID GetFieldID(jclass clazz, const char *name,
+                        const char *sig) {
+        return functions->GetFieldID(this,clazz,name,sig);
+    }
+
+    jobject GetObjectField(jobject obj, jfieldID fieldID) {
+        return functions->GetObjectField(this,obj,fieldID);
+    }
+    jboolean GetBooleanField(jobject obj, jfieldID fieldID) {
+        return functions->GetBooleanField(this,obj,fieldID);
+    }
+    jbyte GetByteField(jobject obj, jfieldID fieldID) {
+        return functions->GetByteField(this,obj,fieldID);
+    }
+    jchar GetCharField(jobject obj, jfieldID fieldID) {
+        return functions->GetCharField(this,obj,fieldID);
+    }
+    jshort GetShortField(jobject obj, jfieldID fieldID) {
+        return functions->GetShortField(this,obj,fieldID);
+    }
+    jint GetIntField(jobject obj, jfieldID fieldID) {
+        return functions->GetIntField(this,obj,fieldID);
+    }
+    jlong GetLongField(jobject obj, jfieldID fieldID) {
+        return functions->GetLongField(this,obj,fieldID);
+    }
+    jfloat GetFloatField(jobject obj, jfieldID fieldID) {
+        return functions->GetFloatField(this,obj,fieldID);
+    }
+    jdouble GetDoubleField(jobject obj, jfieldID fieldID) {
+        return functions->GetDoubleField(this,obj,fieldID);
+    }
+
+    void SetObjectField(jobject obj, jfieldID fieldID, jobject val) {
+        functions->SetObjectField(this,obj,fieldID,val);
+    }
+    void SetBooleanField(jobject obj, jfieldID fieldID,
+                         jboolean val) {
+        functions->SetBooleanField(this,obj,fieldID,val);
+    }
+    void SetByteField(jobject obj, jfieldID fieldID,
+                      jbyte val) {
+        functions->SetByteField(this,obj,fieldID,val);
+    }
+    void SetCharField(jobject obj, jfieldID fieldID,
+                      jchar val) {
+        functions->SetCharField(this,obj,fieldID,val);
+    }
+    void SetShortField(jobject obj, jfieldID fieldID,
+                       jshort val) {
+        functions->SetShortField(this,obj,fieldID,val);
+    }
+    void SetIntField(jobject obj, jfieldID fieldID,
+                     jint val) {
+        functions->SetIntField(this,obj,fieldID,val);
+    }
+    void SetLongField(jobject obj, jfieldID fieldID,
+                      jlong val) {
+        functions->SetLongField(this,obj,fieldID,val);
+    }
+    void SetFloatField(jobject obj, jfieldID fieldID,
+                       jfloat val) {
+        functions->SetFloatField(this,obj,fieldID,val);
+    }
+    void SetDoubleField(jobject obj, jfieldID fieldID,
+                        jdouble val) {
+        functions->SetDoubleField(this,obj,fieldID,val);
+    }
+
+    jmethodID GetStaticMethodID(jclass clazz, const char *name,
+                                const char *sig) {
+        return functions->GetStaticMethodID(this,clazz,name,sig);
+    }
+
+    jobject CallStaticObjectMethod(jclass clazz, jmethodID methodID,
+                             ...) {
+        va_list args;
+        jobject result;
+        va_start(args,methodID);
+        result = functions->CallStaticObjectMethodV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jobject CallStaticObjectMethodV(jclass clazz, jmethodID methodID,
+                              va_list args) {
+        return functions->CallStaticObjectMethodV(this,clazz,methodID,args);
+    }
+    jobject CallStaticObjectMethodA(jclass clazz, jmethodID methodID,
+                              const jvalue *args) {
+        return functions->CallStaticObjectMethodA(this,clazz,methodID,args);
+    }
+
+    jboolean CallStaticBooleanMethod(jclass clazz,
+                                     jmethodID methodID, ...) {
+        va_list args;
+        jboolean result;
+        va_start(args,methodID);
+        result = functions->CallStaticBooleanMethodV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jboolean CallStaticBooleanMethodV(jclass clazz,
+                                      jmethodID methodID, va_list args) {
+        return functions->CallStaticBooleanMethodV(this,clazz,methodID,args);
+    }
+    jboolean CallStaticBooleanMethodA(jclass clazz,
+                                      jmethodID methodID, const jvalue *args) {
+        return functions->CallStaticBooleanMethodA(this,clazz,methodID,args);
+    }
+
+    jbyte CallStaticByteMethod(jclass clazz,
+                               jmethodID methodID, ...) {
+        va_list args;
+        jbyte result;
+        va_start(args,methodID);
+        result = functions->CallStaticByteMethodV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jbyte CallStaticByteMethodV(jclass clazz,
+                                jmethodID methodID, va_list args) {
+        return functions->CallStaticByteMethodV(this,clazz,methodID,args);
+    }
+    jbyte CallStaticByteMethodA(jclass clazz,
+                                jmethodID methodID, const jvalue *args) {
+        return functions->CallStaticByteMethodA(this,clazz,methodID,args);
+    }
+
+    jchar CallStaticCharMethod(jclass clazz,
+                               jmethodID methodID, ...) {
+        va_list args;
+        jchar result;
+        va_start(args,methodID);
+        result = functions->CallStaticCharMethodV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jchar CallStaticCharMethodV(jclass clazz,
+                                jmethodID methodID, va_list args) {
+        return functions->CallStaticCharMethodV(this,clazz,methodID,args);
+    }
+    jchar CallStaticCharMethodA(jclass clazz,
+                                jmethodID methodID, const jvalue *args) {
+        return functions->CallStaticCharMethodA(this,clazz,methodID,args);
+    }
+
+    jshort CallStaticShortMethod(jclass clazz,
+                                 jmethodID methodID, ...) {
+        va_list args;
+        jshort result;
+        va_start(args,methodID);
+        result = functions->CallStaticShortMethodV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jshort CallStaticShortMethodV(jclass clazz,
+                                  jmethodID methodID, va_list args) {
+        return functions->CallStaticShortMethodV(this,clazz,methodID,args);
+    }
+    jshort CallStaticShortMethodA(jclass clazz,
+                                  jmethodID methodID, const jvalue *args) {
+        return functions->CallStaticShortMethodA(this,clazz,methodID,args);
+    }
+
+    jint CallStaticIntMethod(jclass clazz,
+                             jmethodID methodID, ...) {
+        va_list args;
+        jint result;
+        va_start(args,methodID);
+        result = functions->CallStaticIntMethodV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jint CallStaticIntMethodV(jclass clazz,
+                              jmethodID methodID, va_list args) {
+        return functions->CallStaticIntMethodV(this,clazz,methodID,args);
+    }
+    jint CallStaticIntMethodA(jclass clazz,
+                              jmethodID methodID, const jvalue *args) {
+        return functions->CallStaticIntMethodA(this,clazz,methodID,args);
+    }
+
+    jlong CallStaticLongMethod(jclass clazz,
+                               jmethodID methodID, ...) {
+        va_list args;
+        jlong result;
+        va_start(args,methodID);
+        result = functions->CallStaticLongMethodV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jlong CallStaticLongMethodV(jclass clazz,
+                                jmethodID methodID, va_list args) {
+        return functions->CallStaticLongMethodV(this,clazz,methodID,args);
+    }
+    jlong CallStaticLongMethodA(jclass clazz,
+                                jmethodID methodID, const jvalue *args) {
+        return functions->CallStaticLongMethodA(this,clazz,methodID,args);
+    }
+
+    jfloat CallStaticFloatMethod(jclass clazz,
+                                 jmethodID methodID, ...) {
+        va_list args;
+        jfloat result;
+        va_start(args,methodID);
+        result = functions->CallStaticFloatMethodV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jfloat CallStaticFloatMethodV(jclass clazz,
+                                  jmethodID methodID, va_list args) {
+        return functions->CallStaticFloatMethodV(this,clazz,methodID,args);
+    }
+    jfloat CallStaticFloatMethodA(jclass clazz,
+                                  jmethodID methodID, const jvalue *args) {
+        return functions->CallStaticFloatMethodA(this,clazz,methodID,args);
+    }
+
+    jdouble CallStaticDoubleMethod(jclass clazz,
+                                   jmethodID methodID, ...) {
+        va_list args;
+        jdouble result;
+        va_start(args,methodID);
+        result = functions->CallStaticDoubleMethodV(this,clazz,methodID,args);
+        va_end(args);
+        return result;
+    }
+    jdouble CallStaticDoubleMethodV(jclass clazz,
+                                    jmethodID methodID, va_list args) {
+        return functions->CallStaticDoubleMethodV(this,clazz,methodID,args);
+    }
+    jdouble CallStaticDoubleMethodA(jclass clazz,
+                                    jmethodID methodID, const jvalue *args) {
+        return functions->CallStaticDoubleMethodA(this,clazz,methodID,args);
+    }
+
+    void CallStaticVoidMethod(jclass cls, jmethodID methodID, ...) {
+        va_list args;
+        va_start(args,methodID);
+        functions->CallStaticVoidMethodV(this,cls,methodID,args);
+        va_end(args);
+    }
+    void CallStaticVoidMethodV(jclass cls, jmethodID methodID,
+                               va_list args) {
+        functions->CallStaticVoidMethodV(this,cls,methodID,args);
+    }
+    void CallStaticVoidMethodA(jclass cls, jmethodID methodID,
+                               const jvalue * args) {
+        functions->CallStaticVoidMethodA(this,cls,methodID,args);
+    }
+
+    jfieldID GetStaticFieldID(jclass clazz, const char *name,
+                              const char *sig) {
+        return functions->GetStaticFieldID(this,clazz,name,sig);
+    }
+    jobject GetStaticObjectField(jclass clazz, jfieldID fieldID) {
+        return functions->GetStaticObjectField(this,clazz,fieldID);
+    }
+    jboolean GetStaticBooleanField(jclass clazz, jfieldID fieldID) {
+        return functions->GetStaticBooleanField(this,clazz,fieldID);
+    }
+    jbyte GetStaticByteField(jclass clazz, jfieldID fieldID) {
+        return functions->GetStaticByteField(this,clazz,fieldID);
+    }
+    jchar GetStaticCharField(jclass clazz, jfieldID fieldID) {
+        return functions->GetStaticCharField(this,clazz,fieldID);
+    }
+    jshort GetStaticShortField(jclass clazz, jfieldID fieldID) {
+        return functions->GetStaticShortField(this,clazz,fieldID);
+    }
+    jint GetStaticIntField(jclass clazz, jfieldID fieldID) {
+        return functions->GetStaticIntField(this,clazz,fieldID);
+    }
+    jlong GetStaticLongField(jclass clazz, jfieldID fieldID) {
+        return functions->GetStaticLongField(this,clazz,fieldID);
+    }
+    jfloat GetStaticFloatField(jclass clazz, jfieldID fieldID) {
+        return functions->GetStaticFloatField(this,clazz,fieldID);
+    }
+    jdouble GetStaticDoubleField(jclass clazz, jfieldID fieldID) {
+        return functions->GetStaticDoubleField(this,clazz,fieldID);
+    }
+
+    void SetStaticObjectField(jclass clazz, jfieldID fieldID,
+                        jobject value) {
+      functions->SetStaticObjectField(this,clazz,fieldID,value);
+    }
+    void SetStaticBooleanField(jclass clazz, jfieldID fieldID,
+                        jboolean value) {
+      functions->SetStaticBooleanField(this,clazz,fieldID,value);
+    }
+    void SetStaticByteField(jclass clazz, jfieldID fieldID,
+                        jbyte value) {
+      functions->SetStaticByteField(this,clazz,fieldID,value);
+    }
+    void SetStaticCharField(jclass clazz, jfieldID fieldID,
+                        jchar value) {
+      functions->SetStaticCharField(this,clazz,fieldID,value);
+    }
+    void SetStaticShortField(jclass clazz, jfieldID fieldID,
+                        jshort value) {
+      functions->SetStaticShortField(this,clazz,fieldID,value);
+    }
+    void SetStaticIntField(jclass clazz, jfieldID fieldID,
+                        jint value) {
+      functions->SetStaticIntField(this,clazz,fieldID,value);
+    }
+    void SetStaticLongField(jclass clazz, jfieldID fieldID,
+                        jlong value) {
+      functions->SetStaticLongField(this,clazz,fieldID,value);
+    }
+    void SetStaticFloatField(jclass clazz, jfieldID fieldID,
+                        jfloat value) {
+      functions->SetStaticFloatField(this,clazz,fieldID,value);
+    }
+    void SetStaticDoubleField(jclass clazz, jfieldID fieldID,
+                        jdouble value) {
+      functions->SetStaticDoubleField(this,clazz,fieldID,value);
+    }
+
+    jstring NewString(const jchar *unicode, jsize len) {
+        return functions->NewString(this,unicode,len);
+    }
+    jsize GetStringLength(jstring str) {
+        return functions->GetStringLength(this,str);
+    }
+    const jchar *GetStringChars(jstring str, jboolean *isCopy) {
+        return functions->GetStringChars(this,str,isCopy);
+    }
+    void ReleaseStringChars(jstring str, const jchar *chars) {
+        functions->ReleaseStringChars(this,str,chars);
+    }
+
+    jstring NewStringUTF(const char *utf) {
+        return functions->NewStringUTF(this,utf);
+    }
+    jsize GetStringUTFLength(jstring str) {
+        return functions->GetStringUTFLength(this,str);
+    }
+    const char* GetStringUTFChars(jstring str, jboolean *isCopy) {
+        return functions->GetStringUTFChars(this,str,isCopy);
+    }
+    void ReleaseStringUTFChars(jstring str, const char* chars) {
+        functions->ReleaseStringUTFChars(this,str,chars);
+    }
+
+    jsize GetArrayLength(jarray array) {
+        return functions->GetArrayLength(this,array);
+    }
+
+    jobjectArray NewObjectArray(jsize len, jclass clazz,
+                                jobject init) {
+        return functions->NewObjectArray(this,len,clazz,init);
+    }
+    jobject GetObjectArrayElement(jobjectArray array, jsize index) {
+        return functions->GetObjectArrayElement(this,array,index);
+    }
+    void SetObjectArrayElement(jobjectArray array, jsize index,
+                               jobject val) {
+        functions->SetObjectArrayElement(this,array,index,val);
+    }
+
+    jbooleanArray NewBooleanArray(jsize len) {
+        return functions->NewBooleanArray(this,len);
+    }
+    jbyteArray NewByteArray(jsize len) {
+        return functions->NewByteArray(this,len);
+    }
+    jcharArray NewCharArray(jsize len) {
+        return functions->NewCharArray(this,len);
+    }
+    jshortArray NewShortArray(jsize len) {
+        return functions->NewShortArray(this,len);
+    }
+    jintArray NewIntArray(jsize len) {
+        return functions->NewIntArray(this,len);
+    }
+    jlongArray NewLongArray(jsize len) {
+        return functions->NewLongArray(this,len);
+    }
+    jfloatArray NewFloatArray(jsize len) {
+        return functions->NewFloatArray(this,len);
+    }
+    jdoubleArray NewDoubleArray(jsize len) {
+        return functions->NewDoubleArray(this,len);
+    }
+
+    jboolean * GetBooleanArrayElements(jbooleanArray array, jboolean *isCopy) {
+        return functions->GetBooleanArrayElements(this,array,isCopy);
+    }
+    jbyte * GetByteArrayElements(jbyteArray array, jboolean *isCopy) {
+        return functions->GetByteArrayElements(this,array,isCopy);
+    }
+    jchar * GetCharArrayElements(jcharArray array, jboolean *isCopy) {
+        return functions->GetCharArrayElements(this,array,isCopy);
+    }
+    jshort * GetShortArrayElements(jshortArray array, jboolean *isCopy) {
+        return functions->GetShortArrayElements(this,array,isCopy);
+    }
+    jint * GetIntArrayElements(jintArray array, jboolean *isCopy) {
+        return functions->GetIntArrayElements(this,array,isCopy);
+    }
+    jlong * GetLongArrayElements(jlongArray array, jboolean *isCopy) {
+        return functions->GetLongArrayElements(this,array,isCopy);
+    }
+    jfloat * GetFloatArrayElements(jfloatArray array, jboolean *isCopy) {
+        return functions->GetFloatArrayElements(this,array,isCopy);
+    }
+    jdouble * GetDoubleArrayElements(jdoubleArray array, jboolean *isCopy) {
+        return functions->GetDoubleArrayElements(this,array,isCopy);
+    }
+
+    void ReleaseBooleanArrayElements(jbooleanArray array,
+                                     jboolean *elems,
+                                     jint mode) {
+        functions->ReleaseBooleanArrayElements(this,array,elems,mode);
+    }
+    void ReleaseByteArrayElements(jbyteArray array,
+                                  jbyte *elems,
+                                  jint mode) {
+        functions->ReleaseByteArrayElements(this,array,elems,mode);
+    }
+    void ReleaseCharArrayElements(jcharArray array,
+                                  jchar *elems,
+                                  jint mode) {
+        functions->ReleaseCharArrayElements(this,array,elems,mode);
+    }
+    void ReleaseShortArrayElements(jshortArray array,
+                                   jshort *elems,
+                                   jint mode) {
+        functions->ReleaseShortArrayElements(this,array,elems,mode);
+    }
+    void ReleaseIntArrayElements(jintArray array,
+                                 jint *elems,
+                                 jint mode) {
+        functions->ReleaseIntArrayElements(this,array,elems,mode);
+    }
+    void ReleaseLongArrayElements(jlongArray array,
+                                  jlong *elems,
+                                  jint mode) {
+        functions->ReleaseLongArrayElements(this,array,elems,mode);
+    }
+    void ReleaseFloatArrayElements(jfloatArray array,
+                                   jfloat *elems,
+                                   jint mode) {
+        functions->ReleaseFloatArrayElements(this,array,elems,mode);
+    }
+    void ReleaseDoubleArrayElements(jdoubleArray array,
+                                    jdouble *elems,
+                                    jint mode) {
+        functions->ReleaseDoubleArrayElements(this,array,elems,mode);
+    }
+
+    void GetBooleanArrayRegion(jbooleanArray array,
+                               jsize start, jsize len, jboolean *buf) {
+        functions->GetBooleanArrayRegion(this,array,start,len,buf);
+    }
+    void GetByteArrayRegion(jbyteArray array,
+                            jsize start, jsize len, jbyte *buf) {
+        functions->GetByteArrayRegion(this,array,start,len,buf);
+    }
+    void GetCharArrayRegion(jcharArray array,
+                            jsize start, jsize len, jchar *buf) {
+        functions->GetCharArrayRegion(this,array,start,len,buf);
+    }
+    void GetShortArrayRegion(jshortArray array,
+                             jsize start, jsize len, jshort *buf) {
+        functions->GetShortArrayRegion(this,array,start,len,buf);
+    }
+    void GetIntArrayRegion(jintArray array,
+                           jsize start, jsize len, jint *buf) {
+        functions->GetIntArrayRegion(this,array,start,len,buf);
+    }
+    void GetLongArrayRegion(jlongArray array,
+                            jsize start, jsize len, jlong *buf) {
+        functions->GetLongArrayRegion(this,array,start,len,buf);
+    }
+    void GetFloatArrayRegion(jfloatArray array,
+                             jsize start, jsize len, jfloat *buf) {
+        functions->GetFloatArrayRegion(this,array,start,len,buf);
+    }
+    void GetDoubleArrayRegion(jdoubleArray array,
+                              jsize start, jsize len, jdouble *buf) {
+        functions->GetDoubleArrayRegion(this,array,start,len,buf);
+    }
+
+    void SetBooleanArrayRegion(jbooleanArray array, jsize start, jsize len,
+                               const jboolean *buf) {
+        functions->SetBooleanArrayRegion(this,array,start,len,buf);
+    }
+    void SetByteArrayRegion(jbyteArray array, jsize start, jsize len,
+                            const jbyte *buf) {
+        functions->SetByteArrayRegion(this,array,start,len,buf);
+    }
+    void SetCharArrayRegion(jcharArray array, jsize start, jsize len,
+                            const jchar *buf) {
+        functions->SetCharArrayRegion(this,array,start,len,buf);
+    }
+    void SetShortArrayRegion(jshortArray array, jsize start, jsize len,
+                             const jshort *buf) {
+        functions->SetShortArrayRegion(this,array,start,len,buf);
+    }
+    void SetIntArrayRegion(jintArray array, jsize start, jsize len,
+                           const jint *buf) {
+        functions->SetIntArrayRegion(this,array,start,len,buf);
+    }
+    void SetLongArrayRegion(jlongArray array, jsize start, jsize len,
+                            const jlong *buf) {
+        functions->SetLongArrayRegion(this,array,start,len,buf);
+    }
+    void SetFloatArrayRegion(jfloatArray array, jsize start, jsize len,
+                             const jfloat *buf) {
+        functions->SetFloatArrayRegion(this,array,start,len,buf);
+    }
+    void SetDoubleArrayRegion(jdoubleArray array, jsize start, jsize len,
+                              const jdouble *buf) {
+        functions->SetDoubleArrayRegion(this,array,start,len,buf);
+    }
+
+    jint RegisterNatives(jclass clazz, const JNINativeMethod *methods,
+                         jint nMethods) {
+        return functions->RegisterNatives(this,clazz,methods,nMethods);
+    }
+    jint UnregisterNatives(jclass clazz) {
+        return functions->UnregisterNatives(this,clazz);
+    }
+
+    jint MonitorEnter(jobject obj) {
+        return functions->MonitorEnter(this,obj);
+    }
+    jint MonitorExit(jobject obj) {
+        return functions->MonitorExit(this,obj);
+    }
+
+    jint GetJavaVM(JavaVM **vm) {
+        return functions->GetJavaVM(this,vm);
+    }
+
+    void GetStringRegion(jstring str, jsize start, jsize len, jchar *buf) {
+        functions->GetStringRegion(this,str,start,len,buf);
+    }
+    void GetStringUTFRegion(jstring str, jsize start, jsize len, char *buf) {
+        functions->GetStringUTFRegion(this,str,start,len,buf);
+    }
+
+    void * GetPrimitiveArrayCritical(jarray array, jboolean *isCopy) {
+        return functions->GetPrimitiveArrayCritical(this,array,isCopy);
+    }
+    void ReleasePrimitiveArrayCritical(jarray array, void *carray, jint mode) {
+        functions->ReleasePrimitiveArrayCritical(this,array,carray,mode);
+    }
+
+    const jchar * GetStringCritical(jstring string, jboolean *isCopy) {
+        return functions->GetStringCritical(this,string,isCopy);
+    }
+    void ReleaseStringCritical(jstring string, const jchar *cstring) {
+        functions->ReleaseStringCritical(this,string,cstring);
+    }
+
+    jweak NewWeakGlobalRef(jobject obj) {
+        return functions->NewWeakGlobalRef(this,obj);
+    }
+    void DeleteWeakGlobalRef(jweak ref) {
+        functions->DeleteWeakGlobalRef(this,ref);
+    }
+
+    jboolean ExceptionCheck() {
+        return functions->ExceptionCheck(this);
+    }
+
+    jobject NewDirectByteBuffer(void* address, jlong capacity) {
+        return functions->NewDirectByteBuffer(this, address, capacity);
+    }
+    void* GetDirectBufferAddress(jobject buf) {
+        return functions->GetDirectBufferAddress(this, buf);
+    }
+    jlong GetDirectBufferCapacity(jobject buf) {
+        return functions->GetDirectBufferCapacity(this, buf);
+    }
+    jobjectRefType GetObjectRefType(jobject obj) {
+        return functions->GetObjectRefType(this, obj);
+    }
+
+#endif /* __cplusplus */
+};
+
+typedef struct JavaVMOption {
+    char *optionString;
+    void *extraInfo;
+} JavaVMOption;
+
+typedef struct JavaVMInitArgs {
+    jint version;
+
+    jint nOptions;
+    JavaVMOption *options;
+    jboolean ignoreUnrecognized;
+} JavaVMInitArgs;
+
+typedef struct JavaVMAttachArgs {
+    jint version;
+
+    char *name;
+    jobject group;
+} JavaVMAttachArgs;
+
+/* These will be VM-specific. */
+
+#define JDK1_2
+#define JDK1_4
+
+/* End VM-specific. */
+
+struct JNIInvokeInterface_ {
+    void *reserved0;
+    void *reserved1;
+    void *reserved2;
+
+    jint (JNICALL *DestroyJavaVM)(JavaVM *vm);
+
+    jint (JNICALL *AttachCurrentThread)(JavaVM *vm, void **penv, void *args);
+
+    jint (JNICALL *DetachCurrentThread)(JavaVM *vm);
+
+    jint (JNICALL *GetEnv)(JavaVM *vm, void **penv, jint version);
+
+    jint (JNICALL *AttachCurrentThreadAsDaemon)(JavaVM *vm, void **penv, void *args);
+};
+
+struct JavaVM_ {
+    const struct JNIInvokeInterface_ *functions;
+#ifdef __cplusplus
+
+    jint DestroyJavaVM() {
+        return functions->DestroyJavaVM(this);
+    }
+    jint AttachCurrentThread(void **penv, void *args) {
+        return functions->AttachCurrentThread(this, penv, args);
+    }
+    jint DetachCurrentThread() {
+        return functions->DetachCurrentThread(this);
+    }
+
+    jint GetEnv(void **penv, jint version) {
+        return functions->GetEnv(this, penv, version);
+    }
+    jint AttachCurrentThreadAsDaemon(void **penv, void *args) {
+        return functions->AttachCurrentThreadAsDaemon(this, penv, args);
+    }
+#endif
+};
+
+#ifdef _JNI_IMPLEMENTATION_
+#define _JNI_IMPORT_OR_EXPORT_ JNIEXPORT
+#else
+#define _JNI_IMPORT_OR_EXPORT_ JNIIMPORT
+#endif
+_JNI_IMPORT_OR_EXPORT_ jint JNICALL
+JNI_GetDefaultJavaVMInitArgs(void *args);
+
+_JNI_IMPORT_OR_EXPORT_ jint JNICALL
+JNI_CreateJavaVM(JavaVM **pvm, void **penv, void *args);
+
+_JNI_IMPORT_OR_EXPORT_ jint JNICALL
+JNI_GetCreatedJavaVMs(JavaVM **, jsize, jsize *);
+
+/* Defined by native libraries. */
+JNIEXPORT jint JNICALL
+JNI_OnLoad(JavaVM *vm, void *reserved);
+
+JNIEXPORT void JNICALL
+JNI_OnUnload(JavaVM *vm, void *reserved);
+
+#define JNI_VERSION_1_1 0x00010001
+#define JNI_VERSION_1_2 0x00010002
+#define JNI_VERSION_1_4 0x00010004
+#define JNI_VERSION_1_6 0x00010006
+#define JNI_VERSION_1_8 0x00010008
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+
+#endif /* !_JAVASOFT_JNI_H_ */
diff --git a/java/arm-linux/linux/jni_md.h b/java/arm-linux/linux/jni_md.h
new file mode 100644
index 0000000..80eedf3
--- /dev/null
+++ b/java/arm-linux/linux/jni_md.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 1996, 2013, Oracle and/or its affiliates. All rights reserved.
+ * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
+ *
+ * This code is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 only, as
+ * published by the Free Software Foundation.  Oracle designates this
+ * particular file as subject to the "Classpath" exception as provided
+ * by Oracle in the LICENSE file that accompanied this code.
+ *
+ * This code 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
+ * version 2 for more details (a copy is included in the LICENSE file that
+ * accompanied this code).
+ *
+ * You should have received a copy of the GNU General Public License version
+ * 2 along with this work; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Please contact Oracle, 500 Oracle Parkway, Redwood Shores, CA 94065 USA
+ * or visit www.oracle.com if you need additional information or have any
+ * questions.
+ */
+
+#ifndef _JAVASOFT_JNI_MD_H_
+#define _JAVASOFT_JNI_MD_H_
+
+#ifndef __has_attribute
+  #define __has_attribute(x) 0
+#endif
+#if (defined(__GNUC__) && ((__GNUC__ > 4) || (__GNUC__ == 4) && (__GNUC_MINOR__ > 2))) || __has_attribute(visibility)
+  #define JNIEXPORT     __attribute__((visibility("default")))
+  #define JNIIMPORT     __attribute__((visibility("default")))
+#else
+  #define JNIEXPORT
+  #define JNIIMPORT
+#endif
+
+#define JNICALL
+
+typedef int jint;
+#ifdef _LP64 /* 64-bit Solaris */
+typedef long jlong;
+#else
+typedef long long jlong;
+#endif
+
+typedef signed char jbyte;
+
+#endif /* !_JAVASOFT_JNI_MD_H_ */
diff --git a/java/build.properties b/java/build.properties
new file mode 100644
index 0000000..d17072e
--- /dev/null
+++ b/java/build.properties
@@ -0,0 +1,7 @@
+# Project specific information
+package=com.ctre
+robot.class=${package}.Robot
+simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world
+#Uncomment and point at user libraries to include them in the build. Do not put libraries in the \wpilib\java folder, this folder is completely overwritten on plugin update.
+userLibs=C:/temp/ecl
+#userLibs=c:\temp #${user.home}/wpilib/user/lib
\ No newline at end of file
diff --git a/java/build.xml b/java/build.xml
new file mode 100644
index 0000000..40c5c4f
--- /dev/null
+++ b/java/build.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<project name="FRC Deployment" default="deploy">
+
+	<!-- The following properties can be defined to override system level settings. 
+		These should not be touched unless you know what you're doing. The primary 
+		use is to override the wpilib version when working with older robots that 
+		can't compile with the latest libraries. -->
+
+	<!-- By default the system version of WPI is used -->
+	<!-- <property name="version" value=""/> -->
+
+	<!-- By default the system team number is used -->
+	<!-- <property name="team-number" value=""/> -->
+
+	<!-- By default the target is set to 10.TE.AM.2 -->
+	<!-- <property name="target" value=""/> -->
+
+	<!-- Any other property in build.properties can also be overridden. -->
+
+	<property file="${user.home}/wpilib/wpilib.properties" />
+	<property file="build.properties" />
+	<property file="${user.home}/wpilib/java/${version}/ant/build.properties" />
+
+	<import file="${wpilib.ant.dir}/build.xml" />
+
+</project> 
diff --git a/java/java.gradle b/java/java.gradle
new file mode 100644
index 0000000..6ba893f
--- /dev/null
+++ b/java/java.gradle
@@ -0,0 +1,135 @@
+import org.gradle.internal.os.OperatingSystem
+
+apply plugin: 'java'
+apply plugin: 'net.ltgt.errorprone'
+
+repositories {
+    mavenCentral()
+}
+
+configurations.errorprone {
+    resolutionStrategy.force 'com.google.errorprone:error_prone_core:2.0.9'
+}
+
+def generatedJNIHeaderLoc = "${buildDir}/include"
+
+sourceSets {
+    user {
+        java {
+            srcDirs = [userJavaSrc]
+        }
+    }
+    main {
+        java {
+            srcDirs = [javaSrc]
+        }
+    }
+}
+    
+dependencies {
+    compile sourceSets.user.output
+    runtime sourceSets.user.output
+    compile wpilibjDep
+    runtime wpilibjDep
+    compile javaNetTablesDep
+    runtime javaNetTablesDep
+    compile fileTree(dir: javaLibraryLoc, include: ['*.jar'])
+    runtime fileTree(dir: javaLibraryLoc, include: ['*.jar'])
+}
+
+jar {
+    description = 'Generates jar'
+    baseName = libraryName
+    duplicatesStrategy = 'exclude'
+
+    dependsOn { classes }
+
+    if (embedJavaLibraries) {
+        def tree = fileTree(dir: javaLibraryLoc, include: ['*.jar'])
+        tree.each {
+            from zipTree(it.path)
+        }
+    }
+
+    if (embedJavaLibraries) {
+        from sourceSets.user.output
+    }
+}
+
+/**
+ * Generates the JNI headers
+ */
+task jniHeaders {
+    description = 'Generates JNI headers'
+    group = 'WPILib'
+    def outputFolder = file(generatedJNIHeaderLoc)
+    inputs.files sourceSets.main.output
+    outputs.file outputFolder
+    doLast {
+        outputFolder.mkdirs()
+        exec {
+            ignoreExitValue = true
+            executable org.gradle.internal.jvm.Jvm.current().getExecutable('javah')
+            args '-d', outputFolder
+            args '-classpath', sourceSets.main.runtimeClasspath.asPath
+            args jniDefinitions
+        }
+    }
+}
+
+ext.getNativeJNISymbols = {
+    def symbolsList = []
+
+    jniHeaders.outputs.files.each {
+        FileTree tree = fileTree(dir: it)
+        tree.each { File file ->
+            file.eachLine { line ->
+                if (line.trim()) {
+                    if (line.startsWith("JNIEXPORT ") && line.contains('JNICALL')) {
+                        def (p1, p2) = line.split('JNICALL').collect { it.trim() }
+                        // p2 is our JNI call
+                        symbolsList << p2
+                    }
+                }
+            }
+        }
+    }
+
+    return symbolsList
+}
+
+clean {
+    delete generatedJNIHeaderLoc
+}
+
+compileJava {
+    options.compilerArgs << '-Xlint:unchecked' << "-Werror"
+}
+
+javadoc {
+    options.addStringOption('Xdoclint:none', '-quiet')
+}
+
+// This creates a lambda that the main build.gradle can access, which sets up the JNI includes for the
+// target build platform. This lambda is exposed as a property in the main build.gradle.
+ext.setupJniIncludes = { binaries ->
+    def platformSpecificIncludeFlag = { loc, cppCompiler ->
+        if (OperatingSystem.current().isWindows()) {
+            cppCompiler.args "/I$loc"
+        } else {
+            cppCompiler.args '-I', loc
+        }
+    }
+    binaries.all {
+        tasks.withType(CppCompile) {
+            cppCompiler.args '-I', file("${rootDir}/java/arm-linux").absolutePath
+            cppCompiler.args '-I', file("${rootDir}/java/arm-linux/linux").absolutePath
+
+            jniHeaders.outputs.files.each { file ->
+                cppCompiler.args '-I', file.getPath()
+            }
+
+            dependsOn jniHeaders
+        }
+    }
+}
diff --git a/java/src/com/ctre/CANTalon.java b/java/src/com/ctre/CANTalon.java
new file mode 100644
index 0000000..860c627
--- /dev/null
+++ b/java/src/com/ctre/CANTalon.java
@@ -0,0 +1,12 @@
+package com.ctre;
+
+/**
+*  @deprecated Use TalonSRX instead. This deprecated class will be removed in 2019.
+*  @see <a href="https://github.com/CrossTheRoadElec/Phoenix-Documentation/blob/master/Migration%20Guide.md">Phoenix 2018 Migration Guide</a>
+*/
+@Deprecated
+public class CANTalon extends com.ctre.phoenix.motorcontrol.can.TalonSRX {
+	public CANTalon(int deviceNumber) {
+		super(deviceNumber);
+	}
+}
diff --git a/java/src/com/ctre/phoenix/ButtonMonitor.java b/java/src/com/ctre/phoenix/ButtonMonitor.java
new file mode 100644
index 0000000..936fb7a
--- /dev/null
+++ b/java/src/com/ctre/phoenix/ButtonMonitor.java
@@ -0,0 +1,43 @@
+package com.ctre.phoenix;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+public class ButtonMonitor implements ILoopable
+{
+	GenericHID  _gameCntrlr;
+	int _btnIdx;
+	IButtonPressEventHandler  _handler;
+	boolean _isDown = false;
+	
+	public interface IButtonPressEventHandler {
+		void OnButtonPress(int idx, boolean isDown);
+	};
+
+	public ButtonMonitor(GenericHID controller, int buttonIndex,
+			IButtonPressEventHandler ButtonPressEventHandler) {
+		_gameCntrlr = controller;
+		_btnIdx = buttonIndex;
+		_handler = ButtonPressEventHandler;
+	}
+
+	public void process() {
+		boolean down = _gameCntrlr.getRawButton(_btnIdx);
+
+		if (!_isDown && down){
+			_handler.OnButtonPress(_btnIdx, down);
+		}
+
+		_isDown = down;
+	}
+
+	public void onStart() {
+	}
+	public void onLoop() {
+		process();
+	}
+	public boolean isDone() {
+		return false;
+	}
+	public void onStop() {
+	}
+}
diff --git a/java/src/com/ctre/phoenix/CANifier.java b/java/src/com/ctre/phoenix/CANifier.java
new file mode 100644
index 0000000..31d33ce
--- /dev/null
+++ b/java/src/com/ctre/phoenix/CANifier.java
@@ -0,0 +1,616 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ *
+ * Cross The Road Electronics (CTRE) licenses to you the right to
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ *
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+
+package com.ctre.phoenix;
+
+import edu.wpi.first.wpilibj.hal.HAL;
+
+/**
+ * CTRE CANifier
+ *
+ * Device for interfacing common devices to the CAN bus.
+ */
+public class CANifier {
+	private long m_handle;
+
+	/**
+	 * Enum for velocity periods
+	 */
+	public enum VelocityPeriod {
+		Period_1Ms(1),
+		Period_2Ms(2),
+		Period_5Ms(5),
+		Period_10Ms(10),
+		Period_20Ms(20),
+		Period_25Ms(25),
+		Period_50Ms(50),
+		Period_100Ms(100);
+		public static VelocityPeriod valueOf(int value) {
+			for(VelocityPeriod period : values()) {
+				if(period.value == value) {
+					return period;
+				}
+			}
+			return null;
+		}
+		public final int value;
+		VelocityPeriod(int initValue) {
+			this.value = initValue;
+		}
+	}
+	
+	/**
+	 * Enum for the LED Output Channels
+	 */
+	public enum LEDChannel {
+		LEDChannelA(0), LEDChannelB(1), LEDChannelC(2);
+		public static LEDChannel valueOf(int value) {
+			for (LEDChannel mode : values()) {
+				if (mode.value == value) {
+					return mode;
+				}
+			}
+			return null;
+		}
+
+		public final int value;
+
+		LEDChannel(int initValue) {
+			this.value = initValue;
+		}
+	}
+
+	/**
+	 * Enum for the PWM Input Channels
+	 */
+	public enum PWMChannel {
+		PWMChannel0(0), PWMChannel1(1), PWMChannel2(2), PWMChannel3(3);
+		public static PWMChannel valueOf(int value) {
+			for (PWMChannel mode : values()) {
+				if (mode.value == value) {
+					return mode;
+				}
+			}
+			return null;
+		}
+
+		public final int value;
+
+		PWMChannel(int initValue) {
+			this.value = initValue;
+		}
+	}
+
+	public final int PWMChannelCount = 4;
+
+	/**
+	 * General IO Pins on the CANifier
+	 */
+	public enum GeneralPin {
+		QUAD_IDX (CANifierJNI.GeneralPin.QUAD_IDX.value),
+		QUAD_B (CANifierJNI.GeneralPin.QUAD_B.value),
+		QUAD_A (CANifierJNI.GeneralPin.QUAD_A.value),
+		LIMR (CANifierJNI.GeneralPin.LIMR.value),
+		LIMF (CANifierJNI.GeneralPin.LIMF.value),
+		SDA (CANifierJNI.GeneralPin.SDA.value),
+		SCL (CANifierJNI.GeneralPin.SCL.value),
+		SPI_CS (CANifierJNI.GeneralPin.SPI_CS.value),
+		SPI_MISO_PWM2P (CANifierJNI.GeneralPin.SPI_MISO_PWM2P.value),
+		SPI_MOSI_PWM1P (CANifierJNI.GeneralPin.SPI_MOSI_PWM1P.value),
+		SPI_CLK_PWM0P (CANifierJNI.GeneralPin.SPI_CLK_PWM0P.value);
+		public static GeneralPin valueOf(int value) {
+			for (GeneralPin mode : values()) {
+				if (mode.value == value) {
+					return mode;
+				}
+			}
+			return null;
+		}
+
+		public final int value;
+
+		GeneralPin(int initValue) {
+			this.value = initValue;
+		}
+	}
+
+	/**
+	 * Class to hold the pin values.
+	 */
+	public static class PinValues {
+		public boolean QUAD_IDX;
+		public boolean QUAD_B;
+		public boolean QUAD_A;
+		public boolean LIMR;
+		public boolean LIMF;
+		public boolean SDA;
+		public boolean SCL;
+		public boolean SPI_CS_PWM3;
+		public boolean SPI_MISO_PWM2;
+		public boolean SPI_MOSI_PWM1;
+		public boolean SPI_CLK_PWM0;
+	}
+
+	private boolean[] _tempPins = new boolean[11];
+
+	private int m_deviceNumber;
+	/**
+	 * Constructor.
+	 * @param deviceId	The CAN Device ID of the CANifier.
+	 */
+	public CANifier(int deviceId) {
+		m_handle = CANifierJNI.JNI_new_CANifier(deviceId);
+		m_deviceNumber = deviceId;
+		HAL.report(63, deviceId + 1);
+	}
+
+	/**
+	 * Sets the LED Output
+	 * @param percentOutput Output duty cycle expressed as percentage.
+	 * @param ledChannel 		Channel to set the output of.
+	 */
+	public void setLEDOutput(double percentOutput, LEDChannel ledChannel) {
+		/* convert float to integral fixed pt */
+		if (percentOutput > 1) {
+			percentOutput = 1;
+		}
+		if (percentOutput < 0) {
+			percentOutput = 0;
+		}
+		int dutyCycle = (int) (percentOutput * 1023); // [0,1023]
+
+		CANifierJNI.JNI_SetLEDOutput(m_handle, dutyCycle, ledChannel.value);
+	}
+
+	/**
+	 * Sets the output of a General Pin
+	 * @param outputPin 		The pin to use as output.
+	 * @param outputValue 	The desired output state.
+	 * @param outputEnable	Whether this pin is an output. "True" enables output.
+	 */
+	public void setGeneralOutput(GeneralPin outputPin, boolean outputValue, boolean outputEnable) {
+		CANifierJNI.JNI_SetGeneralOutput(m_handle, outputPin.value, outputValue, outputEnable);
+	}
+
+	/**
+	 * Sets the output of all General Pins
+	 * @param outputBits 	A bit mask of all the output states.  LSB->MSB is in the order of the #GeneralPin enum.
+	 * @param isOutputBits A boolean bit mask that sets the pins to be outputs or inputs.  A bit of 1 enables output.
+	 */
+	public void setGeneralOutputs(int outputBits, int isOutputBits) {
+		CANifierJNI.JNI_SetGeneralOutputs(m_handle, outputBits, isOutputBits);
+	}
+
+	/**
+	 * Gets the state of all General Pins
+	 * @param allPins A structure to fill with the current state of all pins.
+	 */
+	public void getGeneralInputs(PinValues allPins) {
+		CANifierJNI.JNI_GetGeneralInputs(m_handle, _tempPins);
+		allPins.LIMF = _tempPins[GeneralPin.LIMF.value];
+		allPins.LIMR = _tempPins[GeneralPin.LIMR.value];
+		allPins.QUAD_A = _tempPins[GeneralPin.QUAD_A.value];
+		allPins.QUAD_B = _tempPins[GeneralPin.QUAD_B.value];
+		allPins.QUAD_IDX = _tempPins[GeneralPin.QUAD_IDX.value];
+		allPins.SCL = _tempPins[GeneralPin.SCL.value];
+		allPins.SDA = _tempPins[GeneralPin.SDA.value];
+		allPins.SPI_CLK_PWM0 = _tempPins[GeneralPin.SPI_CLK_PWM0P.value];
+		allPins.SPI_MOSI_PWM1 = _tempPins[GeneralPin.SPI_MOSI_PWM1P.value];
+		allPins.SPI_MISO_PWM2 = _tempPins[GeneralPin.SPI_MISO_PWM2P.value];
+		allPins.SPI_CS_PWM3 = _tempPins[GeneralPin.SPI_CS.value];
+	}
+
+	/**
+	 * Gets the state of the specified pin
+	 * @param inputPin  The index of the pin.
+	 * @return The state of the pin.
+	 */
+	public boolean getGeneralInput(GeneralPin inputPin) {
+		return CANifierJNI.JNI_GetGeneralInput(m_handle, inputPin.value);
+	}
+
+	/**
+	 * Call GetLastError() generated by this object.
+	 * Not all functions return an error code but can
+	 * potentially report errors.
+	 *
+	 * This function can be used to retrieve those error codes.
+	 *
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode getLastError() {
+		int retval = CANifierJNI.JNI_GetLastError(m_handle);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the PWM Output
+	 * Currently supports PWM 0, PWM 1, and PWM 2
+	 * @param pwmChannel  Index of the PWM channel to output.
+	 * @param dutyCycle   Duty Cycle (0 to 1) to output.  Default period of the signal is 4.2 ms.
+	 */
+	public void setPWMOutput(int pwmChannel, double dutyCycle) {
+		if (dutyCycle < 0) {
+			dutyCycle = 0;
+		} else if (dutyCycle > 1) {
+			dutyCycle = 1;
+		}
+		if (pwmChannel < 0) {
+			pwmChannel = 0;
+		}
+
+		int dutyCyc10bit = (int) (1023 * dutyCycle);
+
+		CANifierJNI.JNI_SetPWMOutput(m_handle, (int) pwmChannel, dutyCyc10bit);
+	}
+
+	/**
+	 * Enables PWM Outputs
+	 * Currently supports PWM 0, PWM 1, and PWM 2
+	 * @param pwmChannel  Index of the PWM channel to enable.
+	 * @param bEnable			"True" enables output on the pwm channel.
+	 */
+	public void enablePWMOutput(int pwmChannel, boolean bEnable) {
+		if (pwmChannel < 0) {
+			pwmChannel = 0;
+		}
+
+		CANifierJNI.JNI_EnablePWMOutput(m_handle, (int) pwmChannel, bEnable);
+	}
+
+	/**
+	 * Gets the PWM Input
+	 * @param pwmChannel  PWM channel to get.
+	 * @param dutyCycleAndPeriod	Double array to hold Duty Cycle [0] and Period [1].
+	 */
+	public void getPWMInput(PWMChannel pwmChannel, double[] dutyCycleAndPeriod) {
+		CANifierJNI.JNI_GetPWMInput(m_handle, pwmChannel.value, dutyCycleAndPeriod);
+	}
+	
+	/**
+	 * Gets the quadrature encoder's position
+	 * @return Position of encoder 
+	 */
+	public int getQuadraturePosition() {
+		return CANifierJNI.JNI_GetQuadraturePosition(m_handle);
+	}
+	
+	/**
+	 * Sets the quadrature encoder's position
+	 * @param newPosition  Position to set
+	 * @param timeoutMs  
+					Timeout value in ms. If nonzero, function will wait for
+					config success and report an error if it times out.
+					If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setQuadraturePosition(int newPosition, int timeoutMs) {
+		return ErrorCode.valueOf(CANifierJNI.JNI_SetQuadraturePosition(m_handle, newPosition, timeoutMs));
+	}
+	
+	/**
+	 * Gets the quadrature encoder's velocity
+	 * @return Velocity of encoder
+	 */
+	public int getQuadratureVelocity() {
+		return CANifierJNI.JNI_GetQuadratureVelocity(m_handle);
+	}
+	
+	/**
+	 * Configures the period of each velocity sample.
+	 * Every 1ms a position value is sampled, and the delta between that sample
+	 * and the position sampled kPeriod ms ago is inserted into a filter.
+	 * kPeriod is configured with this function.
+	 *
+	 * @param period
+	 *            Desired period for the velocity measurement. @see
+	 *            #VelocityMeasPeriod
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configVelocityMeasurementPeriod(VelocityPeriod period, int timeoutMs) {
+		int retval = CANifierJNI.JNI_ConfigVelocityMeasurementPeriod(m_handle, period.value, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	
+	/**
+	 * Sets the number of velocity samples used in the rolling average velocity
+	 * measurement.
+	 *
+	 * @param windowSize
+	 *            Number of samples in the rolling average of velocity
+	 *            measurement. Valid values are 1,2,4,8,16,32. If another
+	 *            value is specified, it will truncate to nearest support value.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configVelocityMeasurementWindow(int windowSize, int timeoutMs) {
+		int retval = CANifierJNI.JNI_ConfigVelocityMeasurementWindow(m_handle, windowSize, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the value of a custom parameter. This is for arbitrary use.
+   *
+   * Sometimes it is necessary to save calibration/duty cycle/output
+   * information in the device. Particularly if the
+   * device is part of a subsystem that can be replaced.
+	 *
+	 * @param newValue
+	 *            Value for custom parameter.
+	 * @param paramIndex
+	 *            Index of custom parameter. [0-1]
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSetCustomParam(int newValue, int paramIndex, int timeoutMs) {
+		int retval = CANifierJNI.JNI_ConfigSetCustomParam(m_handle, newValue, paramIndex, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Gets the value of a custom parameter. This is for arbitrary use.
+   *
+   * Sometimes it is necessary to save calibration/duty cycle/output
+   * information in the device. Particularly if the
+   * device is part of a subsystem that can be replaced.
+	 *
+	 * @param paramIndex
+	 *            Index of custom parameter. [0-1]
+	 * @param timoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Value of the custom param.
+	 */
+	public int configGetCustomParam(int paramIndex, int timoutMs) {
+		int retval = CANifierJNI.JNI_ConfigGetCustomParam(m_handle, paramIndex, timoutMs);
+		return retval;
+	}
+
+	/**
+	 * Sets a parameter. Generally this is not used.
+   * This can be utilized in
+   * - Using new features without updating API installation.
+   * - Errata workarounds to circumvent API implementation.
+   * - Allows for rapid testing / unit testing of firmware.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param value
+	 *            Value of parameter.
+	 * @param subValue
+	 *            Subvalue for parameter. Maximum value of 255.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSetParameter(ParamEnum param, double value, int subValue, int ordinal, int timeoutMs) {
+		return configSetParameter(param.value, value, subValue, ordinal, timeoutMs);
+	}
+
+	/**
+	 * Sets a parameter. Generally this is not used.
+   * This can be utilized in
+   * - Using new features without updating API installation.
+   * - Errata workarounds to circumvent API implementation.
+   * - Allows for rapid testing / unit testing of firmware.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param value
+	 *            Value of parameter.
+	 * @param subValue
+	 *            Subvalue for parameter. Maximum value of 255.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSetParameter(int param, double value, int subValue, int ordinal, int timeoutMs) {
+		int retval = CANifierJNI.JNI_ConfigSetParameter(m_handle, param, value, subValue, ordinal,
+				timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Gets a parameter. Generally this is not used.
+   * This can be utilized in
+   * - Using new features without updating API installation.
+   * - Errata workarounds to circumvent API implementation.
+   * - Allows for rapid testing / unit testing of firmware.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Value of parameter.
+	 */
+	public double configGetParameter(ParamEnum param, int ordinal, int timeoutMs) {
+		return CANifierJNI.JNI_ConfigGetParameter(m_handle, param.value, ordinal, timeoutMs);
+	}
+	/**
+	 * Sets the period of the given status frame.
+	 *
+	 * @param statusFrame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setStatusFramePeriod(CANifierStatusFrame statusFrame, int periodMs, int timeoutMs) {
+		int retval = CANifierJNI.JNI_SetStatusFramePeriod(m_handle, statusFrame.value, periodMs, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Sets the period of the given status frame.
+	 *
+	 * @param statusFrame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setStatusFramePeriod(int statusFrame, int periodMs, int timeoutMs) {
+		int retval = CANifierJNI.JNI_SetStatusFramePeriod(m_handle, statusFrame, periodMs, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Gets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame to get the period of.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Period of the given status frame.
+	 */
+	public int getStatusFramePeriod(CANifierStatusFrame frame, int timeoutMs) {
+		return CANifierJNI.JNI_GetStatusFramePeriod(m_handle, frame.value, timeoutMs);
+	}
+
+	/**
+	 * Sets the period of the given control frame.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setControlFramePeriod(CANifierControlFrame frame, int periodMs) {
+		int retval = CANifierJNI.JNI_SetControlFramePeriod(m_handle, frame.value, periodMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Sets the period of the given control frame.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setControlFramePeriod(int frame, int periodMs) {
+		int retval = CANifierJNI.JNI_SetControlFramePeriod(m_handle, frame, periodMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Gets the firmware version of the device.
+	 *
+	 * @return Firmware version of device.
+	 */
+	public int getFirmwareVersion() {
+		return CANifierJNI.JNI_GetFirmwareVersion(m_handle);
+	}
+
+	/**
+	 * Returns true if the device has reset since last call.
+	 *
+	 * @return Has a Device Reset Occurred?
+	 */
+	public boolean hasResetOccurred() {
+		return CANifierJNI.JNI_HasResetOccurred(m_handle);
+	}
+
+	// ------ Faults ----------//
+	/**
+	 * Gets the CANifier fault status
+	 *
+	 * @param toFill
+	 *            Container for fault statuses.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode getFaults(CANifierFaults toFill) {
+		int bits = CANifierJNI.JNI_GetFaults(m_handle);
+		toFill.update(bits);
+		return getLastError();
+	}
+	/**
+	 * Gets the CANifier sticky fault status
+	 *
+	 * @param toFill
+	 *            Container for sticky fault statuses.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode getStickyFaults(CANifierStickyFaults toFill) {
+		int bits = CANifierJNI.JNI_GetStickyFaults(m_handle);
+		toFill.update(bits);
+		return getLastError();
+	}
+	/**
+	 * Clears the Sticky Faults
+	 *
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode clearStickyFaults(int timeoutMs) {
+		int retval = CANifierJNI.JNI_ClearStickyFaults(m_handle, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Gets the bus voltage seen by the device.
+	 *
+	 * @return The bus voltage value (in volts).
+	 */
+	public double getBusVoltage() {
+		return CANifierJNI.JNI_GetBusVoltage(m_handle);
+	}
+
+	/**
+	 * @return The Device Number
+	 */
+	public int getDeviceID(){
+		return m_deviceNumber;
+	}
+}
diff --git a/java/src/com/ctre/phoenix/CANifierControlFrame.java b/java/src/com/ctre/phoenix/CANifierControlFrame.java
new file mode 100644
index 0000000..c7ce0f0
--- /dev/null
+++ b/java/src/com/ctre/phoenix/CANifierControlFrame.java
@@ -0,0 +1,21 @@
+package com.ctre.phoenix;
+
+public enum CANifierControlFrame {
+	Control_1_General(0x040000), 
+	Control_2_PwmOutput(0x040040);
+
+	public static CANifierControlFrame valueOf(int value) {
+		for (CANifierControlFrame frame : values()) {
+			if (frame.value == value) {
+				return frame;
+			}
+		}
+		return null;
+	}
+
+	public final int value;
+
+	CANifierControlFrame(int initValue) {
+		this.value = initValue;
+	}
+}
diff --git a/java/src/com/ctre/phoenix/CANifierFaults.java b/java/src/com/ctre/phoenix/CANifierFaults.java
new file mode 100644
index 0000000..99a47b9
--- /dev/null
+++ b/java/src/com/ctre/phoenix/CANifierFaults.java
@@ -0,0 +1,16 @@
+package com.ctre.phoenix;
+
+public class CANifierFaults {
+	//!< True iff any of the above flags are true.
+	public boolean hasAnyFault() {
+		return false;
+	}
+	public int toBitfield() {
+		int retval = 0;
+		return retval;
+	}
+	public void update(int bits) {
+	}
+	public CANifierFaults() {
+	}
+};
diff --git a/java/src/com/ctre/phoenix/CANifierJNI.java b/java/src/com/ctre/phoenix/CANifierJNI.java
new file mode 100644
index 0000000..086397d
--- /dev/null
+++ b/java/src/com/ctre/phoenix/CANifierJNI.java
@@ -0,0 +1,107 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ * 
+ * Cross The Road Electronics (CTRE) licenses to you the right to 
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ * 
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, 
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+ package com.ctre.phoenix;
+ 
+ public class CANifierJNI extends CTREJNIWrapper {
+	 
+	public enum GeneralPin
+	{
+		QUAD_IDX(0),
+		QUAD_B (1),
+		QUAD_A (2),
+		LIMR (3),
+		LIMF (4),
+		SDA (5),
+		SCL (6),
+		SPI_CS (7),
+		SPI_MISO_PWM2P (8),
+		SPI_MOSI_PWM1P (9),
+		SPI_CLK_PWM0P (10);
+		
+		final public int value;
+
+		GeneralPin(int value) {
+			this.value = value;
+		}
+	}
+	
+	public static native long JNI_new_CANifier(int deviceNumber);
+
+	public static native void JNI_SetLEDOutput(long handle, int dutyCycle, int ledChannel);
+
+	public static native void JNI_SetGeneralOutputs(long handle, int outputBits, int isOutputBits);
+
+	public static native void JNI_SetGeneralOutput(long handle, int outputPin, boolean outputValue,
+			boolean outputEnable);
+
+	public static native void JNI_SetPWMOutput(long handle, int pwmChannel, int dutyCycle);
+
+	public static native void JNI_EnablePWMOutput(long handle, int pwmChannel, boolean bEnable);
+
+	public static native void JNI_GetGeneralInputs(long handle, boolean[] allPins);
+
+	public static native boolean JNI_GetGeneralInput(long handl, int inputPin);
+
+	public static native void JNI_GetPWMInput(long handle, int pwmChannel, double dutyCycleAndPeriod[]);
+
+	public static native int JNI_GetLastError(long handle);
+
+	public static native double JNI_GetBatteryVoltage(long handle);
+	
+	public static native int JNI_GetQuadraturePosition(long handle);
+	
+	public static native int JNI_SetQuadraturePosition(long handle, int newPosition, int timeoutMs);
+	
+	public static native int JNI_GetQuadratureVelocity(long handle);
+	
+	public static native int JNI_ConfigVelocityMeasurementPeriod(long handle, int period, int timeoutMs);
+	
+	public static native int JNI_ConfigVelocityMeasurementWindow(long handle, int windowSize, int timeoutMs);
+
+	public static native int JNI_ConfigSetCustomParam(long handle, int newValue, int paramIndex, int timeoutMs);
+
+	public static native int JNI_ConfigGetCustomParam(long handle, int paramIndex, int timoutMs);
+
+	public static native int JNI_ConfigSetParameter(long handle, int param, double value, int subValue, int ordinal,
+			int timeoutMs);
+
+	public static native double JNI_ConfigGetParameter(long handle, int param, int ordinal, int timeoutMs);
+
+	public static native int JNI_SetStatusFramePeriod(long handle, int statusFrame, int periodMs, int timeoutMs);
+
+	public static native int JNI_GetStatusFramePeriod(long handle, int frame, int timeoutMs);
+
+	public static native int JNI_SetControlFramePeriod(long handle, int frame, int periodMs);
+
+	public static native int JNI_GetFirmwareVersion(long handle);
+
+	public static native boolean JNI_HasResetOccurred(long handle);
+
+	public static native int JNI_GetFaults(long handle);
+
+	public static native int JNI_GetStickyFaults(long handle);
+
+	public static native int JNI_ClearStickyFaults(long handle, int timeoutMs);
+
+	public static native double JNI_GetBusVoltage(long handle);
+ }
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/CANifierStatusFrame.java b/java/src/com/ctre/phoenix/CANifierStatusFrame.java
new file mode 100644
index 0000000..904b9be
--- /dev/null
+++ b/java/src/com/ctre/phoenix/CANifierStatusFrame.java
@@ -0,0 +1,26 @@
+package com.ctre.phoenix;
+
+public enum CANifierStatusFrame {
+	Status_1_General(0x041400), 
+	Status_2_General(0x041440), 
+	Status_3_PwmInputs0(0x041480), 
+	Status_4_PwmInputs1(0x0414C0), 
+	Status_5_PwmInputs2(0x041500), 
+	Status_6_PwmInputs3(0x041540), 
+	Status_8_Misc(0x0415C0);
+
+	public static CANifierStatusFrame valueOf(int value) {
+		for (CANifierStatusFrame frame : values()) {
+			if (frame.value == value) {
+				return frame;
+			}
+		}
+		return null;
+	}
+
+	public final int value;
+
+	CANifierStatusFrame(int initValue) {
+		this.value = initValue;
+	}
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/CANifierStickyFaults.java b/java/src/com/ctre/phoenix/CANifierStickyFaults.java
new file mode 100644
index 0000000..2cb8dac
--- /dev/null
+++ b/java/src/com/ctre/phoenix/CANifierStickyFaults.java
@@ -0,0 +1,16 @@
+package com.ctre.phoenix;
+
+public class CANifierStickyFaults {
+	//!< True iff any of the above flags are true.
+	public boolean hasAnyFault() {
+		return false;
+	}
+	public int toBitfield() {
+		int retval = 0;
+		return retval;
+	}
+	public void update(int bits) {
+	}
+	public CANifierStickyFaults() {
+	}
+};
diff --git a/java/src/com/ctre/phoenix/CTREJNIWrapper.java b/java/src/com/ctre/phoenix/CTREJNIWrapper.java
new file mode 100644
index 0000000..8556ea5
--- /dev/null
+++ b/java/src/com/ctre/phoenix/CTREJNIWrapper.java
@@ -0,0 +1,21 @@
+package com.ctre.phoenix;
+
+public class CTREJNIWrapper {
+  static boolean libraryLoaded = false;
+  
+  static {
+    if (!libraryLoaded) {
+      try {
+        System.loadLibrary("CTRE_PhoenixCCI");
+      } catch (UnsatisfiedLinkError e) {
+        e.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  public static native int getPortWithModule(byte module, byte channel);
+
+  public static native int getPort(byte channel);
+}
diff --git a/java/src/com/ctre/phoenix/CTRLoggerJNI.java b/java/src/com/ctre/phoenix/CTRLoggerJNI.java
new file mode 100644
index 0000000..059494a
--- /dev/null
+++ b/java/src/com/ctre/phoenix/CTRLoggerJNI.java
@@ -0,0 +1,12 @@
+package com.ctre.phoenix;
+
+import com.ctre.phoenix.CTREJNIWrapper;
+
+/* package */ class CTRLoggerJNI extends CTREJNIWrapper {
+	/* package */ static native int JNI_Logger_Log(int code, String origin, String stackTrace);
+
+	// public static native void JNI_Logger_Close();
+	// public static native void JNI_Logger_Open(int language);
+	// public static native String JNI_Logger_GetShort(int code);
+	// public static native String JNI_Logger_GetLong(int code);
+}
diff --git a/java/src/com/ctre/phoenix/ErrorCode.java b/java/src/com/ctre/phoenix/ErrorCode.java
new file mode 100644
index 0000000..97a47c2
--- /dev/null
+++ b/java/src/com/ctre/phoenix/ErrorCode.java
@@ -0,0 +1,123 @@
+package com.ctre.phoenix;
+
+import java.util.HashMap;
+
+public enum ErrorCode {
+	OK(0), 						//!< No Error - Function executed as expected
+
+	//CAN-Related
+	CAN_MSG_STALE(1),
+	CAN_TX_FULL(-1),
+	TxFailed(-1),				//!< Could not transmit the CAN frame.
+	InvalidParamValue(-2), 	//!< Caller passed an invalid param
+	CAN_INVALID_PARAM(-2),
+	RxTimeout(-3),				//!< CAN frame has not been received within specified period of time.
+	CAN_MSG_NOT_FOUND(-3),
+	TxTimeout(-4),				//!< Not used.
+	CAN_NO_MORE_TX_JOBS(-4),
+	UnexpectedArbId(-5),		//!< Specified CAN Id is invalid.
+	CAN_NO_SESSIONS_AVAIL(-5),
+	BufferFull(+6),			//!< Caller attempted to insert data into a buffer that is full.
+	CAN_OVERFLOW(-6),
+	SensorNotPresent(-7),		//!< Sensor is not present
+	FirmwareTooOld (-8),
+
+
+	//General
+	GeneralError(-100),		//!< User Specified General Error
+	GENERAL_ERROR(-100),
+
+	//Signal
+	SIG_NOT_UPDATED(-200),
+	SigNotUpdated(-200),			//!< Have not received an value response for signal.
+	NotAllPIDValuesUpdated(-201),
+
+	//Gadgeteer Port Error Codes
+	//These include errors between ports and modules
+	GEN_PORT_ERROR(-300),
+	PORT_MODULE_TYPE_MISMATCH(-301),
+	//Gadgeteer Module Error Codes
+	//These apply only to the module units themselves
+	GEN_MODULE_ERROR(-400),
+	MODULE_NOT_INIT_SET_ERROR(-401),
+	MODULE_NOT_INIT_GET_ERROR(-402),
+
+
+	//API
+	WheelRadiusTooSmall(-500),
+	TicksPerRevZero(-501),
+	DistanceBetweenWheelsTooSmall(-502),
+	GainsAreNotSet(-503),
+
+	//Higher Level
+	IncompatibleMode(-600),
+	InvalidHandle(-601),		//!< Handle does not match stored map of handles
+
+
+	//CAN Related
+	PulseWidthSensorNotPresent (10),	//!< Special Code for "isSensorPresent"
+
+	//General
+	GeneralWarning(100),
+	FeatureNotSupported(101),
+	NotImplemented(102),
+	FirmVersionCouldNotBeRetrieved (103),
+	FeaturesNotAvailableYet(104),
+	ControlModeNotValid(105),
+
+	ControlModeNotSupportedYet(106),
+	CascadedPIDNotSupportedYet(107),
+	AuxiliaryPIDNotSupportedYet(107),
+	RemoteSensorsNotSupportedYet(108),
+	MotProfFirmThreshold(109),
+	MotProfFirmThreshold2(110);
+
+	//---------------------- Integral To Enum operators -----------//
+    public final int value; //!< Hold the integral value of an enum instance.
+    /** Keep singleton map to quickly lookup enum via int */
+    private static HashMap<Integer, ErrorCode> _map = null;
+    /** private c'tor for above declarations */
+	private ErrorCode(int initValue) {this.value = initValue;	}
+	/** static c'tor, prepare the map */
+    static {
+    	_map = new HashMap<Integer, ErrorCode>();
+		for (ErrorCode type : ErrorCode.values()) {
+			_map.put(type.value, type);
+		}
+    }
+    /** public lookup to convert int to enum */
+	public static ErrorCode valueOf(int value) {
+		ErrorCode retval = _map.get(value);
+		if (retval != null)
+			return retval;
+		return GeneralError;
+	}
+
+	/** @return the first nonzero error code */
+	public static ErrorCode worstOne(ErrorCode errorCode1, ErrorCode errorCode2) {
+		if (errorCode1.value != 0)
+			return errorCode1;
+		return errorCode2;
+	}
+
+	/** @return the first nonzero error code */
+	public static ErrorCode worstOne(ErrorCode errorCode1, ErrorCode errorCode2, ErrorCode errorCode3) {
+		if (errorCode1.value != 0)
+			return errorCode1;
+		if (errorCode2.value != 0)
+			return errorCode2;
+		return errorCode3;
+	}
+
+	/** @return the first nonzero error code */
+	public static ErrorCode worstOne(ErrorCode errorCode1, ErrorCode errorCode2, ErrorCode errorCode3,
+			ErrorCode errorCode4) {
+		if (errorCode1.value != 0)
+			return errorCode1;
+		if (errorCode2.value != 0)
+			return errorCode2;
+		if (errorCode3.value != 0)
+			return errorCode3;
+		return errorCode4;
+	}
+};
diff --git a/java/src/com/ctre/phoenix/GadgeteerUartClient.java b/java/src/com/ctre/phoenix/GadgeteerUartClient.java
new file mode 100644
index 0000000..b40dad9
--- /dev/null
+++ b/java/src/com/ctre/phoenix/GadgeteerUartClient.java
@@ -0,0 +1,71 @@
+package com.ctre.phoenix;
+
+public interface GadgeteerUartClient
+{
+	public enum GadgeteerProxyType{
+		General(0), 
+		Pigeon(1), 
+		PC_HERO(2),
+		Unknown(-1);
+		private int value; private GadgeteerProxyType(int value) { this.value = value; } 
+		public static GadgeteerProxyType valueOf(int value) {
+			for (GadgeteerProxyType e : GadgeteerProxyType.values()) {
+				if (e.value == value) {
+					return e;
+				}
+			}
+			return Unknown;
+		}
+	};
+
+	public enum GadgeteerConnection	{
+		NotConnected (0),
+		Connecting (1),
+		Connected (2),
+		Unknown(-1);
+		private int value; private GadgeteerConnection(int value) { this.value = value; } 
+		public static GadgeteerConnection valueOf(int value) {
+			for (GadgeteerConnection e : GadgeteerConnection.values()) {
+				if (e.value == value) {
+					return e;
+				}
+			}
+			return Unknown;
+		}
+	};
+
+	public static class GadgeteerUartStatus {
+		public GadgeteerProxyType type;
+		public GadgeteerConnection conn;
+		public int bitrate;
+		public int resetCount;
+	};
+
+
+	enum GadgeteerState
+	{
+		GadState_WaitChirp1(0),
+		GadState_WaitBLInfo(1),
+		GadState_WaitBitrateResp(2),
+		GadState_WaitSwitchDelay(3),
+		GadState_WaitChirp2(4),
+		GadState_Connected_Idle(5),
+		GadState_Connected_ReqChirp(6),
+		GadState_Connected_RespChirp(7),
+		GadState_Connected_ReqCanBus(8),
+		GadState_Connected_RespCanBus(9),
+		GadState_Connected_RespIsoThenChirp(10),
+		GadState_Connected_RespIsoThenCanBus(11);
+		private int value; private GadgeteerState(int value) { this.value = value; } 
+		public static GadgeteerState valueOf(int value) {
+			for (GadgeteerState e : GadgeteerState.values()) {
+				if (e.value == value) {
+					return e;
+				}
+			}
+			return GadState_WaitChirp1;
+		}
+	};
+    int getGadgeteerStatus(GadgeteerUartStatus status);
+};
+
diff --git a/java/src/com/ctre/phoenix/ILoopable.java b/java/src/com/ctre/phoenix/ILoopable.java
new file mode 100644
index 0000000..1e52471
--- /dev/null
+++ b/java/src/com/ctre/phoenix/ILoopable.java
@@ -0,0 +1,9 @@
+package com.ctre.phoenix;
+
+public interface ILoopable
+{
+	public void onStart();
+	public void onLoop();
+	public boolean isDone();
+	public void onStop();
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/Logger.java b/java/src/com/ctre/phoenix/Logger.java
new file mode 100644
index 0000000..c1caaca
--- /dev/null
+++ b/java/src/com/ctre/phoenix/Logger.java
@@ -0,0 +1,38 @@
+package com.ctre.phoenix;
+
+public class Logger
+{
+	/**
+	 * Logs an entry into the Phoenix DS Error/Logger stream
+	 * @param code Error code to log.  If OKAY is passed, no action is taken.
+	 * @param origin Origin string to send to DS/Log
+	 * @return OKAY error code.
+	 */
+	public static ErrorCode log(ErrorCode code, String origin) {
+		/* only take action if the error code is nonzero */
+		if (code != ErrorCode.OK) {
+			String stack = java.util.Arrays.toString(Thread.currentThread().getStackTrace());
+			stack = stack.replaceAll(",", "\n");
+			int errCode = code.value;
+			return ErrorCode.valueOf(CTRLoggerJNI.JNI_Logger_Log(errCode, origin, stack));
+		}
+		/* otherwise return OK */
+		return ErrorCode.OK;
+	}
+
+	//public static void close() {
+	//	//CTRLoggerJNI.JNI_Logger_Close();
+	//}
+	//public static void open() {
+	//	//CTRLoggerJNI.JNI_Logger_Open(2);
+	//}
+	/*
+	public static String getVerbose(int code) {
+		return CTRLoggerJNI.JNI_Logger_GetLong(code);
+	}
+	public static String getShort(int code) {
+		return CTRLoggerJNI.JNI_Logger_GetShort(code);
+	}
+	*/
+}
+
diff --git a/java/src/com/ctre/phoenix/ParamEnum.java b/java/src/com/ctre/phoenix/ParamEnum.java
new file mode 100644
index 0000000..f6b3706
--- /dev/null
+++ b/java/src/com/ctre/phoenix/ParamEnum.java
@@ -0,0 +1,109 @@
+package com.ctre.phoenix;
+
+public enum ParamEnum
+	{
+	eOnBoot_BrakeMode ( 31),
+	eQuadFilterEn ( 91),
+	eQuadIdxPolarity(108),
+	eClearPositionOnIdx (100),
+    eMotionProfileHasUnderrunErr (119),
+	eClearPosOnLimitF (144),
+	eClearPosOnLimitR (145),
+
+	eStatusFramePeriod(300),
+	eOpenloopRamp(301),
+	eClosedloopRamp(302),
+	eNeutralDeadband(303),
+
+	ePeakPosOutput(305),
+	eNominalPosOutput(306),
+	ePeakNegOutput(307),
+	eNominalNegOutput(308),
+
+	eProfileParamSlot_P(310),
+	eProfileParamSlot_I(311),
+	eProfileParamSlot_D(312),
+	eProfileParamSlot_F(313),
+	eProfileParamSlot_IZone(314),
+	eProfileParamSlot_AllowableErr(315),
+	eProfileParamSlot_MaxIAccum(316),
+	eProfileParamSlot_PeakOutput(317),
+
+	eClearPositionOnLimitF(320),
+	eClearPositionOnLimitR(321),
+	eClearPositionOnQuadIdx(322),
+
+	eSampleVelocityPeriod(325),
+	eSampleVelocityWindow(326),
+
+	eFeedbackSensorType(330),
+    eSelectedSensorPosition(331),
+	eFeedbackNotContinuous (332),
+	eRemoteSensorSource (333), // RemoteSensorSource_t
+	eRemoteSensorDeviceID (334), // [0,62] DeviceID
+	eSensorTerm (335), // feedbackDevice_t (ordinal is the register)
+	eRemoteSensorClosedLoopDisableNeutralOnLOS (336),
+	ePIDLoopPolarity(337),
+	ePIDLoopPeriod(338),
+	eSelectedSensorCoefficient(339),
+
+	eForwardSoftLimitThreshold(340),
+	eReverseSoftLimitThreshold(341),
+	eForwardSoftLimitEnable(342),
+	eReverseSoftLimitEnable(343),
+
+	eNominalBatteryVoltage(350),
+	eBatteryVoltageFilterSize(351),
+
+	eContinuousCurrentLimitAmps(360),
+	ePeakCurrentLimitMs(361),
+	ePeakCurrentLimitAmps(362),
+
+	eClosedLoopIAccum(370),
+
+	eCustomParam(380),
+
+	eStickyFaults(390),
+
+	eAnalogPosition(400),
+	eQuadraturePosition(401),
+	ePulseWidthPosition(402),
+
+	eMotMag_Accel(410),
+	eMotMag_VelCruise(411),
+
+	eLimitSwitchSource (421), // ordinal (fwd=0,reverse=1), @see LimitSwitchSource_t
+	eLimitSwitchNormClosedAndDis ( 422), // ordinal (fwd=0,reverse=1). @see LimitSwitchNormClosedAndDis_t
+	eLimitSwitchDisableNeutralOnLOS ( 423),
+	eLimitSwitchRemoteDevID ( 424),
+	eSoftLimitDisableNeutralOnLOS(425),
+
+	ePulseWidthPeriod_EdgesPerRot(430),
+	ePulseWidthPeriod_FilterWindowSz(431),
+
+	eYawOffset(160),
+	eCompassOffset(161),
+	eBetaGain(162),
+	eEnableCompassFusion(163),
+	eGyroNoMotionCal (	164),
+	eEnterCalibration (	165),
+	eFusedHeadingOffset	( 166),
+	eStatusFrameRate	( 169),
+	eAccumZ	( 170),
+	eTempCompDisable	( 171),
+	eMotionMeas_tap_threshX ( 172),
+	eMotionMeas_tap_threshY ( 173),
+	eMotionMeas_tap_threshZ ( 174),
+	eMotionMeas_tap_count ( 175),
+	eMotionMeas_tap_time ( 176),
+	eMotionMeas_tap_time_multi ( 177),
+	eMotionMeas_shake_reject_thresh ( 178),
+	eMotionMeas_shake_reject_time ( 179),
+	eMotionMeas_shake_reject_timeout ( 180);
+
+	public final int value;
+	ParamEnum(int initValue)
+	{
+		this.value = initValue;
+	}
+};
diff --git a/java/src/com/ctre/phoenix/Util.java b/java/src/com/ctre/phoenix/Util.java
new file mode 100644
index 0000000..4ccedc7
--- /dev/null
+++ b/java/src/com/ctre/phoenix/Util.java
@@ -0,0 +1,48 @@
+package com.ctre.phoenix;
+
+public class Util
+{
+	public static double cap(double value, double peak)
+	{
+		if(value < -peak) value = -peak;
+		if(value > peak) value = peak;
+		return value;
+	}
+	
+	public static int scaleRotationsToNativeUnits(double scalar, double fullRotations) {
+		/* first assume we don't have config info, prep the default return */
+		int retval = (int) fullRotations;
+		/* apply scalar if its available */
+		if (scalar > 0) {
+		  retval = (int) (fullRotations * scalar);
+		}
+		return retval;
+	}
+	public static int scaleVelocityToNativeUnits(double scalar, double rpm) {
+		/* first assume we don't have config info, prep the default return */
+		int retval = (int) rpm;
+		/* apply scalar if its available */
+		if (scalar > 0) {
+		  retval = (int) (rpm * scalar);
+		}
+		return retval;
+	}
+	public static double scaleNativeUnitsToRotations(double scalar, long nativePos) {
+		/* first assume we don't have config info, prep the default return */
+		double retval = (double) nativePos;
+		/* retrieve scaling info */
+		if (scalar > 0) {
+		  retval = ((double) nativePos) / scalar;
+		}
+		return retval;
+	}
+	public static double scaleNativeUnitsToRpm(double scalar, long nativeVel) {
+		/* first assume we don't have config info, prep the default return */
+		double retval = (double) nativeVel;
+		/* apply scalar if its available */
+		if (scalar > 0) {
+		  retval = (double) (nativeVel) / (scalar);
+		}
+		return retval;
+	}
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motion/MotionProfileStatus.java b/java/src/com/ctre/phoenix/motion/MotionProfileStatus.java
new file mode 100644
index 0000000..5901a75
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motion/MotionProfileStatus.java
@@ -0,0 +1,57 @@
+package com.ctre.phoenix.motion;
+
+/**
+ * Motion Profile Status This is simply a data transer object.
+ */
+public class MotionProfileStatus {
+	/**
+	 * The available empty slots in the trajectory buffer.
+	 *
+	 * The robot API holds a "top buffer" of trajectory points, so your
+	 * applicaion can dump several points at once. The API will then stream them
+	 * into the Talon's low-level buffer, allowing the Talon to act on them.
+	 */
+	public int topBufferRem;
+	/**
+	 * The number of points in the top trajectory buffer.
+	 */
+	public int topBufferCnt;
+	/**
+	 * The number of points in the low level Talon buffer.
+	 */
+	public int btmBufferCnt;
+	/**
+	 * Set if isUnderrun ever gets set. Only is cleared by
+	 * clearMotionProfileHasUnderrun() to ensure robot logic can react or
+	 * instrument it.
+	 *
+	 * @see com.ctre.phoenix.motorcontrol.can.BaseMotorController#clearMotionProfileHasUnderrun(int)
+	 */
+	public boolean hasUnderrun;
+	/**
+	 * This is set if Talon needs to shift a point from its buffer into the
+	 * active trajectory point however the buffer is empty. This gets cleared
+	 * automatically when is resolved.
+	 */
+	public boolean isUnderrun;
+	/**
+	 * True if the active trajectory point has not empty, false otherwise. The
+	 * members in activePoint are only valid if this signal is set.
+	 */
+	public boolean activePointValid;
+
+	public boolean isLast;
+
+	public int profileSlotSelect;
+	/**
+	 * The current output mode of the motion profile executer (disabled,
+	 * enabled, or hold). When changing the set() value in MP mode, it's
+	 * important to check this signal to confirm the change takes effect before
+	 * interacting with the top buffer.
+	 */
+	public SetValueMotionProfile outputEnable;
+	
+	public int timeDurMs;
+	
+	public int profileSlotSelect1;
+}
diff --git a/java/src/com/ctre/phoenix/motion/SetValueMotionProfile.java b/java/src/com/ctre/phoenix/motion/SetValueMotionProfile.java
new file mode 100644
index 0000000..82210c4
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motion/SetValueMotionProfile.java
@@ -0,0 +1,20 @@
+package com.ctre.phoenix.motion;
+
+public enum SetValueMotionProfile {
+	Invalid(-1), Disable(0), Enable(1), Hold(2);
+
+	public final int value;
+
+	SetValueMotionProfile(int initValue) {
+		this.value = initValue;
+	}
+	
+	public static SetValueMotionProfile valueOf(int value) {
+		for (SetValueMotionProfile e : SetValueMotionProfile.values()) {
+			if (e.value == value) {
+				return e;
+			}
+		}
+		return Invalid;
+	}
+}
diff --git a/java/src/com/ctre/phoenix/motion/TrajectoryPoint.java b/java/src/com/ctre/phoenix/motion/TrajectoryPoint.java
new file mode 100644
index 0000000..293038f
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motion/TrajectoryPoint.java
@@ -0,0 +1,85 @@
+package com.ctre.phoenix.motion;
+
+/**
+ * Motion Profile Trajectory Point This is simply a data transfer object.
+ */
+public class TrajectoryPoint {
+	/**
+	 * Duration to apply to a particular trajectory pt.
+	 * This time unit is ADDED to the existing base time set by
+	 * configMotionProfileTrajectoryPeriod().
+	 */
+	public enum TrajectoryDuration
+	{
+		Trajectory_Duration_0ms(0),
+		Trajectory_Duration_5ms(5),
+		Trajectory_Duration_10ms(10),
+		Trajectory_Duration_20ms(20),
+		Trajectory_Duration_30ms(30),
+		Trajectory_Duration_40ms(40),
+		Trajectory_Duration_50ms(50),
+		Trajectory_Duration_100ms(100);
+
+		public int value = 5;
+
+		private TrajectoryDuration(int value)
+		{
+			this.value = value;
+		}
+
+		public TrajectoryDuration valueOf(int val)
+		{
+			for(TrajectoryDuration td: TrajectoryDuration.values())
+			{
+				if(td.value == val) return td;
+			}
+			return Trajectory_Duration_100ms;
+		}
+	}
+
+	public double position; // !< The position to servo to.
+	public double velocity; // !< The velocity to feed-forward.
+	public double headingDeg; // !< Not used.  Use auxiliaryPos instead.  @see auxiliaryPos
+	public double auxiliaryPos; // !< The position for auxiliary PID to target.
+
+	/**
+	 * Which slot to get PIDF gains. PID is used for position servo. F is used
+	 * as the Kv constant for velocity feed-forward. Typically this is hard-coded
+	 * to a particular slot, but you are free to gain schedule if need be.
+	 * Choose from [0,3]
+	 */
+	public int profileSlotSelect0;
+
+	/**
+	 * Which slot to get PIDF gains for auxiliary PId.
+	 * This only has impact during MotionProfileArc Control mode.
+	 * Choose from [0,1].
+	 */
+	public int profileSlotSelect1;
+
+	/**
+	 * Set to true to signal Talon that this is the final point, so do not
+	 * attempt to pop another trajectory point from out of the Talon buffer.
+	 * Instead continue processing this way point. Typically the velocity member
+	 * variable should be zero so that the motor doesn't spin indefinitely.
+	 */
+	public boolean isLastPoint;
+	/**
+	 * Set to true to signal Talon to zero the selected sensor. When generating
+	 * MPs, one simple method is to make the first target position zero, and the
+	 * final target position the target distance from the current position. Then
+	 * when you fire the MP, the current position gets set to zero. If this is
+	 * the intent, you can set zeroPos on the first trajectory point.
+	 *
+	 * Otherwise you can leave this false for all points, and offset the
+	 * positions of all trajectory points so they are correct.
+	 */
+	public boolean zeroPos;
+
+	/**
+	 * Duration to apply this trajectory pt.
+	 * This time unit is ADDED to the existing base time set by
+	 * configMotionProfileTrajectoryPeriod().
+	 */
+	public TrajectoryDuration timeDur;
+}
diff --git a/java/src/com/ctre/phoenix/motorcontrol/ControlFrame.java b/java/src/com/ctre/phoenix/motorcontrol/ControlFrame.java
new file mode 100644
index 0000000..5b77424
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/ControlFrame.java
@@ -0,0 +1,14 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum ControlFrame
+{
+	Control_3_General(0x040080),
+	Control_4_Advanced(0x0400C0),
+	Control_6_MotProfAddTrajPoint(0x040140);
+	
+	public final int value;
+	ControlFrame(int initValue)
+	{
+		this.value = initValue;
+	}
+};
diff --git a/java/src/com/ctre/phoenix/motorcontrol/ControlFrameEnhanced.java b/java/src/com/ctre/phoenix/motorcontrol/ControlFrameEnhanced.java
new file mode 100644
index 0000000..adc6512
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/ControlFrameEnhanced.java
@@ -0,0 +1,16 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum ControlFrameEnhanced
+{
+	Control_2_Enable_50m(0x040040),
+	Control_3_General(0x040080),
+	Control_4_Advanced(0x0400C0),
+	Control_5_FeedbackOutputOverride(0x040100),
+	Control_6_MotProfAddTrajPoint(0x040140);
+	
+	public final int value;
+	ControlFrameEnhanced(int initValue)
+	{
+		this.value = initValue;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/ControlMode.java b/java/src/com/ctre/phoenix/motorcontrol/ControlMode.java
new file mode 100644
index 0000000..834a8ce
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/ControlMode.java
@@ -0,0 +1,21 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum ControlMode
+{
+	PercentOutput(0),
+	Position(1),
+	Velocity(2),
+	Current(3),
+	Follower(5),
+	MotionProfile(6),
+	MotionMagic(7),
+	MotionProfileArc(10),
+
+	Disabled(15);
+
+	public final int value;
+	ControlMode(int initValue)
+	{
+		this.value = initValue;
+	}
+};
diff --git a/java/src/com/ctre/phoenix/motorcontrol/DemandType.java b/java/src/com/ctre/phoenix/motorcontrol/DemandType.java
new file mode 100644
index 0000000..b0688bb
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/DemandType.java
@@ -0,0 +1,25 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum DemandType {
+  /**
+	 * Ignore the demand value and apply neutral/no-change.
+	 */
+	Neutral(0),
+	/**
+	 * When closed-looping, set the target of the aux PID loop to the demand value.
+	 *
+	 * When following, follow the processed output of the combined
+	 * primary/aux PID output.  The demand value is ignored.
+	 */
+	AuxPID(1), //!< Target value of PID loop 1.  When f
+	/**
+	 * When closed-looping, add this arbitrarily to the closed-loop output.
+	 */
+	ArbitraryFeedForward(2); //!< Simply add to the output
+
+	public int value;
+	DemandType(int value)
+	{
+		this.value = value;
+	}
+};
diff --git a/java/src/com/ctre/phoenix/motorcontrol/Faults.java b/java/src/com/ctre/phoenix/motorcontrol/Faults.java
new file mode 100644
index 0000000..47f5c91
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/Faults.java
@@ -0,0 +1,88 @@
+package com.ctre.phoenix.motorcontrol;
+
+public class Faults {
+	public boolean UnderVoltage;
+	public boolean ForwardLimitSwitch;
+	public boolean ReverseLimitSwitch;
+	public boolean ForwardSoftLimit;
+	public boolean ReverseSoftLimit;
+	public boolean HardwareFailure;
+	public boolean ResetDuringEn;
+	public boolean SensorOverflow;
+	public boolean SensorOutOfPhase;
+	public boolean HardwareESDReset;
+	public boolean RemoteLossOfSignal;
+	//!< True iff any of the above flags are true.
+	public boolean hasAnyFault() {
+		return 	UnderVoltage |
+				ForwardLimitSwitch |
+				ReverseLimitSwitch |
+				ForwardSoftLimit |
+				ReverseSoftLimit |
+				HardwareFailure |
+				ResetDuringEn |
+				SensorOverflow |
+				SensorOutOfPhase |
+				HardwareESDReset |
+				RemoteLossOfSignal;
+	}
+	public int toBitfield() {
+		int retval = 0;
+		int mask = 1;
+		retval |= UnderVoltage ? mask : 0; mask <<= 1;
+		retval |= ForwardLimitSwitch ? mask : 0; mask <<= 1;
+		retval |= ReverseLimitSwitch ? mask : 0; mask <<= 1;
+		retval |= ForwardSoftLimit ? mask : 0; mask <<= 1;
+		retval |= ReverseSoftLimit ? mask : 0; mask <<= 1;
+		retval |= HardwareFailure ? mask : 0; mask <<= 1;
+		retval |= ResetDuringEn ? mask : 0; mask <<= 1;
+		retval |= SensorOverflow ? mask : 0; mask <<= 1;
+		retval |= SensorOutOfPhase ? mask : 0; mask <<= 1;
+		retval |= HardwareESDReset ? mask : 0; mask <<= 1;
+		retval |= RemoteLossOfSignal ? mask : 0; mask <<= 1;
+		return retval;
+	}
+	public void update(int bits) {
+		int mask = 1;
+		UnderVoltage = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ForwardLimitSwitch = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ReverseLimitSwitch = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ForwardSoftLimit = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ReverseSoftLimit = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		HardwareFailure = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ResetDuringEn = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		SensorOverflow = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		SensorOutOfPhase = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		HardwareESDReset = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		RemoteLossOfSignal = ((bits & mask)!=0) ? true : false; mask <<= 1;
+	}
+	public Faults() {
+		UnderVoltage = false;
+		ForwardLimitSwitch = false;
+		ReverseLimitSwitch = false;
+		ForwardSoftLimit = false;
+		ReverseSoftLimit = false;
+		HardwareFailure =false;
+		ResetDuringEn = false;
+		SensorOverflow = false;
+		SensorOutOfPhase = false;
+		HardwareESDReset = false;
+		RemoteLossOfSignal = false;
+	}
+	public String toString() {
+		StringBuilder work = new StringBuilder();
+		work.append(" UnderVoltage:"); work.append(UnderVoltage ? "1" : "0");
+		work.append( " ForwardLimitSwitch:"); work.append(ForwardLimitSwitch ? "1" : "0");
+		work.append( " ReverseLimitSwitch:"); work.append(ReverseLimitSwitch ? "1" : "0");
+		work.append( " ForwardSoftLimit:"); work.append(ForwardSoftLimit ? "1" : "0");
+		work.append( " ReverseSoftLimit:"); work.append(ReverseSoftLimit ? "1" : "0");
+		work.append( " HardwareFailure:"); work.append(HardwareFailure ? "1" : "0");
+		work.append( " ResetDuringEn:"); work.append(ResetDuringEn ? "1" : "0");
+		work.append( " SensorOverflow:"); work.append(SensorOverflow ? "1" : "0");
+		work.append( " SensorOutOfPhase:"); work.append(SensorOutOfPhase ? "1" : "0");
+		work.append( " HardwareESDReset:"); work.append(HardwareESDReset ? "1" : "0");
+		work.append( " RemoteLossOfSignal:"); work.append(RemoteLossOfSignal ? "1" : "0");
+		return work.toString();
+	}
+};
+
diff --git a/java/src/com/ctre/phoenix/motorcontrol/FeedbackDevice.java b/java/src/com/ctre/phoenix/motorcontrol/FeedbackDevice.java
new file mode 100644
index 0000000..56b457d
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/FeedbackDevice.java
@@ -0,0 +1,25 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum FeedbackDevice {
+	None(-1),
+
+	QuadEncoder(0),
+	Analog(2),
+	Tachometer(4),
+	PulseWidthEncodedPosition(8),
+
+	SensorSum(9),
+	SensorDifference(10),
+	RemoteSensor0(11),
+	RemoteSensor1(12),
+	SoftwareEmulatedSensor(15),
+
+	CTRE_MagEncoder_Absolute(8),
+	CTRE_MagEncoder_Relative(0);
+	
+	public final int value;
+	FeedbackDevice(int initValue)
+	{
+		this.value = initValue;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/FollowerType.java b/java/src/com/ctre/phoenix/motorcontrol/FollowerType.java
new file mode 100644
index 0000000..57a0d37
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/FollowerType.java
@@ -0,0 +1,12 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum FollowerType {
+  PercentOutput(0),
+  AuxOutput1(1);
+
+	public int value;
+	FollowerType(int value)
+	{
+		this.value = value;
+	}
+};
diff --git a/java/src/com/ctre/phoenix/motorcontrol/GroupMotorControllers.java b/java/src/com/ctre/phoenix/motorcontrol/GroupMotorControllers.java
new file mode 100644
index 0000000..70bb15a
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/GroupMotorControllers.java
@@ -0,0 +1,23 @@
+package com.ctre.phoenix.motorcontrol;
+
+import java.util.*;
+
+public class GroupMotorControllers
+{
+	static List<IMotorController> _ms = new ArrayList<IMotorController>();
+	
+	public static void register(IMotorController mc)
+	{
+		_ms.add(mc);
+	}
+	
+	public static int getCount()
+	{
+		return _ms.size();
+	}
+	
+	public static IMotorController get(int idx)
+	{
+		return _ms.get(idx);
+	}
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/IFollower.java b/java/src/com/ctre/phoenix/motorcontrol/IFollower.java
new file mode 100644
index 0000000..c31ee04
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/IFollower.java
@@ -0,0 +1,8 @@
+package com.ctre.phoenix.motorcontrol;
+import com.ctre.phoenix.motorcontrol.IMotorController;
+
+public interface IFollower
+{
+	void follow(IMotorController masterToFollow);
+	void valueUpdated();
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/IMotorController.java b/java/src/com/ctre/phoenix/motorcontrol/IMotorController.java
new file mode 100644
index 0000000..8eb6a20
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/IMotorController.java
@@ -0,0 +1,204 @@
+package com.ctre.phoenix.motorcontrol;
+
+import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix.ParamEnum;
+import com.ctre.phoenix.motion.MotionProfileStatus;
+import com.ctre.phoenix.motion.TrajectoryPoint;
+
+public interface IMotorController
+		extends com.ctre.phoenix.signals.IOutputSignal, com.ctre.phoenix.signals.IInvertable, IFollower {
+	// ------ Set output routines. ----------//
+	public void set(ControlMode Mode, double demand);
+
+	public void set(ControlMode Mode, double demand0, double demand1);
+
+	public void set(ControlMode Mode, double demand0, DemandType demand1Type, double demand1);
+
+	public void neutralOutput();
+
+	public void setNeutralMode(NeutralMode neutralMode);
+
+	// ------ Invert behavior ----------//
+	public void setSensorPhase(boolean PhaseSensor);
+
+	public void setInverted(boolean invert);
+
+	public boolean getInverted();
+
+	// ----- general output shaping ------------------//
+	public ErrorCode configOpenloopRamp(double secondsFromNeutralToFull, int timeoutMs);
+
+	public ErrorCode configClosedloopRamp(double secondsFromNeutralToFull, int timeoutMs);
+
+	public ErrorCode configPeakOutputForward(double percentOut, int timeoutMs);
+
+	public ErrorCode configPeakOutputReverse(double percentOut, int timeoutMs);
+
+	public ErrorCode configNominalOutputForward(double percentOut, int timeoutMs);
+
+	public ErrorCode configNominalOutputReverse(double percentOut, int timeoutMs);
+
+	public ErrorCode configNeutralDeadband(double percentDeadband, int timeoutMs);
+
+	// ------ Voltage Compensation ----------//
+	public ErrorCode configVoltageCompSaturation(double voltage, int timeoutMs);
+
+	public ErrorCode configVoltageMeasurementFilter(int filterWindowSamples, int timeoutMs);
+
+	public void enableVoltageCompensation(boolean enable);
+
+	// ------ General Status ----------//
+	public double getBusVoltage() ;
+
+	public double getMotorOutputPercent() ;
+
+	public double getMotorOutputVoltage() ;
+
+	public double getOutputCurrent() ;
+
+	public double getTemperature() ;
+
+	// ------ sensor selection ----------//
+	public ErrorCode configSelectedFeedbackSensor(RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs);
+
+	public ErrorCode configSelectedFeedbackCoefficient(double coefficient, int pidIdx, int timeoutMs);
+
+	public ErrorCode configRemoteFeedbackFilter(int deviceID, RemoteSensorSource remoteSensorSource, int remoteOrdinal,
+			int timeoutMs);
+
+	public ErrorCode configSensorTerm(SensorTerm sensorTerm, FeedbackDevice feedbackDevice, int timeoutMs);
+
+	// ------- sensor status --------- //
+	public int getSelectedSensorPosition(int pidIdx);
+
+	public int getSelectedSensorVelocity(int pidIdx);
+
+	public ErrorCode setSelectedSensorPosition(int sensorPos, int pidIdx, int timeoutMs);
+
+	// ------ status frame period changes ----------//
+	public ErrorCode setControlFramePeriod(ControlFrame frame, int periodMs);
+
+	public ErrorCode setStatusFramePeriod(StatusFrame frame, int periodMs, int timeoutMs);
+
+	public int getStatusFramePeriod(StatusFrame frame, int timeoutMs);
+
+	//----- velocity signal conditionaing ------//
+	/* not supported */
+
+	//------ remote limit switch ----------//
+	public ErrorCode configForwardLimitSwitchSource(RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs);
+
+	public ErrorCode configReverseLimitSwitchSource(RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs);
+
+	public void overrideLimitSwitchesEnable(boolean enable);
+
+	// ------ local limit switch ----------//
+	/* not supported */
+
+	// ------ soft limit ----------//
+	public ErrorCode configForwardSoftLimitThreshold(int forwardSensorLimit, int timeoutMs);
+
+	public ErrorCode configReverseSoftLimitThreshold(int reverseSensorLimit, int timeoutMs);
+
+	public ErrorCode configForwardSoftLimitEnable(boolean enable, int timeoutMs);
+
+	public ErrorCode configReverseSoftLimitEnable(boolean enable, int timeoutMs);
+
+	public void overrideSoftLimitsEnable(boolean enable);
+
+	// ------ Current Lim ----------//
+	/* not supported */
+
+	// ------ General Close loop ----------//
+	public ErrorCode config_kP(int slotIdx, double value, int timeoutMs);
+
+	public ErrorCode config_kI(int slotIdx, double value, int timeoutMs);
+
+	public ErrorCode config_kD(int slotIdx, double value, int timeoutMs);
+
+	public ErrorCode config_kF(int slotIdx, double value, int timeoutMs);
+
+	public ErrorCode config_IntegralZone(int slotIdx, int izone, int timeoutMs);
+
+	public ErrorCode configAllowableClosedloopError(int slotIdx, int allowableCloseLoopError, int timeoutMs);
+
+	public ErrorCode configMaxIntegralAccumulator(int slotIdx, double iaccum, int timeoutMs);
+
+	public ErrorCode configClosedLoopPeakOutput(int slotIdx, double percentOut, int timeoutMs);
+
+	public ErrorCode configClosedLoopPeriod(int slotIdx, int loopTimeMs, int timeoutMs);
+
+	public ErrorCode configAuxPIDPolarity(boolean invert, int timeoutMs);
+
+	//------ Close loop State ----------//
+	public ErrorCode setIntegralAccumulator(double iaccum, int pidIdx, int timeoutMs);
+
+	public int getClosedLoopError(int pidIdx);
+
+	public double getIntegralAccumulator(int pidIdx) ;
+
+	public double getErrorDerivative(int pidIdx) ;
+
+	public void selectProfileSlot(int slotIdx, int pidIdx);
+
+	public int getClosedLoopTarget(int pidIdx); // will be added to JNI
+
+	public int getActiveTrajectoryPosition();
+
+	public int getActiveTrajectoryVelocity();
+
+	public double getActiveTrajectoryHeading();
+
+	// ------ Motion Profile Settings used in Motion Magic and Motion Profile
+	public ErrorCode configMotionCruiseVelocity(int sensorUnitsPer100ms, int timeoutMs);
+
+	public ErrorCode configMotionAcceleration(int sensorUnitsPer100msPerSec, int timeoutMs);
+	
+	public ErrorCode configMotionProfileTrajectoryPeriod(int baseTrajDurationMs, int timeoutMs);
+
+	// ------ Motion Profile Buffer ----------//
+	public ErrorCode clearMotionProfileTrajectories();
+	public int getMotionProfileTopLevelBufferCount();
+	public ErrorCode pushMotionProfileTrajectory(TrajectoryPoint trajPt);
+	public boolean isMotionProfileTopLevelBufferFull();
+	public void processMotionProfileBuffer();
+	public ErrorCode getMotionProfileStatus(MotionProfileStatus statusToFill);
+	public ErrorCode clearMotionProfileHasUnderrun(int timeoutMs);
+	public ErrorCode changeMotionControlFramePeriod(int periodMs);
+
+	// ------ error ----------//
+	public ErrorCode getLastError();
+
+	// ------ Faults ----------//
+	public ErrorCode getFaults(Faults toFill) ;
+
+	public ErrorCode getStickyFaults(StickyFaults toFill) ;
+
+	public ErrorCode clearStickyFaults(int timeoutMs);
+
+	// ------ Firmware ----------//
+	public int getFirmwareVersion();
+
+	public boolean hasResetOccurred();
+
+	// ------ Custom Persistent Params ----------//
+	public ErrorCode configSetCustomParam(int newValue, int paramIndex, int timeoutMs);
+
+	public int configGetCustomParam(int paramIndex, int timoutMs);
+
+	//------ Generic Param API, typically not used ----------//
+	public ErrorCode configSetParameter(ParamEnum param, double value, int subValue, int ordinal, int timeoutMs);
+	public ErrorCode configSetParameter(int param, double value, int subValue, int ordinal, int timeoutMs);
+
+	public double configGetParameter(ParamEnum paramEnum, int ordinal, int timeoutMs) ;
+	public double configGetParameter(int paramEnum, int ordinal, int timeoutMs) ;
+
+	//------ Misc. ----------//
+	public int getBaseID();
+	public int getDeviceID();
+	public ControlMode getControlMode();
+	// ----- Follower ------//
+	/* in parent interface */
+}
diff --git a/java/src/com/ctre/phoenix/motorcontrol/IMotorControllerEnhanced.java b/java/src/com/ctre/phoenix/motorcontrol/IMotorControllerEnhanced.java
new file mode 100644
index 0000000..5f93c2d
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/IMotorControllerEnhanced.java
@@ -0,0 +1,86 @@
+package com.ctre.phoenix.motorcontrol;
+
+import com.ctre.phoenix.ErrorCode;
+
+public interface IMotorControllerEnhanced extends IMotorController {
+	 //------ Set output routines. ----------//
+    /* in parent */
+
+    //------ Invert behavior ----------//
+    /* in parent */
+
+    //----- general output shaping ------------------//
+    /* in parent */
+
+    //------ Voltage Compensation ----------//
+    /* in parent */
+
+    //------ General Status ----------//
+    /* in parent */
+
+    //------ sensor selection ----------//
+    /* expand the options */
+	 public ErrorCode configSelectedFeedbackSensor(FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs );
+
+    //------- sensor status --------- //
+    /* in parent */
+
+    //------ status frame period changes ----------//
+    public ErrorCode setStatusFramePeriod(StatusFrameEnhanced frame, int periodMs, int timeoutMs );
+    public int getStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs );
+
+    //----- velocity signal conditionaing ------//
+    public ErrorCode configVelocityMeasurementPeriod(VelocityMeasPeriod period, int timeoutMs );
+    public ErrorCode configVelocityMeasurementWindow(int windowSize, int timeoutMs );
+
+    //------ remote limit switch ----------//
+    /* in parent */
+
+    //------ local limit switch ----------//
+    public ErrorCode configForwardLimitSwitchSource(LimitSwitchSource type, LimitSwitchNormal normalOpenOrClose, int timeoutMs );
+    public ErrorCode configReverseLimitSwitchSource(LimitSwitchSource type, LimitSwitchNormal normalOpenOrClose, int timeoutMs );
+
+    //------ soft limit ----------//
+    /* in parent */
+
+    //------ Current Lim ----------//
+    public ErrorCode configPeakCurrentLimit(int amps, int timeoutMs );
+    public ErrorCode configPeakCurrentDuration(int milliseconds, int timeoutMs );
+    public ErrorCode configContinuousCurrentLimit(int amps, int timeoutMs );
+    public void enableCurrentLimit(boolean enable);
+
+    //------ General Close loop ----------//
+    /* in parent */
+
+    //------ Motion Profile Settings used in Motion Magic and Motion Profile ----------//
+    /* in parent */
+
+    //------ Motion Profile Buffer ----------//
+    /* in parent */
+
+    //------ error ----------//
+    /* in parent */
+
+    //------ Faults ----------//
+    /* in parent */
+
+    //------ Firmware ----------//
+    /* in parent */
+
+    //------ Custom Persistent Params ----------//
+    /* in parent */
+
+    //------ Generic Param API, typically not used ----------//
+    /* in parent */
+
+    //------ Misc. ----------//
+    /* in parent */
+
+    //------ RAW Sensor API ----------//
+    /**
+     * @retrieve object that can get/set individual RAW sensor values.
+     */
+    //SensorCollection SensorCollection { get; }
+    
+    
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/LimitSwitchNormal.java b/java/src/com/ctre/phoenix/motorcontrol/LimitSwitchNormal.java
new file mode 100644
index 0000000..139de84
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/LimitSwitchNormal.java
@@ -0,0 +1,11 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum LimitSwitchNormal {
+	NormallyOpen(0), NormallyClosed(1), Disabled(2);
+
+	public int value;
+
+	LimitSwitchNormal(int value) {
+		this.value = value;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/LimitSwitchSource.java b/java/src/com/ctre/phoenix/motorcontrol/LimitSwitchSource.java
new file mode 100644
index 0000000..5118402
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/LimitSwitchSource.java
@@ -0,0 +1,11 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum LimitSwitchSource {
+	FeedbackConnector(0), RemoteTalonSRX(1), RemoteCANifier(2), Deactivated(3);
+
+	public int value;
+
+	LimitSwitchSource(int value) {
+		this.value = value;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/NeutralMode.java b/java/src/com/ctre/phoenix/motorcontrol/NeutralMode.java
new file mode 100644
index 0000000..e2f56c2
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/NeutralMode.java
@@ -0,0 +1,17 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum NeutralMode {
+	/** Use the NeutralMode that is set by the jumper wire on the CAN device */
+	EEPROMSetting(0),
+	/** Do not attempt to stop the motor. Instead allow it to coast to a stop
+	 without applying resistance. */
+	Coast(1),
+	/** Stop the motor's rotation by applying a force. */
+	Brake(2);
+	
+	public int value;
+	NeutralMode(int value)
+	{
+		this.value = value;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/RemoteFeedbackDevice.java b/java/src/com/ctre/phoenix/motorcontrol/RemoteFeedbackDevice.java
new file mode 100644
index 0000000..e2c54aa
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/RemoteFeedbackDevice.java
@@ -0,0 +1,17 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum RemoteFeedbackDevice {
+	None(-1),
+
+	SensorSum(9),
+	SensorDifference(10),
+	RemoteSensor0(11),
+	RemoteSensor1(12),
+	SoftwareEmulatedSensor(15);
+	
+	public final int value;
+	RemoteFeedbackDevice(int initValue)
+	{
+		this.value = initValue;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/RemoteLimitSwitchSource.java b/java/src/com/ctre/phoenix/motorcontrol/RemoteLimitSwitchSource.java
new file mode 100644
index 0000000..f634561
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/RemoteLimitSwitchSource.java
@@ -0,0 +1,11 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum RemoteLimitSwitchSource {
+	RemoteTalonSRX(1), RemoteCANifier(2), Deactivated(3);
+
+	public int value;
+
+	RemoteLimitSwitchSource(int value) {
+		this.value = value;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/RemoteSensorSource.java b/java/src/com/ctre/phoenix/motorcontrol/RemoteSensorSource.java
new file mode 100644
index 0000000..dfdd4a8
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/RemoteSensorSource.java
@@ -0,0 +1,23 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum RemoteSensorSource {
+	Off(0),
+	TalonSRX_SelectedSensor(1),
+	Pigeon_Yaw(2),
+	Pigeon_Pitch(3),
+	Pigeon_Roll(4),
+	CANifier_Quadrature(5),
+	CANifier_PWMInput0(6),
+	CANifier_PWMInput1(7),
+	CANifier_PWMInput2(8),
+	CANifier_PWMInput3(9),
+	GadgeteerPigeon_Yaw(10),
+	GadgeteerPigeon_Pitch(11),
+	GadgeteerPigeon_Roll(12);
+
+	public int value;
+	RemoteSensorSource(int value)
+	{
+		this.value = value;
+	}
+};
diff --git a/java/src/com/ctre/phoenix/motorcontrol/SensorCollection.java b/java/src/com/ctre/phoenix/motorcontrol/SensorCollection.java
new file mode 100644
index 0000000..7045229
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/SensorCollection.java
@@ -0,0 +1,218 @@
+package com.ctre.phoenix.motorcontrol;
+
+import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix.motorcontrol.can.BaseMotorController;
+import com.ctre.phoenix.motorcontrol.can.MotControllerJNI;
+
+public class SensorCollection {
+
+	private long _handle;
+
+	public SensorCollection(BaseMotorController motorController) {
+		_handle = motorController.getHandle();
+
+	}
+
+	/**
+	 * Get the position of whatever is in the analog pin of the Talon, regardless of
+	 *   whether it is actually being used for feedback.
+	 *
+	 * @return  the 24bit analog value.  The bottom ten bits is the ADC (0 - 1023)
+	 *          on the analog pin of the Talon. The upper 14 bits tracks the overflows and underflows
+	 *          (continuous sensor).
+	 */
+
+	public int getAnalogIn() {
+		return MotControllerJNI.GetAnalogIn(_handle);
+	}
+
+	/**
+	 * Sets analog position.
+	 *
+	 * @param   newPosition The new position.
+	 * @param   timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 *
+	 * @return  an ErrorCode.
+	 */
+
+	public ErrorCode setAnalogPosition(int newPosition, int timeoutMs) {
+		int retval = MotControllerJNI.SetAnalogPosition(_handle, newPosition, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Get the position of whatever is in the analog pin of the Talon, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the ADC (0 - 1023) on analog pin of the Talon.
+	 */
+
+	public int getAnalogInRaw() {
+		return MotControllerJNI.GetAnalogInRaw(_handle);
+	}
+
+	/**
+	 * Get the velocity of whatever is in the analog pin of the Talon, regardless of
+	 *   whether it is actually being used for feedback.
+	 *
+	 * @return  the speed in units per 100ms where 1024 units is one rotation.
+	 */
+
+	public int getAnalogInVel() {
+		return MotControllerJNI.GetAnalogInVel(_handle);
+	}
+
+	/**
+	 * Get the quadrature position of the Talon, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the quadrature position.
+	 */
+
+	public int getQuadraturePosition() {
+		return MotControllerJNI.GetQuadraturePosition(_handle);
+	}
+
+	/**
+	 * Change the quadrature reported position.  Typically this is used to "zero" the
+	 *   sensor. This only works with Quadrature sensor.  To set the selected sensor position
+	 *   regardless of what type it is, see SetSelectedSensorPosition in the motor controller class.
+	 *
+	 * @param   newPosition The position value to apply to the sensor.
+	 * @param   timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 *
+	 * @return  error code.
+	 */
+
+	public ErrorCode setQuadraturePosition(int newPosition, int timeoutMs) {
+		int retval = MotControllerJNI.SetQuadraturePosition(_handle, newPosition, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Get the quadrature velocity, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the quadrature velocity in units per 100ms.
+	 */
+
+	public int getQuadratureVelocity() {
+		return MotControllerJNI.GetQuadratureVelocity(_handle);
+	}
+
+	/**
+	 * Gets pulse width position, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the pulse width position.
+	 */
+
+	public int getPulseWidthPosition() {
+		return MotControllerJNI.GetPulseWidthPosition(_handle);
+	}
+
+	/**
+	 * Sets pulse width position.
+	 *
+	 * @param   newPosition The position value to apply to the sensor.
+	 * @param   timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 *
+	 * @return  an ErrErrorCode
+	 */
+	public ErrorCode setPulseWidthPosition(int newPosition, int timeoutMs) {
+		int retval = MotControllerJNI.SetPulseWidthPosition(_handle, newPosition, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Gets pulse width velocity, regardless of whether
+	 *   it is actually being used for feedback.
+	 *
+	 * @return  the pulse width velocity in units per 100ms (where 4096 units is 1 rotation).
+	 */
+
+	public int getPulseWidthVelocity() {
+		return MotControllerJNI.GetPulseWidthVelocity(_handle);
+	}
+
+	/**
+	 * Gets pulse width rise to fall time.
+	 *
+	 * @return  the pulse width rise to fall time in microseconds.
+	 */
+
+	public int getPulseWidthRiseToFallUs() {
+		return MotControllerJNI.GetPulseWidthRiseToFallUs(_handle);
+	}
+
+	/**
+	 * Gets pulse width rise to rise time.
+	 *
+	 * @return  the pulse width rise to rise time in microseconds.
+	 */
+
+	public int getPulseWidthRiseToRiseUs() {
+		return MotControllerJNI.GetPulseWidthRiseToRiseUs(_handle);
+	}
+
+	/**
+	 * Gets pin state quad a.
+	 *
+	 * @return  the pin state of quad a (1 if asserted, 0 if not asserted).
+	 */
+
+	public boolean getPinStateQuadA() {
+		return MotControllerJNI.GetPinStateQuadA(_handle) != 0;
+	}
+
+	/**
+	 * Gets pin state quad b.
+	 *
+	 * @return  Digital level of QUADB pin (1 if asserted, 0 if not asserted).
+	 */
+
+	public boolean getPinStateQuadB() {
+		return MotControllerJNI.GetPinStateQuadB(_handle) != 0;
+	}
+
+	/**
+	 * Gets pin state quad index.
+	 *
+	 * @return  Digital level of QUAD Index pin (1 if asserted, 0 if not asserted).
+	 */
+
+	public boolean getPinStateQuadIdx() {
+		return MotControllerJNI.GetPinStateQuadIdx(_handle) != 0;
+	}
+
+	/**
+	 * Is forward limit switch closed.
+	 *
+	 * @return  '1' iff forward limit switch is closed, 0 iff switch is open. This function works
+	 *          regardless if limit switch feature is enabled.
+	 */
+
+	public boolean isFwdLimitSwitchClosed() {
+		return MotControllerJNI.IsFwdLimitSwitchClosed(_handle) != 0;
+	}
+
+	/**
+	 * Is reverse limit switch closed.
+	 *
+	 * @return  '1' iff reverse limit switch is closed, 0 iff switch is open. This function works
+	 *          regardless if limit switch feature is enabled.
+	 */
+
+	public boolean isRevLimitSwitchClosed() {
+		return MotControllerJNI.IsRevLimitSwitchClosed(_handle) != 0;
+	}
+}
diff --git a/java/src/com/ctre/phoenix/motorcontrol/SensorTerm.java b/java/src/com/ctre/phoenix/motorcontrol/SensorTerm.java
new file mode 100644
index 0000000..8419a0d
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/SensorTerm.java
@@ -0,0 +1,14 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum SensorTerm {
+	Sum0(0),
+	Sum1(1),
+	Diff0(2),
+	Diff1(3);
+	
+	public int value;
+	SensorTerm(int value)
+	{
+		this.value = value;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/StatusFrame.java b/java/src/com/ctre/phoenix/motorcontrol/StatusFrame.java
new file mode 100644
index 0000000..e6108d6
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/StatusFrame.java
@@ -0,0 +1,30 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum StatusFrame {
+	Status_1_General(0x1400),
+	Status_2_Feedback0(0x1440),
+	Status_4_AinTempVbat(0x14C0),
+	Status_6_Misc(0x1540),
+	Status_7_CommStatus(0x1580),
+	Status_9_MotProfBuffer(0x1600),
+	/**
+	 * Old name for Status 10 Frame.
+	 * Use Status_10_Targets instead.
+	 */
+	Status_10_MotionMagic(0x1640),
+	/**
+	 * Correct name for Status 10 Frame.
+	 * Functionally equivalent to Status_10_MotionMagic
+	 */
+	Status_10_Targets(0x1640),
+	Status_12_Feedback1(0x16C0),
+	Status_13_Base_PIDF0(0x1700),
+	Status_14_Turn_PIDF1(0x1740),
+	Status_15_FirmwareApiStatus(0x1780);
+
+	public int value;
+	StatusFrame(int value)
+	{
+		this.value = value;
+	}
+};
diff --git a/java/src/com/ctre/phoenix/motorcontrol/StatusFrameEnhanced.java b/java/src/com/ctre/phoenix/motorcontrol/StatusFrameEnhanced.java
new file mode 100644
index 0000000..0a8c3c0
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/StatusFrameEnhanced.java
@@ -0,0 +1,25 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum StatusFrameEnhanced {
+	Status_1_General(0x1400),
+	Status_2_Feedback0(0x1440),
+	Status_4_AinTempVbat(0x14C0),
+	Status_6_Misc(0x1540),
+	Status_7_CommStatus(0x1580),
+	Status_9_MotProfBuffer(0x1600),
+	Status_10_MotionMagic(0x1640),
+	Status_12_Feedback1(0x16C0),
+	Status_13_Base_PIDF0(0x1700),
+	Status_14_Turn_PIDF1(0x1740),
+	Status_15_FirmareApiStatus(0x1780),
+
+	Status_3_Quadrature(0x1480),
+	Status_8_PulseWidth(0x15C0),
+	Status_11_UartGadgeteer(0x1680);
+	
+	public int value;
+	StatusFrameEnhanced(int value)
+	{
+		this.value = value;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/StickyFaults.java b/java/src/com/ctre/phoenix/motorcontrol/StickyFaults.java
new file mode 100644
index 0000000..e18f581
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/StickyFaults.java
@@ -0,0 +1,83 @@
+package com.ctre.phoenix.motorcontrol;
+
+public class StickyFaults {
+	public boolean UnderVoltage;
+	public boolean ForwardLimitSwitch;
+	public boolean ReverseLimitSwitch;
+	public boolean ForwardSoftLimit;
+	public boolean ReverseSoftLimit;
+	public boolean ResetDuringEn;
+	public boolean SensorOverflow;
+	public boolean SensorOutOfPhase;
+	public boolean HardwareESDReset;
+	public boolean RemoteLossOfSignal;
+
+	//!< True iff any of the above flags are true.
+	public boolean hasAnyFault() {
+		return 	UnderVoltage |
+				ForwardLimitSwitch |
+				ReverseLimitSwitch |
+				ForwardSoftLimit |
+				ReverseSoftLimit |
+				ResetDuringEn |
+				SensorOverflow |
+				SensorOutOfPhase |
+				HardwareESDReset |
+				RemoteLossOfSignal;
+	}
+	public int toBitfield() {
+		int retval = 0;
+		int mask = 1;
+		retval |= UnderVoltage ? mask : 0; mask <<= 1;
+		retval |= ForwardLimitSwitch ? mask : 0; mask <<= 1;
+		retval |= ReverseLimitSwitch ? mask : 0; mask <<= 1;
+		retval |= ForwardSoftLimit ? mask : 0; mask <<= 1;
+		retval |= ReverseSoftLimit ? mask : 0; mask <<= 1;
+		retval |= ResetDuringEn ? mask : 0; mask <<= 1;
+		retval |= SensorOverflow ? mask : 0; mask <<= 1;
+		retval |= SensorOutOfPhase ? mask : 0; mask <<= 1;
+		retval |= HardwareESDReset ? mask : 0; mask <<= 1;
+		retval |= RemoteLossOfSignal ? mask : 0; mask <<= 1;
+		return retval;
+	}
+	public void update(int bits) {
+		int mask = 1;
+		UnderVoltage = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ForwardLimitSwitch = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ReverseLimitSwitch = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ForwardSoftLimit = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ReverseSoftLimit = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		ResetDuringEn = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		SensorOverflow = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		SensorOutOfPhase = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		HardwareESDReset = ((bits & mask)!=0) ? true : false; mask <<= 1;
+		RemoteLossOfSignal = ((bits & mask)!=0) ? true : false; mask <<= 1;
+	}
+	public StickyFaults() {
+		UnderVoltage = false;
+		ForwardLimitSwitch = false;
+		ReverseLimitSwitch = false;
+		ForwardSoftLimit = false;
+		ReverseSoftLimit = false;
+		ResetDuringEn = false;
+		SensorOverflow = false;
+		SensorOutOfPhase = false;
+		HardwareESDReset = false;
+		RemoteLossOfSignal = false;
+	}
+	public String toString() {
+		StringBuilder work = new StringBuilder();
+		work.append(" UnderVoltage:"); work.append(UnderVoltage ? "1" : "0");
+		work.append( " ForwardLimitSwitch:"); work.append(ForwardLimitSwitch ? "1" : "0");
+		work.append( " ReverseLimitSwitch:"); work.append(ReverseLimitSwitch ? "1" : "0");
+		work.append( " ForwardSoftLimit:"); work.append(ForwardSoftLimit ? "1" : "0");
+		work.append( " ReverseSoftLimit:"); work.append(ReverseSoftLimit ? "1" : "0");
+		work.append( " ResetDuringEn:"); work.append(ResetDuringEn ? "1" : "0");
+		work.append( " SensorOverflow:"); work.append(SensorOverflow ? "1" : "0");
+		work.append( " SensorOutOfPhase:"); work.append(SensorOutOfPhase ? "1" : "0");
+		work.append( " HardwareESDReset:"); work.append(HardwareESDReset ? "1" : "0");
+		work.append( " RemoteLossOfSignal:"); work.append(RemoteLossOfSignal ? "1" : "0");
+		return work.toString();
+	}
+};
+
diff --git a/java/src/com/ctre/phoenix/motorcontrol/VelocityMeasPeriod.java b/java/src/com/ctre/phoenix/motorcontrol/VelocityMeasPeriod.java
new file mode 100644
index 0000000..b708bb1
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/VelocityMeasPeriod.java
@@ -0,0 +1,18 @@
+package com.ctre.phoenix.motorcontrol;
+
+public enum VelocityMeasPeriod {
+	Period_1Ms(1),
+	Period_2Ms(2),
+	Period_5Ms(5),
+	Period_10Ms(10),
+	Period_20Ms(20),
+	Period_25Ms(25),
+	Period_50Ms(50),
+	Period_100Ms(100);
+	
+	public int value;
+	VelocityMeasPeriod(int value)
+	{
+		this.value = value;
+	}
+};
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/motorcontrol/can/BaseMotorController.java b/java/src/com/ctre/phoenix/motorcontrol/can/BaseMotorController.java
new file mode 100644
index 0000000..0e15160
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/can/BaseMotorController.java
@@ -0,0 +1,1787 @@
+package com.ctre.phoenix.motorcontrol.can;
+
+import com.ctre.phoenix.motorcontrol.ControlFrame;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.DemandType;
+import com.ctre.phoenix.motorcontrol.Faults;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.FollowerType;
+import com.ctre.phoenix.motorcontrol.IMotorController;
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.RemoteFeedbackDevice;
+import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.SensorCollection;
+import com.ctre.phoenix.motorcontrol.SensorTerm;
+import com.ctre.phoenix.motorcontrol.StatusFrame;
+import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
+import com.ctre.phoenix.motorcontrol.StickyFaults;
+import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod;
+import com.ctre.phoenix.motorcontrol.can.MotControllerJNI;
+import com.ctre.phoenix.ParamEnum;
+import com.ctre.phoenix.motion.MotionProfileStatus;
+import com.ctre.phoenix.motion.SetValueMotionProfile;
+import com.ctre.phoenix.motion.TrajectoryPoint;
+import com.ctre.phoenix.ErrorCode;
+
+/**
+ * Base motor controller features for all CTRE CAN motor controllers.
+ */
+public abstract class BaseMotorController implements com.ctre.phoenix.motorcontrol.IMotorController {
+
+	private ControlMode m_controlMode = ControlMode.PercentOutput;
+	private ControlMode m_sendMode = ControlMode.PercentOutput;
+
+	private int _arbId = 0;
+	private boolean _invert = false;
+
+	protected long m_handle;
+
+	private int [] _motionProfStats = new int[11];
+
+	private SensorCollection _sensorColl;
+
+	// --------------------- Constructors -----------------------------//
+	/**
+	 * Constructor for motor controllers.
+	 *
+	 * @param arbId
+	 */
+	public BaseMotorController(int arbId) {
+		m_handle = MotControllerJNI.Create(arbId);
+		_arbId = arbId;
+
+		_sensorColl = new SensorCollection(this);
+	}
+	/**
+	 * @return CCI handle for child classes.
+	 */
+	public long getHandle() {
+		return m_handle;
+	}
+
+	/**
+	 * Returns the Device ID
+	 *
+	 * @return Device number.
+	 */
+	public int getDeviceID() {
+		return MotControllerJNI.GetDeviceNumber(m_handle);
+	}
+
+	// ------ Set output routines. ----------//
+	/**
+	 * Sets the appropriate output on the talon, depending on the mode.
+	 * @param mode The output mode to apply.
+	 * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
+	 * In Current mode, output value is in amperes.
+	 * In Velocity mode, output value is in position change / 100ms.
+	 * In Position mode, output value is in encoder ticks or an analog value,
+	 *   depending on the sensor.
+	 * In Follower mode, the output value is the integer device ID of the talon to
+	 * duplicate.
+	 *
+	 * @param outputValue The setpoint value, as described above.
+	 *
+	 *
+	 *	Standard Driving Example:
+	 *	_talonLeft.set(ControlMode.PercentOutput, leftJoy);
+	 *	_talonRght.set(ControlMode.PercentOutput, rghtJoy);
+	 */
+	public void set(ControlMode mode, double outputValue) {
+		set(mode, outputValue, DemandType.Neutral, 0);
+	}
+	/**
+	 * @param mode Sets the appropriate output on the talon, depending on the mode.
+	 * @param demand0 The output value to apply.
+	 * 	such as advanced feed forward and/or auxiliary close-looping in firmware.
+	 * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
+	 * In Current mode, output value is in amperes.
+	 * In Velocity mode, output value is in position change / 100ms.
+	 * In Position mode, output value is in encoder ticks or an analog value,
+	 *   depending on the sensor. See
+	 * In Follower mode, the output value is the integer device ID of the talon to
+	 * duplicate.
+	 *
+	 * @param demand1 Supplemental value.  This will also be control mode specific for future features.
+	 */
+	public void set(ControlMode mode, double demand0, double demand1) {
+		set(mode, demand0, DemandType.Neutral, demand1);
+	}
+	/**
+	 * @param mode Sets the appropriate output on the talon, depending on the mode.
+	 * @param demand0 The output value to apply.
+	 * 	such as advanced feed forward and/or auxiliary close-looping in firmware.
+	 * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
+	 * In Current mode, output value is in amperes.
+	 * In Velocity mode, output value is in position change / 100ms.
+	 * In Position mode, output value is in encoder ticks or an analog value,
+	 *   depending on the sensor. See
+	 * In Follower mode, the output value is the integer device ID of the talon to
+	 * duplicate.
+	 *
+	 * @param demand1Type The demand type for demand1.
+	 * Neutral: Ignore demand1 and apply no change to the demand0 output.
+	 * AuxPID: Use demand1 to set the target for the auxiliary PID 1.
+	 * ArbitraryFeedForward: Use demand1 as an arbitrary additive value to the
+	 *	 demand0 output.  In PercentOutput the demand0 output is the motor output,
+	 *   and in closed-loop modes the demand0 output is the output of PID0.
+	 * @param demand1 Supplmental output value.  Units match the set mode.
+	 *
+	 *
+	 *  Arcade Drive Example:
+	 *		_talonLeft.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, +joyTurn);
+	 *		_talonRght.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, -joyTurn);
+	 *
+	 *	Drive Straight Example:
+	 *	Note: Selected Sensor Configuration is necessary for both PID0 and PID1.
+	 *		_talonLeft.follow(_talonRght, FollwerType.AuxOutput1);
+	 *		_talonRght.set(ControlMode.PercentOutput, joyForward, DemandType.AuxPID, desiredRobotHeading);
+	 *
+	 *	Drive Straight to a Distance Example:
+	 *	Note: Other configurations (sensor selection, PID gains, etc.) need to be set.
+	 *		_talonLeft.follow(_talonRght, FollwerType.AuxOutput1);
+	 *		_talonRght.set(ControlMode.MotionMagic, targetDistance, DemandType.AuxPID, desiredRobotHeading);
+	 */
+	public void set(ControlMode mode, double demand0, DemandType demand1Type, double demand1){
+		m_controlMode = mode;
+		m_sendMode = mode;
+		int work;
+
+		switch (m_controlMode) {
+		case PercentOutput:
+			// case TimedPercentOutput:
+			MotControllerJNI.Set_4(m_handle, m_sendMode.value, demand0, demand1, demand1Type.value);
+			break;
+		case Follower:
+			/* did caller specify device ID */
+			if ((0 <= demand0) && (demand0 <= 62)) { // [0,62]
+				work = getBaseID();
+				work >>= 16;
+				work <<= 8;
+				work |= ((int) demand0) & 0xFF;
+			} else {
+				work = (int) demand0;
+			}
+			/* single precision guarantees 16bits of integral precision,
+		   * so float/double cast on work is safe */
+			MotControllerJNI.Set_4(m_handle, m_sendMode.value, (double)work, demand1, demand1Type.value);
+			break;
+		case Velocity:
+		case Position:
+		case MotionMagic:
+		case MotionProfile:
+		case MotionProfileArc:
+			MotControllerJNI.Set_4(m_handle, m_sendMode.value, demand0, demand1, demand1Type.value);
+			break;
+		case Current:
+			MotControllerJNI.SetDemand(m_handle, m_sendMode.value, (int) (1000. * demand0), 0); /* milliamps */
+			break;
+		case Disabled:
+			/* fall thru... */
+		default:
+			MotControllerJNI.SetDemand(m_handle, m_sendMode.value, 0, 0);
+			break;
+		}
+
+	}
+
+	/**
+	 * Neutral the motor output by setting control mode to disabled.
+	 */
+	public void neutralOutput() {
+		set(ControlMode.Disabled, 0);
+	}
+
+	/**
+	 * Sets the mode of operation during neutral throttle output.
+	 *
+	 * @param neutralMode
+	 *            The desired mode of operation when the Controller output
+	 *            throttle is neutral (ie brake/coast)
+	 **/
+	public void setNeutralMode(NeutralMode neutralMode) {
+		MotControllerJNI.SetNeutralMode(m_handle, neutralMode.value);
+	}
+	/**
+	 * Enables a future feature called "Heading Hold".
+	 * For now this simply updates the CAN signal to the motor controller.
+	 * Future firmware updates will use this.
+	 *
+	 *	@param enable true/false enable
+	 */
+	public void enableHeadingHold(boolean enable) {
+		/* this routine is moot as the Set() call updates the signal on each call */
+		//MotControllerJNI.EnableHeadingHold(m_handle, enable ? 1 : 0);
+	}
+	/**
+	 * For now this simply updates the CAN signal to the motor controller.
+	 * Future firmware updates will use this to control advanced auxiliary loop behavior.
+	 *
+	 *	@param value
+	 */
+	public void selectDemandType(boolean value) {
+		/* this routine is moot as the Set() call updates the signal on each call */
+		//MotControllerJNI.SelectDemandType(m_handle, value ? 1 : 0);
+	}
+
+	// ------ Invert behavior ----------//
+	/**
+	 * Sets the phase of the sensor. Use when controller forward/reverse output
+	 * doesn't correlate to appropriate forward/reverse reading of sensor.
+	 * Pick a value so that positive PercentOutput yields a positive change in sensor.
+	 * After setting this, user can freely call SetInvert() with any value.
+	 *
+	 * @param PhaseSensor
+	 *            Indicates whether to invert the phase of the sensor.
+	 */
+	public void setSensorPhase(boolean PhaseSensor) {
+		MotControllerJNI.SetSensorPhase(m_handle, PhaseSensor);
+	}
+
+	/**
+	 * Inverts the hbridge output of the motor controller.
+	 *
+	 * This does not impact sensor phase and should not be used to correct sensor polarity.
+	 *
+	 * This will invert the hbridge output but NOT the LEDs.
+	 * This ensures....
+	 *  - Green LEDs always represents positive request from robot-controller/closed-looping mode.
+	 *  - Green LEDs correlates to forward limit switch.
+	 *  - Green LEDs correlates to forward soft limit.
+	 *
+	 * @param invert
+	 *            Invert state to set.
+	 */
+	public void setInverted(boolean invert) {
+		_invert = invert; /* cache for getter */
+		MotControllerJNI.SetInverted(m_handle, invert);
+	}
+	/**
+	 * @return invert setting of motor output.
+	 */
+	public boolean getInverted() {
+		return _invert;
+	}
+
+	// ----- general output shaping ------------------//
+	/**
+	 * Configures the open-loop ramp rate of throttle output.
+	 *
+	 * @param secondsFromNeutralToFull
+	 *            Minimum desired time to go from neutral to full throttle. A
+	 *            value of '0' will disable the ramp.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configOpenloopRamp(double secondsFromNeutralToFull, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigOpenLoopRamp(m_handle, secondsFromNeutralToFull, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the closed-loop ramp rate of throttle output.
+	 *
+	 * @param secondsFromNeutralToFull
+	 *            Minimum desired time to go from neutral to full throttle. A
+	 *            value of '0' will disable the ramp.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configClosedloopRamp(double secondsFromNeutralToFull, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigClosedLoopRamp(m_handle, secondsFromNeutralToFull, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the forward peak output percentage.
+	 *
+	 * @param percentOut
+	 *            Desired peak output percentage. [0,1]
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configPeakOutputForward(double percentOut, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigPeakOutputForward(m_handle, percentOut, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the reverse peak output percentage.
+	 *
+	 * @param percentOut
+	 *            Desired peak output percentage.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configPeakOutputReverse(double percentOut, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigPeakOutputReverse(m_handle, percentOut, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Configures the forward nominal output percentage.
+	 *
+	 * @param percentOut
+	 *            Nominal (minimum) percent output. [0,+1]
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configNominalOutputForward(double percentOut, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigNominalOutputForward(m_handle, percentOut, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the reverse nominal output percentage.
+	 *
+	 * @param percentOut
+	 *            Nominal (minimum) percent output. [-1,0]
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configNominalOutputReverse(double percentOut, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigNominalOutputReverse(m_handle, percentOut, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the output deadband percentage.
+	 *
+	 * @param percentDeadband
+	 *            Desired deadband percentage. Minimum is 0.1%, Maximum is 25%.
+	 *            Pass 0.04 for 4% (factory default).
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configNeutralDeadband(double percentDeadband, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigNeutralDeadband(m_handle, percentDeadband, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	// ------ Voltage Compensation ----------//
+	/**
+	 * Configures the Voltage Compensation saturation voltage.
+	 *
+	 * @param voltage
+	 *            This is the max voltage to apply to the hbridge when voltage
+	 *            compensation is enabled.  For example, if 10 (volts) is specified
+	 *            and a TalonSRX is commanded to 0.5 (PercentOutput, closed-loop, etc)
+	 *            then the TalonSRX will attempt to apply a duty-cycle to produce 5V.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configVoltageCompSaturation(double voltage, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigVoltageCompSaturation(m_handle, voltage, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the voltage measurement filter.
+	 *
+	 * @param filterWindowSamples
+	 *            Number of samples in the rolling average of voltage
+	 *            measurement.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configVoltageMeasurementFilter(int filterWindowSamples, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigVoltageMeasurementFilter(m_handle, filterWindowSamples, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Enables voltage compensation. If enabled, voltage compensation works in
+	 * all control modes.
+	 *
+	 * @param enable
+	 *            Enable state of voltage compensation.
+	 **/
+	public void enableVoltageCompensation(boolean enable) {
+		MotControllerJNI.EnableVoltageCompensation(m_handle, enable);
+	}
+
+	// ------ General Status ----------//
+	/**
+	 * Gets the bus voltage seen by the device.
+	 *
+	 * @return The bus voltage value (in volts).
+	 */
+	public double getBusVoltage() {
+		return MotControllerJNI.GetBusVoltage(m_handle);
+	}
+
+	/**
+	 * Gets the output percentage of the motor controller.
+	 *
+	 * @return Output of the motor controller (in percent).
+	 */
+	public double getMotorOutputPercent() {
+		return MotControllerJNI.GetMotorOutputPercent(m_handle);
+	}
+
+	/**
+	 * @return applied voltage to motor  in volts.
+	 */
+	public double getMotorOutputVoltage() {
+		return getBusVoltage() * getMotorOutputPercent();
+	}
+
+	/**
+	 * Gets the output current of the motor controller.
+	 *
+	 * @return The output current (in amps).
+	 */
+	public double getOutputCurrent() {
+		return MotControllerJNI.GetOutputCurrent(m_handle);
+	}
+
+	/**
+	 * Gets the temperature of the motor controller.
+	 *
+	 * @return Temperature of the motor controller (in 'C)
+	 */
+	public double getTemperature() {
+		return MotControllerJNI.GetTemperature(m_handle);
+	}
+
+	// ------ sensor selection ----------//
+	/**
+	 * Select the remote feedback device for the motor controller.
+	 * Most CTRE CAN motor controllers will support remote sensors over CAN.
+	 *
+	 * @param feedbackDevice
+	 *            Remote Feedback Device to select.
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSelectedFeedbackSensor(RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigSelectedFeedbackSensor(m_handle, feedbackDevice.value, pidIdx, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Select the feedback device for the motor controller.
+	 *
+	 * @param feedbackDevice
+	 *            Feedback Device to select.
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSelectedFeedbackSensor(FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigSelectedFeedbackSensor(m_handle, feedbackDevice.value, pidIdx, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * The Feedback Coefficient is a scalar applied to the value of the
+	 * feedback sensor.  Useful when you need to scale your sensor values
+	 * within the closed-loop calculations.  Default value is 1.
+	 *
+	 * Selected Feedback Sensor register in firmware is the decoded sensor value
+	 * multiplied by the Feedback Coefficient.
+	 *
+	 * @param coefficient
+	 *            Feedback Coefficient value.  Maximum value of 1.
+	 *						Resolution is 1/(2^16).  Cannot be 0.
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSelectedFeedbackCoefficient(double coefficient, int pidIdx, int timeoutMs) {
+	  int retval = MotControllerJNI.ConfigSelectedFeedbackCoefficient(m_handle, coefficient, pidIdx, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Select what remote device and signal to assign to Remote Sensor 0 or Remote Sensor 1.
+	 * After binding a remote device and signal to Remote Sensor X, you may select Remote Sensor X
+	 * as a PID source for closed-loop features.
+	 *
+	 * @param deviceID
+ 	 *            The CAN ID of the remote sensor device.
+	 * @param remoteSensorSource
+	 *            The remote sensor device and signal type to bind.
+	 * @param remoteOrdinal
+	 *            0 for configuring Remote Sensor 0
+	 *            1 for configuring Remote Sensor 1
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configRemoteFeedbackFilter(int deviceID, RemoteSensorSource remoteSensorSource, int remoteOrdinal,
+			int timeoutMs) {
+		int retval = MotControllerJNI.ConfigRemoteFeedbackFilter(m_handle, deviceID, remoteSensorSource.value, remoteOrdinal,
+				timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Select what sensor term should be bound to switch feedback device.
+	 * Sensor Sum = Sensor Sum Term 0 - Sensor Sum Term 1
+	 * Sensor Difference = Sensor Diff Term 0 - Sensor Diff Term 1
+	 * The four terms are specified with this routine.  Then Sensor Sum/Difference
+	 * can be selected for closed-looping.
+	 *
+	 * @param sensorTerm Which sensor term to bind to a feedback source.
+	 * @param feedbackDevice The sensor signal to attach to sensorTerm.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSensorTerm(SensorTerm sensorTerm, FeedbackDevice feedbackDevice, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigSensorTerm(m_handle, sensorTerm.value, feedbackDevice.value, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	// ------- sensor status --------- //
+	/**
+	 * Get the selected sensor position (in raw sensor units).
+	 *
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop. See
+	 *            Phoenix-Documentation for how to interpret.
+	 *
+	 * @return Position of selected sensor (in raw sensor units).
+	 */
+	public int getSelectedSensorPosition(int pidIdx) {
+		return MotControllerJNI.GetSelectedSensorPosition(m_handle, pidIdx);
+	}
+
+	/**
+	 * Get the selected sensor velocity.
+	 *
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @return selected sensor (in raw sensor units) per 100ms.
+	 * See Phoenix-Documentation for how to interpret.
+	 */
+	public int getSelectedSensorVelocity(int pidIdx) {
+		return MotControllerJNI.GetSelectedSensorVelocity(m_handle, pidIdx);
+	}
+
+	/**
+	 * Sets the sensor position to the given value.
+	 *
+	 * @param sensorPos
+	 *            Position to set for the selected sensor (in raw sensor units).
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setSelectedSensorPosition(int sensorPos, int pidIdx, int timeoutMs) {
+		int retval = MotControllerJNI.SetSelectedSensorPosition(m_handle, sensorPos, pidIdx, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	// ------ status frame period changes ----------//
+	/**
+	 * Sets the period of the given control frame.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setControlFramePeriod(ControlFrame frame, int periodMs) {
+		int retval = MotControllerJNI.SetControlFramePeriod(m_handle, frame.value, periodMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the period of the given status frame.
+	 *
+	 * User ensure CAN Bus utilization is not high.
+	 *
+	 * This setting is not persistent and is lost when device is reset.
+	 * If this is a concern, calling application can use HasReset()
+	 * to determine if the status frame needs to be reconfigured.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setControlFramePeriod(int frame, int periodMs) {
+		int retval = MotControllerJNI.SetControlFramePeriod(m_handle, frame, periodMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the period of the given status frame.
+	 *
+	 * User ensure CAN Bus utilization is not high.
+	 *
+	 * This setting is not persistent and is lost when device is reset. If this
+	 * is a concern, calling application can use HasReset() to determine if the
+	 * status frame needs to be reconfigured.
+	 *
+	 * @param frameValue
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setStatusFramePeriod(int frameValue, int periodMs, int timeoutMs) {
+		int retval = MotControllerJNI.SetStatusFramePeriod(m_handle, frameValue, periodMs, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setStatusFramePeriod(StatusFrame frame, int periodMs, int timeoutMs) {
+		return setStatusFramePeriod(frame.value, periodMs, timeoutMs);
+	}
+
+	/**
+	 * Gets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame to get the period of.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Period of the given status frame.
+	 */
+	public int getStatusFramePeriod(int frame, int timeoutMs) {
+		return MotControllerJNI.GetStatusFramePeriod(m_handle, frame, timeoutMs);
+	}
+
+	/**
+	 * Gets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame to get the period of.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Period of the given status frame.
+	 */
+	public int getStatusFramePeriod(StatusFrame frame, int timeoutMs) {
+		return MotControllerJNI.GetStatusFramePeriod(m_handle, frame.value, timeoutMs);
+	}
+
+	/**
+	 * Gets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame to get the period of.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Period of the given status frame.
+	 */
+	public int getStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs) {
+		return MotControllerJNI.GetStatusFramePeriod(m_handle, frame.value, timeoutMs);
+	}
+
+	// ----- velocity signal conditionaing ------//
+
+	/**
+	 * Sets the period over which velocity measurements are taken.
+	 *
+	 * @param period
+	 *            Desired period for the velocity measurement. @see
+	 *            #VelocityMeasPeriod
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configVelocityMeasurementPeriod(VelocityMeasPeriod period, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigVelocityMeasurementPeriod(m_handle, period.value, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the number of velocity samples used in the rolling average velocity
+	 * measurement.
+	 *
+	 * @param windowSize
+	 *            Number of samples in the rolling average of velocity
+	 *            measurement. Valid values are 1,2,4,8,16,32. If another value
+	 *            is specified, it will truncate to nearest support value.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configVelocityMeasurementWindow(int windowSize, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigVelocityMeasurementWindow(m_handle, windowSize, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	// ------ remote limit switch ----------//
+	/**
+	 * Configures the forward limit switch for a remote source. For example, a
+	 * CAN motor controller may need to monitor the Limit-F pin of another Talon
+	 * or CANifier.
+	 *
+	 * @param type
+	 *            Remote limit switch source. User can choose between a remote
+	 *            Talon SRX, CANifier, or deactivate the feature.
+	 * @param normalOpenOrClose
+	 *            Setting for normally open, normally closed, or disabled. This
+	 *            setting matches the web-based configuration drop down.
+	 * @param deviceID
+	 *            Device ID of remote source (Talon SRX or CANifier device ID).
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configForwardLimitSwitchSource(RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs) {
+		return configForwardLimitSwitchSource(type.value, normalOpenOrClose.value, deviceID, timeoutMs);
+	}
+
+	/**
+	 * Configures the reverse limit switch for a remote source. For example, a
+	 * CAN motor controller may need to monitor the Limit-R pin of another Talon
+	 * or CANifier.
+	 *
+	 * @param type
+	 *            Remote limit switch source. User can choose between a remote
+	 *            Talon SRX, CANifier, or deactivate the feature.
+	 * @param normalOpenOrClose
+	 *            Setting for normally open, normally closed, or disabled. This
+	 *            setting matches the web-based configuration drop down.
+	 * @param deviceID
+	 *            Device ID of remote source (Talon SRX or CANifier device ID).
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configReverseLimitSwitchSource(RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int deviceID, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigReverseLimitSwitchSource(m_handle, type.value, normalOpenOrClose.value,
+				deviceID, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures a limit switch for a local/remote source.
+	 *
+	 * For example, a CAN motor controller may need to monitor the Limit-R pin
+	 * of another Talon, CANifier, or local Gadgeteer feedback connector.
+	 *
+	 * If the sensor is remote, a device ID of zero is assumed. If that's not
+	 * desired, use the four parameter version of this function.
+	 *
+	 * @param type
+	 *            Limit switch source. @see #LimitSwitchSource User can choose
+	 *            between the feedback connector, remote Talon SRX, CANifier, or
+	 *            deactivate the feature.
+	 * @param normalOpenOrClose
+	 *            Setting for normally open, normally closed, or disabled. This
+	 *            setting matches the web-based configuration drop down.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configForwardLimitSwitchSource(LimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int timeoutMs) {
+		return configForwardLimitSwitchSource(type.value, normalOpenOrClose.value, 0x00000000, timeoutMs);
+	}
+
+	protected ErrorCode configForwardLimitSwitchSource(int typeValue, int normalOpenOrCloseValue, int deviceID,
+			int timeoutMs) {
+		int retval = MotControllerJNI.ConfigForwardLimitSwitchSource(m_handle, typeValue, normalOpenOrCloseValue,
+				deviceID, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures a limit switch for a local/remote source.
+	 *
+	 * For example, a CAN motor controller may need to monitor the Limit-R pin
+	 * of another Talon, CANifier, or local Gadgeteer feedback connector.
+	 *
+	 * If the sensor is remote, a device ID of zero is assumed. If that's not
+	 * desired, use the four parameter version of this function.
+	 *
+	 * @param typeValue
+	 *            Limit switch source. @see #LimitSwitchSource User can choose
+	 *            between the feedback connector, remote Talon SRX, CANifier, or
+	 *            deactivate the feature.
+	 * @param normalOpenOrCloseValue
+	 *            Setting for normally open, normally closed, or disabled. This
+	 *            setting matches the web-based configuration drop down.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	protected ErrorCode configReverseLimitSwitchSource(int typeValue, int normalOpenOrCloseValue, int deviceID,
+			int timeoutMs) {
+		int retval = MotControllerJNI.ConfigReverseLimitSwitchSource(m_handle, typeValue, normalOpenOrCloseValue,
+				deviceID, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the enable state for limit switches.
+	 *
+	 * @param enable
+	 *            Enable state for limit switches.
+	 **/
+	public void overrideLimitSwitchesEnable(boolean enable) {
+		MotControllerJNI.OverrideLimitSwitchesEnable(m_handle, enable);
+	}
+
+	// ------ soft limit ----------//
+	/**
+	 * Configures the forward soft limit threhold.
+	 *
+	 * @param forwardSensorLimit
+	 *            Forward Sensor Position Limit (in raw sensor units).
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configForwardSoftLimitThreshold(int forwardSensorLimit, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigForwardSoftLimitThreshold(m_handle, forwardSensorLimit, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the reverse soft limit threshold.
+	 *
+	 * @param reverseSensorLimit
+	 *            Reverse Sensor Position Limit (in raw sensor units).
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configReverseSoftLimitThreshold(int reverseSensorLimit, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigReverseSoftLimitThreshold(m_handle, reverseSensorLimit, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the forward soft limit enable.
+	 *
+	 * @param enable
+	 *            Forward Sensor Position Limit Enable.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configForwardSoftLimitEnable(boolean enable, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigForwardSoftLimitEnable(m_handle, enable, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the reverse soft limit enable.
+	 *
+	 * @param enable
+	 *            Reverse Sensor Position Limit Enable.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configReverseSoftLimitEnable(boolean enable, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigReverseSoftLimitEnable(m_handle, enable, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Can be used to override-disable the soft limits.
+	 * This function can be used to quickly disable soft limits without
+	 * having to modify the persistent configuration.
+	 *
+	 * @param enable
+	 *            Enable state for soft limit switches.
+	 */
+	public void overrideSoftLimitsEnable(boolean enable) {
+		MotControllerJNI.OverrideSoftLimitsEnable(m_handle, enable);
+	}
+
+	// ------ Current Lim ----------//
+	/* not available in base */
+
+	// ------ General Close loop ----------//
+	/**
+	 * Sets the 'P' constant in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param value
+	 *            Value of the P constant.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode config_kP(int slotIdx, double value, int timeoutMs) {
+		int retval = MotControllerJNI.Config_kP(m_handle, slotIdx,  value, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the 'I' constant in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param value
+	 *            Value of the I constant.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode config_kI(int slotIdx, double value, int timeoutMs) {
+		int retval = MotControllerJNI.Config_kI(m_handle, slotIdx,  value, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the 'D' constant in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param value
+	 *            Value of the D constant.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode config_kD(int slotIdx, double value, int timeoutMs) {
+		int retval = MotControllerJNI.Config_kD(m_handle, slotIdx,  value, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the 'F' constant in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param value
+	 *            Value of the F constant.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode config_kF(int slotIdx, double value, int timeoutMs) {
+		int retval = MotControllerJNI.Config_kF(m_handle, slotIdx,  value, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the Integral Zone constant in the given parameter slot. If the
+	 * (absolute) closed-loop error is outside of this zone, integral
+	 * accumulator is automatically cleared. This ensures than integral wind up
+	 * events will stop after the sensor gets far enough from its target.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param izone
+	 *            Value of the Integral Zone constant (closed loop error units X
+	 *            1ms).
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode config_IntegralZone(int slotIdx, int izone, int timeoutMs) {
+		int retval = MotControllerJNI.Config_IntegralZone(m_handle, slotIdx,  izone, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the allowable closed-loop error in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param allowableClosedLoopError
+	 *            Value of the allowable closed-loop error.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configAllowableClosedloopError(int slotIdx, int allowableClosedLoopError, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigAllowableClosedloopError(m_handle, slotIdx, allowableClosedLoopError,
+				timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the maximum integral accumulator in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param iaccum
+	 *            Value of the maximum integral accumulator (closed loop error
+	 *            units X 1ms).
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configMaxIntegralAccumulator(int slotIdx, double iaccum, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigMaxIntegralAccumulator(m_handle, slotIdx, iaccum, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the peak closed-loop output.  This peak output is slot-specific and
+	 *   is applied to the output of the associated PID loop.
+	 * This setting is seperate from the generic Peak Output setting.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param percentOut
+	 *            Peak Percent Output from 0 to 1.  This value is absolute and
+	 *						the magnitude will apply in both forward and reverse directions.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configClosedLoopPeakOutput(int slotIdx, double percentOut, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigClosedLoopPeakOutput(m_handle, slotIdx, percentOut, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the loop time (in milliseconds) of the PID closed-loop calculations.
+	 * Default value is 1 ms.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param loopTimeMs
+	 *            Loop timing of the closed-loop calculations.  Minimum value of
+	 *						1 ms, maximum of 64 ms.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+  public ErrorCode configClosedLoopPeriod(int slotIdx, int loopTimeMs, int timeoutMs) {
+	  int retval = MotControllerJNI.ConfigClosedLoopPeriod(m_handle, slotIdx, loopTimeMs, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configures the Polarity of the Auxiliary PID (PID1).
+	 *
+	 * Standard Polarity:
+	 *    Primary Output = PID0 + PID1
+	 *    Auxiliary Output = PID0 - PID1
+	 *
+	 * Inverted Polarity:
+	 *    Primary Output = PID0 - PID1
+	 *    Auxiliary Output = PID0 + PID1
+	 *
+	 * @param invert
+	 *            If true, use inverted PID1 output polarity.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code
+	 */
+	public ErrorCode configAuxPIDPolarity(boolean invert, int timeoutMs){
+		return configSetParameter(ParamEnum.ePIDLoopPolarity, invert ? 1:0, 0, 1, timeoutMs);
+	}
+
+	/**
+	 * Sets the integral accumulator. Typically this is used to clear/zero the
+	 * integral accumulator, however some use cases may require seeding the
+	 * accumulator for a faster response.
+	 *
+	 * @param iaccum
+	 *            Value to set for the integral accumulator (closed loop error
+	 *            units X 1ms).
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setIntegralAccumulator(double iaccum, int pidIdx, int timeoutMs) {
+		int retval = MotControllerJNI.SetIntegralAccumulator(m_handle,  iaccum, pidIdx, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Gets the closed-loop error. The units depend on which control mode is in
+	 * use. See Phoenix-Documentation information on units.
+	 *
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @return Closed-loop error value.
+	 */
+	public int getClosedLoopError(int pidIdx) {
+		return MotControllerJNI.GetClosedLoopError(m_handle, pidIdx);
+	}
+
+	/**
+	 * Gets the iaccum value.
+	 *
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @return Integral accumulator value (Closed-loop error X 1ms).
+	 */
+	public double getIntegralAccumulator(int pidIdx) {
+		return MotControllerJNI.GetIntegralAccumulator(m_handle, pidIdx);
+	}
+
+
+	/**
+	 * Gets the derivative of the closed-loop error.
+	 *
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @return The error derivative value.
+	 */
+	public double getErrorDerivative(int pidIdx) {
+		return MotControllerJNI.GetErrorDerivative(m_handle, pidIdx);
+	}
+
+	/**
+	 * Selects which profile slot to use for closed-loop control.
+	 *
+	 * @param slotIdx
+	 *            Profile slot to select.
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 **/
+	public void selectProfileSlot(int slotIdx, int pidIdx) {
+		MotControllerJNI.SelectProfileSlot(m_handle, slotIdx, pidIdx);
+	}
+
+	/**
+	 * Gets the current target of a given closed loop.
+	 *
+	 * @param pidIdx
+	 *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
+	 * @return The closed loop target.
+	 */
+	public int getClosedLoopTarget(int pidIdx) {
+		return MotControllerJNI.GetClosedLoopTarget(m_handle, pidIdx);
+	}
+
+	/**
+	 * Gets the active trajectory target position using
+	 * MotionMagic/MotionProfile control modes.
+	 *
+	 * @return The Active Trajectory Position in sensor units.
+	 */
+	public int getActiveTrajectoryPosition() {
+		return MotControllerJNI.GetActiveTrajectoryPosition(m_handle);
+	}
+
+	/**
+	 * Gets the active trajectory target velocity using
+	 * MotionMagic/MotionProfile control modes.
+	 *
+	 * @return The Active Trajectory Velocity in sensor units per 100ms.
+	 */
+	public int getActiveTrajectoryVelocity() {
+		return MotControllerJNI.GetActiveTrajectoryVelocity(m_handle);
+	}
+
+	/**
+	 * Gets the active trajectory target heading using
+	 * MotionMagicArc/MotionProfileArc control modes.
+	 *
+	 * @return The Active Trajectory Heading in degreees.
+	 */
+	public double getActiveTrajectoryHeading() {
+		return MotControllerJNI.GetActiveTrajectoryHeading(m_handle);
+	}
+
+	// ------ Motion Profile Settings used in Motion Magic and Motion Profile ----------//
+
+	/**
+	 * Sets the Motion Magic Cruise Velocity. This is the peak target velocity
+	 * that the motion magic curve generator can use.
+	 *
+	 * @param sensorUnitsPer100ms
+	 *            Motion Magic Cruise Velocity (in raw sensor units per 100 ms).
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configMotionCruiseVelocity(int sensorUnitsPer100ms, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigMotionCruiseVelocity(m_handle, sensorUnitsPer100ms, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the Motion Magic Acceleration. This is the target acceleration that
+	 * the motion magic curve generator can use.
+	 *
+	 * @param sensorUnitsPer100msPerSec
+	 *            Motion Magic Acceleration (in raw sensor units per 100 ms per
+	 *            second).
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configMotionAcceleration(int sensorUnitsPer100msPerSec, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigMotionAcceleration(m_handle, sensorUnitsPer100msPerSec, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	//------ Motion Profile Buffer ----------//
+	/**
+	 * Clear the buffered motion profile in both controller's RAM (bottom), and in the
+	 * API (top).
+	 */
+	public ErrorCode clearMotionProfileTrajectories() {
+		int retval = MotControllerJNI.ClearMotionProfileTrajectories(m_handle);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Retrieve just the buffer count for the api-level (top) buffer. This
+	 * routine performs no CAN or data structure lookups, so its fast and ideal
+	 * if caller needs to quickly poll the progress of trajectory points being
+	 * emptied into controller's RAM. Otherwise just use GetMotionProfileStatus.
+	 *
+	 * @return number of trajectory points in the top buffer.
+	 */
+	public int getMotionProfileTopLevelBufferCount() {
+		return MotControllerJNI.GetMotionProfileTopLevelBufferCount(m_handle);
+	}
+	/**
+	 * Push another trajectory point into the top level buffer (which is emptied
+	 * into the motor controller's bottom buffer as room allows).
+	 * @param trajPt to push into buffer.
+	 * The members should be filled in with these values...
+	 *
+	 * 		targPos:  servo position in sensor units.
+	 *		targVel:  velocity to feed-forward in sensor units
+	 *                 per 100ms.
+	 * 		profileSlotSelect0  Which slot to get PIDF gains. PID is used for position servo. F is used
+	 *						   as the Kv constant for velocity feed-forward. Typically this is hardcoded
+	 *						   to the a particular slot, but you are free gain schedule if need be.
+	 *						   Choose from [0,3]
+	 *		profileSlotSelect1 Which slot to get PIDF gains for auxiliary PId.
+	 *						   This only has impact during MotionProfileArc Control mode.
+	 *						   Choose from [0,1].
+	 * 	   isLastPoint  set to nonzero to signal motor controller to keep processing this
+	 *                     trajectory point, instead of jumping to the next one
+	 *                     when timeDurMs expires.  Otherwise MP executer will
+	 *                     eventually see an empty buffer after the last point
+	 *                     expires, causing it to assert the IsUnderRun flag.
+	 *                     However this may be desired if calling application
+	 *                     never wants to terminate the MP.
+	 *		zeroPos  set to nonzero to signal motor controller to "zero" the selected
+	 *                 position sensor before executing this trajectory point.
+	 *                 Typically the first point should have this set only thus
+	 *                 allowing the remainder of the MP positions to be relative to
+	 *                 zero.
+	 *		timeDur Duration to apply this trajectory pt.
+	 * 				This time unit is ADDED to the exising base time set by
+	 * 				configMotionProfileTrajectoryPeriod().
+	 * @return CTR_OKAY if trajectory point push ok. ErrorCode if buffer is
+	 *         full due to kMotionProfileTopBufferCapacity.
+	 */
+	public ErrorCode pushMotionProfileTrajectory(TrajectoryPoint trajPt) {
+		int retval = MotControllerJNI.PushMotionProfileTrajectory2(m_handle,
+				trajPt.position, trajPt.velocity, trajPt.auxiliaryPos,
+				trajPt.profileSlotSelect0, trajPt.profileSlotSelect1,
+				trajPt.isLastPoint, trajPt.zeroPos, trajPt.timeDur.value);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Retrieve just the buffer full for the api-level (top) buffer. This
+	 * routine performs no CAN or data structure lookups, so its fast and ideal
+	 * if caller needs to quickly poll. Otherwise just use
+	 * GetMotionProfileStatus.
+	 *
+	 * @return number of trajectory points in the top buffer.
+	 */
+	public boolean isMotionProfileTopLevelBufferFull() {
+		return MotControllerJNI.IsMotionProfileTopLevelBufferFull(m_handle);
+	}
+
+	/**
+	 * This must be called periodically to funnel the trajectory points from the
+	 * API's top level buffer to the controller's bottom level buffer. Recommendation
+	 * is to call this twice as fast as the execution rate of the motion
+	 * profile. So if MP is running with 20ms trajectory points, try calling
+	 * this routine every 10ms. All motion profile functions are thread-safe
+	 * through the use of a mutex, so there is no harm in having the caller
+	 * utilize threading.
+	 */
+	public void processMotionProfileBuffer() {
+		MotControllerJNI.ProcessMotionProfileBuffer(m_handle);
+	}
+	/**
+	 * Retrieve all status information.
+	 * For best performance, Caller can snapshot all status information regarding the
+	 * motion profile executer.
+	 *
+	 * @param statusToFill  Caller supplied object to fill.
+	 *
+	 * The members are filled, as follows...
+	 *
+	 *	topBufferRem:	The available empty slots in the trajectory buffer.
+	 * 	 				The robot API holds a "top buffer" of trajectory points, so your applicaion
+	 * 	 				can dump several points at once.  The API will then stream them into the
+	 * 	 		 		low-level buffer, allowing the motor controller to act on them.
+	 *
+	 *	topBufferRem: The number of points in the top trajectory buffer.
+	 *
+	 *	btmBufferCnt: The number of points in the low level controller buffer.
+	 *
+	 *	hasUnderrun: 	Set if isUnderrun ever gets set.
+	 * 	 	 	 	 	Only is cleared by clearMotionProfileHasUnderrun() to ensure
+	 *
+	 *	isUnderrun:		This is set if controller needs to shift a point from its buffer into
+	 *					the active trajectory point however
+	 *					the buffer is empty.
+	 *					This gets cleared automatically when is resolved.
+	 *
+	 *	activePointValid:	True if the active trajectory point has not empty, false otherwise. The members in activePoint are only valid if this signal is set.
+	 *
+	 *	isLast:	is set/cleared based on the MP executer's current
+	 *                trajectory point's IsLast value.  This assumes
+	 *                IsLast was set when PushMotionProfileTrajectory
+	 *                was used to insert the currently processed trajectory
+	 *                point.
+	 *
+	 *	profileSlotSelect: The currently processed trajectory point's
+	 *      			  selected slot.  This can differ in the currently selected slot used
+	 *       				 for Position and Velocity servo modes
+	 *
+	 *	outputEnable:		The current output mode of the motion profile
+	 *						executer (disabled, enabled, or hold).  When changing the set()
+	 *						value in MP mode, it's important to check this signal to
+	 *						confirm the change takes effect before interacting with the top buffer.
+	 */
+	public ErrorCode getMotionProfileStatus(MotionProfileStatus statusToFill) {
+		int retval = MotControllerJNI.GetMotionProfileStatus2(m_handle, _motionProfStats);
+		statusToFill.topBufferRem = _motionProfStats[0];
+		statusToFill.topBufferCnt = _motionProfStats[1];
+		statusToFill.btmBufferCnt = _motionProfStats[2];
+		statusToFill.hasUnderrun = _motionProfStats[3] != 0;
+		statusToFill.isUnderrun = _motionProfStats[4] != 0;
+		statusToFill.activePointValid = _motionProfStats[5] != 0;
+		statusToFill.isLast = _motionProfStats[6] != 0;
+		statusToFill.profileSlotSelect = _motionProfStats[7];
+		statusToFill.outputEnable = SetValueMotionProfile.valueOf(_motionProfStats[8]);
+		statusToFill.timeDurMs = _motionProfStats[9];
+		statusToFill.profileSlotSelect1 = _motionProfStats[10];
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Clear the "Has Underrun" flag. Typically this is called after application
+	 * has confirmed an underrun had occured.
+	 *
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode clearMotionProfileHasUnderrun(int timeoutMs) {
+		int retval = MotControllerJNI.ClearMotionProfileHasUnderrun(m_handle, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Calling application can opt to speed up the handshaking between the robot
+	 * API and the controller to increase the download rate of the controller's Motion
+	 * Profile. Ideally the period should be no more than half the period of a
+	 * trajectory point.
+	 *
+	 * @param periodMs
+	 *            The transmit period in ms.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode changeMotionControlFramePeriod(int periodMs) {
+		int retval = MotControllerJNI.ChangeMotionControlFramePeriod(m_handle, periodMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * When trajectory points are processed in the motion profile executer, the MPE determines
+	 * how long to apply the active trajectory point by summing baseTrajDurationMs with the
+	 * timeDur of the trajectory point (see TrajectoryPoint).
+	 *
+	 * This allows general selection of the execution rate of the points with 1ms resolution,
+	 * while allowing some degree of change from point to point.
+	 * @param baseTrajDurationMs The base duration time of every trajectory point.
+	 * 							This is summed with the trajectory points unique timeDur.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configMotionProfileTrajectoryPeriod(int baseTrajDurationMs, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigMotionProfileTrajectoryPeriod(m_handle, baseTrajDurationMs, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	// ------ error ----------//
+	/**
+	 * Gets the last error generated by this object. Not all functions return an
+	 * error code but can potentially report errors. This function can be used
+	 * to retrieve those error codes.
+	 *
+	 * @return Last Error Code generated by a function.
+	 */
+	public ErrorCode getLastError() {
+		int retval = MotControllerJNI.GetLastError(m_handle);
+		return ErrorCode.valueOf(retval);
+	}
+
+	// ------ Faults ----------//
+	/**
+	 * Polls the various fault flags.
+	 *
+	 * @param toFill
+	 *            Caller's object to fill with latest fault flags.
+	 * @return Last Error Code generated by a function.
+	 */
+	public ErrorCode getFaults(Faults toFill) {
+		int bits = MotControllerJNI.GetFaults(m_handle);
+		toFill.update(bits);
+		return getLastError();
+	}
+
+	/**
+	 * Polls the various sticky fault flags.
+	 *
+	 * @param toFill
+	 *            Caller's object to fill with latest sticky fault flags.
+	 * @return Last Error Code generated by a function.
+	 */
+	public ErrorCode getStickyFaults(StickyFaults toFill) {
+		int bits = MotControllerJNI.GetStickyFaults(m_handle);
+		toFill.update(bits);
+		return getLastError();
+	}
+
+	/**
+	 * Clears all sticky faults.
+	 *
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Last Error Code generated by a function.
+	 */
+	public ErrorCode clearStickyFaults(int timeoutMs) {
+		int retval = MotControllerJNI.ClearStickyFaults(m_handle, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	// ------ Firmware ----------//
+	/**
+	 * Gets the firmware version of the device.
+	 *
+	 * @return Firmware version of device. For example: version 1-dot-2 is
+	 *         0x0102.
+	 */
+	public int getFirmwareVersion() {
+		return MotControllerJNI.GetFirmwareVersion(m_handle);
+	}
+
+	/**
+	 * Returns true if the device has reset since last call.
+	 *
+	 * @return Has a Device Reset Occurred?
+	 */
+	public boolean hasResetOccurred() {
+		return MotControllerJNI.HasResetOccurred(m_handle);
+	}
+
+	//------ Custom Persistent Params ----------//
+	/**
+	 * Sets the value of a custom parameter. This is for arbitrary use.
+	 *
+	 * Sometimes it is necessary to save calibration/limit/target information in
+	 * the device. Particularly if the device is part of a subsystem that can be
+	 * replaced.
+	 *
+	 * @param newValue
+	 *            Value for custom parameter.
+	 * @param paramIndex
+	 *            Index of custom parameter [0,1]
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSetCustomParam(int newValue, int paramIndex, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigSetCustomParam(m_handle, newValue, paramIndex, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Gets the value of a custom parameter.
+	 *
+	 * @param paramIndex
+	 *            Index of custom parameter [0,1].
+	 * @param timoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Value of the custom param.
+	 */
+	public int configGetCustomParam(int paramIndex, int timoutMs) {
+		int retval = MotControllerJNI.ConfigGetCustomParam(m_handle, paramIndex, timoutMs);
+		return retval;
+	}
+
+	// ------ Generic Param API ----------//
+	/**
+	 * Sets a parameter. Generally this is not used. This can be utilized in -
+	 * Using new features without updating API installation. - Errata
+	 * workarounds to circumvent API implementation. - Allows for rapid testing
+	 * / unit testing of firmware.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param value
+	 *            Value of parameter.
+	 * @param subValue
+	 *            Subvalue for parameter. Maximum value of 255.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSetParameter(ParamEnum param, double value, int subValue, int ordinal, int timeoutMs) {
+		return configSetParameter(param.value, value, subValue, ordinal, timeoutMs);
+	}
+	/**
+	 * Sets a parameter.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param value
+	 *            Value of parameter.
+	 * @param subValue
+	 *            Subvalue for parameter. Maximum value of 255.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSetParameter(int param, double value, int subValue, int ordinal, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigSetParameter(m_handle, param,  value, subValue, ordinal,
+				timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Gets a parameter.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Value of parameter.
+	 */
+	public double configGetParameter(ParamEnum param, int ordinal, int timeoutMs) {
+		return configGetParameter(param.value, ordinal, timeoutMs);
+	}
+	/**
+	 * Gets a parameter.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Value of parameter.
+	 */
+	public double configGetParameter(int param, int ordinal, int timeoutMs) {
+		return MotControllerJNI.ConfigGetParameter(m_handle, param, ordinal, timeoutMs);
+	}
+
+	// ------ Misc. ----------//
+	public int getBaseID() {
+		return _arbId;
+	}
+
+	/**
+	 * @return control mode motor controller is in
+	 */
+	public ControlMode getControlMode() {
+		return m_controlMode;
+	}
+
+	// ----- Follower ------//
+	/**
+	 * Set the control mode and output value so that this motor controller will
+	 * follow another motor controller. Currently supports following Victor SPX
+	 * and Talon SRX.
+	 *
+	 * @param masterToFollow
+	 *						Motor Controller object to follow.
+	 * @param followerType
+	 *						Type of following control.  Use AuxOutput1 to follow the master
+	 *						device's auxiliary output 1.
+	 *						Use PercentOutput for standard follower mode.
+	 */
+	public void follow(IMotorController masterToFollow, FollowerType followerType) {
+		int id32 = masterToFollow.getBaseID();
+		int id24 = id32;
+		id24 >>= 16;
+		id24 = (short) id24;
+		id24 <<= 8;
+		id24 |= (id32 & 0xFF);
+		set(ControlMode.Follower, id24);
+
+		switch (followerType){
+			case PercentOutput:
+				set(ControlMode.Follower, (double)id24);
+				break;
+			case AuxOutput1:
+			  /* follow the motor controller, but set the aux flag
+		     * to ensure we follow the processed output */
+			  set(ControlMode.Follower, (double)id24, DemandType.AuxPID, 0);
+				break;
+			default:
+			  neutralOutput();
+				break;
+		}
+	}
+	/**
+	 * Set the control mode and output value so that this motor controller will
+	 * follow another motor controller. Currently supports following Victor SPX
+	 * and Talon SRX.
+	 */
+	public void follow(IMotorController masterToFollow) {
+    follow(masterToFollow, FollowerType.PercentOutput);
+	}
+	/**
+	 * When master makes a device, this routine is called to signal the update.
+	 */
+	public void valueUpdated() {
+		// MT
+	}
+
+	/**
+	 * @return object that can get/set individual raw sensor values.
+	 */
+	public SensorCollection getSensorCollection() {
+		return _sensorColl;
+	}
+}
diff --git a/java/src/com/ctre/phoenix/motorcontrol/can/MotControllerJNI.java b/java/src/com/ctre/phoenix/motorcontrol/can/MotControllerJNI.java
new file mode 100644
index 0000000..635ce39
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/can/MotControllerJNI.java
@@ -0,0 +1,762 @@
+package com.ctre.phoenix.motorcontrol.can;
+
+import com.ctre.phoenix.CTREJNIWrapper;
+
+public class MotControllerJNI extends CTREJNIWrapper {
+
+	public static native long Create(int baseArbId);
+
+	/**
+	 * Returns the Device ID
+	 *
+	 * @return Device number.
+	 */
+	public static native int GetDeviceNumber(long handle);
+
+	/**
+	 * Sets the demand (output) of the motor controller.
+	 *
+	 * @param mode
+	 *            Control Mode of the Motor Controller
+	 * @param demand0
+	 *            Primary Demand value
+	 * @param demand1
+	 *            Secondary Demand value
+	 **/
+	public static native void SetDemand(long handle, int mode, int demand0, int demand1);
+
+	/**
+	 * Sets the demand (output) of the motor controller.
+	 **/
+	public static native void Set_4(long handle, int mode, double demand0, double demand1, int demand1Type);
+
+	/**
+	 * Sets the mode of operation during neutral throttle output.
+	 *
+	 * @param neutralMode
+	 *            The desired mode of operation when the Controller output
+	 *            throttle is neutral (ie brake/coast)
+	 **/
+	public static native void SetNeutralMode(long handle, int neutralMode);
+
+	/**
+	 * Sets the phase of the sensor. Use when controller forward/reverse output
+	 * doesn't correlate to appropriate forward/reverse reading of sensor.
+	 *
+	 * @param PhaseSensor
+	 *            Indicates whether to invert the phase of the sensor.
+	 **/
+	public static native void SetSensorPhase(long handle, boolean PhaseSensor);
+
+	/**
+	 * Inverts the output of the motor controller. LEDs, sensor phase, and limit
+	 * switches will also be inverted to match the new forward/reverse
+	 * directions.
+	 *
+	 * @param invert
+	 *            Invert state to set.
+	 **/
+	public static native void SetInverted(long handle, boolean invert);
+
+	/**
+	 * Configures the open-loop ramp rate of throttle output.
+	 *
+	 * @param secondsFromNeutralToFull
+	 *            Minimum desired time to go from neutral to full throttle. A
+	 *            value of '0' will disable the ramp.
+	 * @param timeoutMs
+	 *            Timeout value in ms. Function will generate error if config is
+	 *            not successful within timeout.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigOpenLoopRamp(long handle, double secondsFromNeutralToFull, int timeoutMs);
+
+	/**
+	 * Configures the closed-loop ramp rate of throttle output.
+	 *
+	 * @param secondsFromNeutralToFull
+	 *            Minimum desired time to go from neutral to full throttle. A
+	 *            value of '0' will disable the ramp.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigClosedLoopRamp(long handle, double secondsFromNeutralToFull, int timeoutMs);
+
+	/**
+	 * Configures the forward peak output percentage.
+	 *
+	 * @param percentOut
+	 *            Desired peak output percentage.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigPeakOutputForward(long handle, double percentOut, int timeoutMs);
+
+	/**
+	 * Configures the reverse peak output percentage.
+	 *
+	 * @param percentOut
+	 *            Desired peak output percentage.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigPeakOutputReverse(long handle, double percentOut, int timeoutMs);
+
+	/**
+	 * Configures the forward nominal output percentage.
+	 *
+	 * @param percentOut
+	 *            Nominal (minimum) percent output.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigNominalOutputForward(long handle, double percentOut, int timeoutMs);
+
+	/**
+	 * Configures the reverse nominal output percentage.
+	 *
+	 * @param percentOut
+	 *            Nominal (minimum) percent output.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigNominalOutputReverse(long handle, double percentOut, int timeoutMs);
+
+	/**
+	 * Configures the output deadband percentage.
+	 *
+	 * @param percentDeadband
+	 *            Desired deadband percentage. Minimum is 0.1%, Maximum is 25%.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigNeutralDeadband(long handle, double percentDeadband, int timeoutMs);
+
+	/**
+	 * Configures the Voltage Compensation saturation voltage.
+	 *
+	 * @param voltage
+	 *            TO-DO: Comment me!
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigVoltageCompSaturation(long handle, double voltage, int timeoutMs);
+
+	/**
+	 * Configures the voltage measurement filter.
+	 *
+	 * @param filterWindowSamples
+	 *            Number of samples in the rolling average of voltage
+	 *            measurement.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigVoltageMeasurementFilter(long handle, int filterWindowSamples, int timeoutMs);
+
+	/**
+	 * Enables voltage compensation. If enabled, voltage compensation works in
+	 * all control modes.
+	 *
+	 * @param enable
+	 *            Enable state of voltage compensation.
+	 **/
+	public static native void EnableVoltageCompensation(long handle, boolean enable);
+
+	/**
+	 * Gets the bus voltage seen by the motor controller.
+	 *
+	 * @return The bus voltage value (in volts).
+	 */
+	public static native double GetBusVoltage(long handle);
+
+	/**
+	 * Gets the output percentage of the motor controller.
+	 *
+	 * @return Output of the motor controller (in percent).
+	 */
+	public static native double GetMotorOutputPercent(long handle);
+
+	/**
+	 * Gets the output current of the motor controller.
+	 *
+	 * @return Output current (in amps).
+	 */
+	public static native double GetOutputCurrent(long handle);
+
+	/**
+	 * Gets the temperature of the motor controller.
+	 *
+	 * @return The temperature of the motor controller (in 'C)
+	 */
+	public static native double GetTemperature(long handle);
+
+	/**
+	 * Configures the remote feedback filter.
+	 *
+	 * @param handle
+	 *            handle of device.
+	 * @param deviceID
+	 *            ID of remote device.
+	 * @param remoteSensorSource
+	 *            Type of remote sensor.
+	 * @param remoteOrdinal
+	 *            Ordinal of remote source [0-1].
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigRemoteFeedbackFilter(long handle, int deviceID, int remoteSensorSource,
+			int remoteOrdinal, int timeoutMs);
+
+	/**
+	 * Select the feedback device for the motor controller.
+	 *
+	 * @param feedbackDevice
+	 *            Feedback Device to select.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigSelectedFeedbackSensor(long handle, int feedbackDevice, int pidIdx, int timeoutMs);
+
+	public static native int ConfigSensorTerm(long handle, int sensorTerm, int feedbackDevice, int timeoutMs);
+
+	/**
+	 * Get the selected sensor position.
+	 *
+	 * @return Position of selected sensor (in Raw Sensor Units).
+	 */
+	public static native int GetSelectedSensorPosition(long handle, int pidIdx);
+
+	/**
+	 * Get the selected sensor velocity.
+	 *
+	 * @return Velocity of selected sensor (in Raw Sensor Units per 100 ms).
+	 */
+	public static native int GetSelectedSensorVelocity(long handle, int pidIdx);
+
+	/**
+	 * Sets the sensor position to the given value.
+	 *
+	 * @param sensorPos
+	 *            Position to set for the selected sensor (in Raw Sensor Units).
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int SetSelectedSensorPosition(long handle, int sensorPos, int pidIdx, int timeoutMs);
+
+	/**
+	 * Sets the period of the given control frame.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int SetControlFramePeriod(long handle, int frame, int periodMs);
+
+	/**
+	 * Sets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int SetStatusFramePeriod(long handle, int frame, int periodMs, int timeoutMs);
+
+	/**
+	 * Gets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame to get the period of.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return The period of the given status frame.
+	 */
+	public static native int GetStatusFramePeriod(long handle, int frame, int timeoutMs);
+
+	/**
+	 * Sets the period over which velocity measurements are taken.
+	 *
+	 * @param period
+	 *            Desired period for the velocity measurement. @see
+	 *            #VelocityMeasPeriod
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigVelocityMeasurementPeriod(long handle, int period, int timeoutMs);
+
+	/**
+	 * Sets the number of velocity samples used in the rolling average velocity
+	 * measurement.
+	 *
+	 * @param windowSize
+	 *            Number of samples in the rolling average of velocity
+	 *            measurement.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigVelocityMeasurementWindow(long handle, int windowSize, int timeoutMs);
+
+	/**
+	 * Configures the forward limit switch for a remote source.
+	 *
+	 * @param type
+	 *            Remote limit switch source. @see #LimitSwitchSource
+	 * @param normalOpenOrClose
+	 *            Setting for normally open or normally closed.
+	 * @param deviceID
+	 *            Device ID of remote source.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigForwardLimitSwitchSource(long handle, int type, int normalOpenOrClose, int deviceID,
+			int timeoutMs);
+
+	/**
+	 * Configures the reverse limit switch for a remote source.
+	 *
+	 * @param type
+	 *            Remote limit switch source. @see #LimitSwitchSource
+	 * @param normalOpenOrClose
+	 *            Setting for normally open or normally closed.
+	 * @param deviceID
+	 *            Device ID of remote source.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigReverseLimitSwitchSource(long handle, int type, int normalOpenOrClose, int deviceID,
+			int timeoutMs);
+
+	/**
+	 * Sets the enable state for limit switches.
+	 *
+	 * @param enable
+	 *            Enable state for limit switches.
+	 **/
+	public static native void OverrideLimitSwitchesEnable(long handle, boolean enable);
+
+	/**
+	 * Configures the forward soft limit.
+	 *
+	 * @param forwardSensorLimit
+	 *            Forward Sensor Position Limit (in Raw Sensor Units).
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigForwardSoftLimitThreshold(long handle, int forwardSensorLimit, int timeoutMs);
+
+	/**
+	 * Configures the reverse soft limit.
+	 *
+	 * @param reverseSensorLimit
+	 *            Reverse Sensor Position Limit (in Raw Sensor Units).
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigReverseSoftLimitThreshold(long handle, int reverseSensorLimit, int timeoutMs);
+
+	public static native int ConfigForwardSoftLimitEnable(long handle, boolean enable, int timeoutMs);
+
+	public static native int ConfigReverseSoftLimitEnable(long handle, boolean enable, int timeoutMs);
+
+	/**
+	 * Sets the enable state for soft limit switches.
+	 *
+	 * @param enable
+	 *            Enable state for soft limit switches.
+	 **/
+	public static native void OverrideSoftLimitsEnable(long handle, boolean enable);
+
+	/**
+	 * Sets the 'P' constant in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param value
+	 *            Value of the P constant.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int Config_kP(long handle, int slotIdx, double value, int timeoutMs);
+
+	/**
+	 * Sets the 'I' constant in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param value
+	 *            Value of the I constant.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int Config_kI(long handle, int slotIdx, double value, int timeoutMs);
+
+	/**
+	 * Sets the 'D' constant in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param value
+	 *            Value of the D constant.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int Config_kD(long handle, int slotIdx, double value, int timeoutMs);
+
+	/**
+	 * Sets the 'F' constant in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param value
+	 *            Value of the F constant.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int Config_kF(long handle, int slotIdx, double value, int timeoutMs);
+
+	/**
+	 * Sets the Integral Zone constant in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param izone
+	 *            Value of the Integral Zone constant.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int Config_IntegralZone(long handle, int slotIdx, double izone, int timeoutMs);
+
+	/**
+	 * Sets the allowable closed-loop error in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param allowableClosedLoopError
+	 *            Value of the allowable closed-loop error.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigAllowableClosedloopError(long handle, int slotIdx, int allowableClosedLoopError,
+			int timeoutMs);
+
+	/**
+	 * Sets the maximum integral accumulator in the given parameter slot.
+	 *
+	 * @param slotIdx
+	 *            Parameter slot for the constant.
+	 * @param iaccum
+	 *            Value of the maximum integral accumulator.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigMaxIntegralAccumulator(long handle, int slotIdx, double iaccum, int timeoutMs);
+
+	/**
+	 * Sets the integral accumulator.
+	 *
+	 * @param iaccum
+	 *            Value to set for the integral accumulator.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @param pidIdx
+	 *            Which closed loop to manipulate. 0 for primary, 1 for auxiliary
+	 *            secondary loop.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int SetIntegralAccumulator(long handle, double iaccum, int pidIdx, int timeoutMs);
+
+	/**
+	 * Gets the closed-loop error.
+	 *
+	 * @param pidIdx
+	 *            Which closed loop to manipulate. 0 for primary, 1 for auxiliary
+	 *            secondary loop.
+	 * @return Closed-loop error value.
+	 */
+	public static native int GetClosedLoopError(long handle, int pidIdx);
+
+	/**
+	 * Gets the iaccum value.
+	 *
+	 * @param pidIdx
+	 *            Which closed loop to manipulate. 0 for primary, 1 for auxiliary
+	 *            secondary loop.
+	 * @return Integral accumulator value.
+	 */
+	public static native double GetIntegralAccumulator(long handle, int pidIdx);
+
+	/**
+	 * Gets the derivative of the closed-loop error.
+	 *
+	 * @param pidIdx
+	 *            Which closed loop to manipulate. 0 for primary, 1 for auxiliary
+	 *            secondary loop.
+	 * @return Error derivative value.
+	 */
+	public static native double GetErrorDerivative(long handle, int pidIdx);
+
+	/**
+	 * Selects which profile slot to use for closed-loop control.
+	 *
+	 * @param slotIdx
+	 *            Profile slot to select.
+	 * @param pidIdx
+	 *            Which closed loop to manipulate. 0 for primary, 1 for auxiliary
+	 *            secondary loop.
+	 **/
+	public static native void SelectProfileSlot(long handle, int slotIdx, int pidIdx);
+
+	public static native int GetActiveTrajectoryPosition(long handle);
+
+	public static native int GetActiveTrajectoryVelocity(long handle);
+
+	public static native double GetActiveTrajectoryHeading(long handle);
+
+	/**
+	 * Sets the Motion Magic Cruise Velocity.
+	 *
+	 * @param sensorUnitsPer100ms
+	 *            Motion Magic Cruise Velocity (in Raw Sensor Units per 100 ms).
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigMotionCruiseVelocity(long handle, int sensorUnitsPer100ms, int timeoutMs);
+
+	/**
+	 * Sets the Motion Magic Acceleration.
+	 *
+	 * @param sensorUnitsPer100msPerSec
+	 *            Motion Magic Acceleration (in Raw Sensor Units per 100 ms per
+	 *            second).
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigMotionAcceleration(long handle, int sensorUnitsPer100msPerSec, int timeoutMs);
+
+	public static native int ClearMotionProfileTrajectories(long handle);
+
+	public static native int GetMotionProfileTopLevelBufferCount(long handle);
+
+	public static native int PushMotionProfileTrajectory(long handle, double position, double velocity,
+			double headingDeg, int profileSlotSelect, boolean isLastPoint, boolean zeroPos);
+
+	public static native int PushMotionProfileTrajectory2(long handle, double position, double velocity, double headingDeg,
+			int profileSlotSelect0, int profileSlotSelect1, boolean isLastPoint, boolean zeroPos, int durationMs);
+
+	public static native boolean IsMotionProfileTopLevelBufferFull(long handle);
+
+	public static native int ProcessMotionProfileBuffer(long handle);
+
+	public static native int GetMotionProfileStatus(long handle, int[] toFill_9);
+
+	public static native int GetMotionProfileStatus2(long handle, int[] toFill_11);
+
+	public static native int ClearMotionProfileHasUnderrun(long handle, int timeoutMs);
+
+	public static native int ChangeMotionControlFramePeriod(long handle, int periodMs);
+
+	public static native int ConfigMotionProfileTrajectoryPeriod(long handle, int periodMs, int timeoutMs);
+
+	/**
+	 * Gets the last error generated by this object.
+	 *
+	 * @return Last Error Code generated by a function.
+	 */
+	public static native int GetLastError(long handle);
+
+	/**
+	 * Gets the firmware version of the device.
+	 *
+	 * @return Firmware version of device.
+	 */
+	public static native int GetFirmwareVersion(long handle);
+
+	/**
+	 * Returns true if the device has reset.
+	 *
+	 * @return Has a Device Reset Occurred?
+	 */
+	public static native boolean HasResetOccurred(long handle);
+
+	/**
+	 * Sets the value of a custom parameter.
+	 *
+	 * @param newValue
+	 *            Value for custom parameter.
+	 * @param paramIndex
+	 *            Index of custom parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigSetCustomParam(long handle, int newValue, int paramIndex, int timeoutMs);
+
+	/**
+	 * Gets the value of a custom parameter.
+	 *
+	 * @param paramIndex
+	 *            Index of custom parameter.
+	 * @param timoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Value of the custom param.
+	 */
+	public static native int ConfigGetCustomParam(long handle, int paramIndex, int timoutMs);
+
+	/**
+	 * Sets a parameter.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param value
+	 *            Value of parameter.
+	 * @param subValue
+	 *            Subvalue for parameter. Maximum value of 255.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigSetParameter(long handle, int param, double value, int subValue, int ordinal,
+			int timeoutMs);
+
+	/**
+	 * Gets a parameter.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Value of parameter.
+	 */
+	public static native double ConfigGetParameter(long handle, int param, int ordinal, int timeoutMs);
+
+	/**
+	 * Configures the peak current limit of the motor controller.
+	 *
+	 * @param amps
+	 *            Peak current limit (in amps).
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigPeakCurrentLimit(long handle, int amps, int timeoutMs);
+
+	/**
+	 * Configures the maximum time allowed at peak current limit of the motor
+	 * controller.
+	 *
+	 * @param milliseconds
+	 *            Maximum time allowed at peak current limit (in milliseconds).
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigPeakCurrentDuration(long handle, int milliseconds, int timeoutMs);
+
+	/**
+	 * Configures the continuous current limit.
+	 *
+	 * @param amps
+	 *            Continuous Current Limit.
+	 * @param timeoutMs
+	 *            Timeout value in ms. @see #ConfigOpenLoopRamp
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public static native int ConfigContinuousCurrentLimit(long handle, int amps, int timeoutMs);
+
+	/**
+	 * Enables the current limit feature.
+	 *
+	 * @param enable
+	 *            Enable state of current limit.
+	 **/
+	public static native int EnableCurrentLimit(long handle, boolean enable);
+
+	public static native int GetAnalogIn(long handle);
+
+	public static native int SetAnalogPosition(long handle, int newPosition, int timeoutMs);
+
+	public static native int GetAnalogInRaw(long handle);
+
+	public static native int GetAnalogInVel(long handle);
+
+	public static native int GetQuadraturePosition(long handle);
+
+	public static native int SetQuadraturePosition(long handle, int newPosition, int timeoutMs);
+
+	public static native int GetQuadratureVelocity(long handle);
+
+	public static native int GetPulseWidthPosition(long handle);
+
+	public static native int SetPulseWidthPosition(long handle, int newPosition, int timeoutMs);
+
+	public static native int GetPulseWidthVelocity(long handle);
+
+	public static native int GetPulseWidthRiseToFallUs(long handle);
+
+	public static native int GetPulseWidthRiseToRiseUs(long handle);
+
+	public static native int GetPinStateQuadA(long handle);
+
+	public static native int GetPinStateQuadB(long handle);
+
+	public static native int GetPinStateQuadIdx(long handle);
+
+	public static native int IsFwdLimitSwitchClosed(long handle);
+
+	public static native int IsRevLimitSwitchClosed(long handle);
+
+	public static native int GetFaults(long handle);
+
+	public static native int GetStickyFaults(long handle);
+
+	public static native int ClearStickyFaults(long handle, int timeoutMs);
+
+	public static native int SelectDemandType(long handle, int enable);
+
+	public static native int SetMPEOutput(long handle, int mpeOutput);
+
+	public static native int EnableHeadingHold(long handle, int enable);
+
+	public static native int GetClosedLoopTarget(long handle, int pidIdx);
+
+	public static native int ConfigSelectedFeedbackCoefficient(long handle, double coefficient, int pidIdx, int timeoutMs);
+
+	public static native int ConfigClosedLoopPeakOutput(long handle, int slotIdx, double percentOut, int timeoutMs);
+
+	public static native int ConfigClosedLoopPeriod(long handle, int slotIdx, int loopTimeMs, int timeoutMs);
+}
diff --git a/java/src/com/ctre/phoenix/motorcontrol/can/TalonSRX.java b/java/src/com/ctre/phoenix/motorcontrol/can/TalonSRX.java
new file mode 100644
index 0000000..1bbc149
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/can/TalonSRX.java
@@ -0,0 +1,232 @@
+package com.ctre.phoenix.motorcontrol.can;
+import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced;
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
+import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod;
+import com.ctre.phoenix.motorcontrol.can.MotControllerJNI;
+
+import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.wpilibj.hal.HAL;
+/**
+ * CTRE Talon SRX Motor Controller when used on CAN Bus.
+ */
+public class TalonSRX extends com.ctre.phoenix.motorcontrol.can.BaseMotorController
+		implements IMotorControllerEnhanced {
+
+	public TalonSRX(int deviceNumber) {
+		super(deviceNumber | 0x02040000);
+		HAL.report(tResourceType.kResourceType_CANTalonSRX, deviceNumber + 1);
+	}
+	/**
+	 * Sets the period of the given status frame.
+	 *
+	 * User ensure CAN Bus utilization is not high.
+	 *
+	 * This setting is not persistent and is lost when device is reset.
+	 * If this is a concern, calling application can use HasReset()
+	 * to determine if the status frame needs to be reconfigured.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setStatusFramePeriod(StatusFrameEnhanced frame, int periodMs, int timeoutMs) {
+		return super.setStatusFramePeriod(frame.value, periodMs, timeoutMs);
+	}
+	/**
+	 * Gets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame to get the period of.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Period of the given status frame.
+	 */
+	public int getStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs) {
+
+		return super.getStatusFramePeriod(frame, timeoutMs);
+	}
+	/**
+	 * Configures the period of each velocity sample.
+	 * Every 1ms a position value is sampled, and the delta between that sample
+	 * and the position sampled kPeriod ms ago is inserted into a filter.
+	 * kPeriod is configured with this function.
+	 *
+	 * @param period
+	 *            Desired period for the velocity measurement. @see
+	 *            #VelocityMeasPeriod
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configVelocityMeasurementPeriod(VelocityMeasPeriod period, int timeoutMs) {
+		return super.configVelocityMeasurementPeriod(period, timeoutMs);
+	}
+	/**
+	 * Sets the number of velocity samples used in the rolling average velocity
+	 * measurement.
+	 *
+	 * @param windowSize
+	 *            Number of samples in the rolling average of velocity
+	 *            measurement. Valid values are 1,2,4,8,16,32. If another
+	 *            value is specified, it will truncate to nearest support value.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configVelocityMeasurementWindow(int windowSize, int timeoutMs) {
+		return super.configVelocityMeasurementWindow(windowSize, timeoutMs);
+	}
+	/**
+	 * Configures a limit switch for a local/remote source.
+	 *
+	 * For example, a CAN motor controller may need to monitor the Limit-R pin
+	 * of another Talon, CANifier, or local Gadgeteer feedback connector.
+	 *
+	 * If the sensor is remote, a device ID of zero is assumed.
+	 * If that's not desired, use the four parameter version of this function.
+	 *
+	 * @param type
+	 *            Limit switch source.
+	 *            User can choose between the feedback connector, remote Talon SRX, CANifier, or deactivate the feature.
+	 * @param normalOpenOrClose
+	 *            Setting for normally open, normally closed, or disabled. This setting
+	 *            matches the web-based configuration drop down.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for
+	 *            config success and report an error if it times out.
+	 *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configForwardLimitSwitchSource(LimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int timeoutMs) {
+
+		return super.configForwardLimitSwitchSource(type.value, normalOpenOrClose.value, 0x00000000, timeoutMs);
+	}
+	/**
+	 * Configures a limit switch for a local/remote source.
+	 *
+	 * For example, a CAN motor controller may need to monitor the Limit-R pin
+	 * of another Talon, CANifier, or local Gadgeteer feedback connector.
+	 *
+	 * If the sensor is remote, a device ID of zero is assumed. If that's not
+	 * desired, use the four parameter version of this function.
+	 *
+	 * @param type
+	 *            Limit switch source. @see #LimitSwitchSource User can choose
+	 *            between the feedback connector, remote Talon SRX, CANifier, or
+	 *            deactivate the feature.
+	 * @param normalOpenOrClose
+	 *            Setting for normally open, normally closed, or disabled. This
+	 *            setting matches the web-based configuration drop down.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configReverseLimitSwitchSource(LimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+			int timeoutMs) {
+		return super.configReverseLimitSwitchSource(type.value, normalOpenOrClose.value, 0x00000000, timeoutMs);
+	}
+
+	// ------ Current Lim ----------//
+	/**
+	 * Configure the peak allowable current (when current limit is enabled).
+	 * 
+	 * Current limit is activated when current exceeds the peak limit for longer
+	 * than the peak duration. Then software will limit to the continuous limit.
+	 * This ensures current limiting while allowing for momentary excess current
+	 * events.
+	 *
+	 * For simpler current-limiting (single threshold) use
+	 * ConfigContinuousCurrentLimit() and set the peak to zero:
+	 * ConfigPeakCurrentLimit(0).
+	 * 
+	 * @param amps
+	 *            Amperes to limit.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 */
+	public ErrorCode configPeakCurrentLimit(int amps, int timeoutMs) {
+		int retval =  MotControllerJNI.ConfigPeakCurrentLimit(m_handle, amps, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configure the peak allowable duration (when current limit is enabled).
+	 *
+	 * Current limit is activated when current exceeds the peak limit for longer
+	 * than the peak duration. Then software will limit to the continuous limit.
+	 * This ensures current limiting while allowing for momentary excess current
+	 * events.
+	 *
+	 * For simpler current-limiting (single threshold) use
+	 * ConfigContinuousCurrentLimit() and set the peak to zero:
+	 * ConfigPeakCurrentLimit(0).
+	 * 
+	 * @param milliseconds
+	 *            How long to allow current-draw past peak limit.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 */
+	public ErrorCode configPeakCurrentDuration(int milliseconds, int timeoutMs) {
+		int retval = MotControllerJNI.ConfigPeakCurrentDuration(m_handle, milliseconds, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Configure the continuous allowable current-draw (when current limit is
+	 * enabled).
+	 *
+	 * Current limit is activated when current exceeds the peak limit for longer
+	 * than the peak duration. Then software will limit to the continuous limit.
+	 * This ensures current limiting while allowing for momentary excess current
+	 * events.
+	 *
+	 * For simpler current-limiting (single threshold) use
+	 * ConfigContinuousCurrentLimit() and set the peak to zero:
+	 * ConfigPeakCurrentLimit(0).
+	 * 
+	 * @param amps
+	 *            Amperes to limit.
+	 * @param timeoutMs
+	 *            Timeout value in ms. If nonzero, function will wait for config
+	 *            success and report an error if it times out. If zero, no
+	 *            blocking or checking is performed.
+	 */
+	public ErrorCode configContinuousCurrentLimit(int amps, int timeoutMs) {
+		int retval =  MotControllerJNI.ConfigContinuousCurrentLimit(m_handle, amps, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Enable or disable Current Limit.
+	 * 
+	 * @param enable
+	 *            Enable state of current limit.
+	 * @see ConfigPeakCurrentLimit, ConfigPeakCurrentDuration,
+	 *      ConfigContinuousCurrentLimit
+	 */
+	public void enableCurrentLimit(boolean enable) {
+		MotControllerJNI.EnableCurrentLimit(m_handle, enable);
+	}
+}
diff --git a/java/src/com/ctre/phoenix/motorcontrol/can/VictorSPX.java b/java/src/com/ctre/phoenix/motorcontrol/can/VictorSPX.java
new file mode 100644
index 0000000..5bf8313
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/can/VictorSPX.java
@@ -0,0 +1,21 @@
+package com.ctre.phoenix.motorcontrol.can;
+
+import com.ctre.phoenix.motorcontrol.IMotorController;
+
+import edu.wpi.first.wpilibj.hal.HAL;
+/**
+ * VEX Victor SPX Motor Controller when used on CAN Bus.
+ */
+public class VictorSPX extends com.ctre.phoenix.motorcontrol.can.BaseMotorController
+    implements IMotorController {
+	/**
+	 * Constructor
+	 * 
+	 * @param deviceNumber
+	 *            [0,62]
+	 */
+	public VictorSPX(int deviceNumber) {
+		super(deviceNumber | 0x01040000);
+		HAL.report(65, deviceNumber + 1);
+	}
+}
diff --git a/java/src/com/ctre/phoenix/motorcontrol/can/WPI_TalonSRX.java b/java/src/com/ctre/phoenix/motorcontrol/can/WPI_TalonSRX.java
new file mode 100644
index 0000000..5fd93f2
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/can/WPI_TalonSRX.java
@@ -0,0 +1,231 @@
+/**
+ * WPI Compliant motor controller class.
+ * WPILIB's object model requires many interfaces to be implemented to use
+ * the various features.
+ * This includes...
+ * - Software PID loops running in the robot controller
+ * - LiveWindow/Test mode features
+ * - Motor Safety (auto-turn off of motor if Set stops getting called)
+ * - Single Parameter set that assumes a simple motor controller.
+ */
+package com.ctre.phoenix.motorcontrol.can;
+
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import edu.wpi.first.wpilibj.MotorSafety;
+import edu.wpi.first.wpilibj.MotorSafetyHelper;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.hal.HAL;
+
+public class WPI_TalonSRX extends TalonSRX implements SpeedController, Sendable, MotorSafety {
+
+	private String _description;
+	private double _speed;
+	private MotorSafetyHelper _safetyHelper;
+
+	/** Constructor */
+	public WPI_TalonSRX(int deviceNumber) {
+		super(deviceNumber);
+		HAL.report(66, deviceNumber + 1);
+		_description = "Talon SRX " + deviceNumber;
+		/* prep motor safety */
+		_safetyHelper = new MotorSafetyHelper(this);
+		_safetyHelper.setExpiration(0.0);
+		_safetyHelper.setSafetyEnabled(false);
+
+		LiveWindow.add(this);
+		setName("Talon SRX ", deviceNumber);
+	}
+
+	// ------ set/get routines for WPILIB interfaces ------//
+	@Override
+	public void set(double speed) {
+		_speed = speed;
+		set(ControlMode.PercentOutput, _speed);
+		_safetyHelper.feed();
+	}
+
+	@Override
+	public void pidWrite(double output) {
+		set(output);
+	}
+
+	/**
+	 * Common interface for getting the current set speed of a speed controller.
+	 *
+	 * @return The current set speed. Value is between -1.0 and 1.0.
+	 */
+	@Override
+	public double get() {
+		return _speed;
+	}
+
+	// ---------Intercept CTRE calls for motor safety ---------//
+	public void set(ControlMode mode, double value) {
+		/* intercept the advanced Set and feed motor-safety */
+		super.set(mode, value);
+		_safetyHelper.feed();
+	}
+
+	public void set(ControlMode mode, double demand0, double demand1) {
+		/* intercept the advanced Set and feed motor-safety */
+		super.set(mode, demand0, demand1);
+		_safetyHelper.feed();
+	}
+
+	// ----------------------- Invert routines -------------------//
+	@Override
+	public void setInverted(boolean isInverted) {
+		super.setInverted(isInverted);
+	}
+
+	@Override
+	public boolean getInverted() {
+		return super.getInverted();
+	}
+
+	// ----------------------- turn-motor-off routines-------------------//
+	@Override
+	public void disable() {
+		neutralOutput();
+	}
+
+	/**
+	 * Common interface to stop the motor until Set is called again.
+	 */
+	@Override
+	public void stopMotor() {
+		neutralOutput();
+	}
+	// -------- Motor Safety--------//
+
+	/**
+	 * Set the safety expiration time.
+	 *
+	 * @param timeout
+	 *            The timeout (in seconds) for this motor object
+	 */
+	@Override
+	public void setExpiration(double timeout) {
+		_safetyHelper.setExpiration(timeout);
+	}
+
+	/**
+	 * Return the safety expiration time.
+	 *
+	 * @return The expiration time value.
+	 */
+	@Override
+	public double getExpiration() {
+		return _safetyHelper.getExpiration();
+	}
+
+	/**
+	 * Check if the motor is currently alive or stopped due to a timeout.
+	 *
+	 * @return a bool value that is true if the motor has NOT timed out and
+	 *         should still be running.
+	 */
+	@Override
+	public boolean isAlive() {
+		return _safetyHelper.isAlive();
+	}
+
+	/**
+	 * Check if motor safety is enabled.
+	 *
+	 * @return True if motor safety is enforced for this object
+	 */
+	@Override
+	public boolean isSafetyEnabled() {
+		return _safetyHelper.isSafetyEnabled();
+	}
+
+	@Override
+	public void setSafetyEnabled(boolean enabled) {
+		_safetyHelper.setSafetyEnabled(enabled);
+	}
+
+	// ---- essentially a copy of SendableBase -------//
+	private String m_name = "";
+	private String m_subsystem = "Ungrouped";
+
+	/**
+	 * Free the resources used by this object.
+	 */
+	public void free() {
+		LiveWindow.remove(this);
+	}
+
+	@Override
+	public final synchronized String getName() {
+		return m_name;
+	}
+
+	@Override
+	public final synchronized void setName(String name) {
+		m_name = name;
+	}
+
+	/**
+	 * Sets the name of the sensor with a channel number.
+	 *
+	 * @param moduleType
+	 *            A string that defines the module name in the label for the
+	 *            value
+	 * @param channel
+	 *            The channel number the device is plugged into
+	 */
+	protected final void setName(String moduleType, int channel) {
+		setName(moduleType + "[" + channel + "]");
+	}
+
+	/**
+	 * Sets the name of the sensor with a module and channel number.
+	 *
+	 * @param moduleType
+	 *            A string that defines the module name in the label for the
+	 *            value
+	 * @param moduleNumber
+	 *            The number of the particular module type
+	 * @param channel
+	 *            The channel number the device is plugged into (usually PWM)
+	 */
+	protected final void setName(String moduleType, int moduleNumber, int channel) {
+		setName(moduleType + "[" + moduleNumber + "," + channel + "]");
+	}
+
+	@Override
+	public final synchronized String getSubsystem() {
+		return m_subsystem;
+	}
+
+	@Override
+	public final synchronized void setSubsystem(String subsystem) {
+		m_subsystem = subsystem;
+	}
+
+	/**
+	 * Add a child component.
+	 *
+	 * @param child
+	 *            child component
+	 */
+	protected final void addChild(Object child) {
+		LiveWindow.addChild(this, child);
+	}
+
+	@Override
+	public void initSendable(SendableBuilder builder) {
+		builder.setSmartDashboardType("Speed Controller");
+		builder.setSafeState(this::stopMotor);
+		builder.addDoubleProperty("Value", this::get, this::set);
+	}
+
+	@Override
+	public String getDescription() {
+		return _description;
+	}
+}
diff --git a/java/src/com/ctre/phoenix/motorcontrol/can/WPI_VictorSPX.java b/java/src/com/ctre/phoenix/motorcontrol/can/WPI_VictorSPX.java
new file mode 100644
index 0000000..9c7436f
--- /dev/null
+++ b/java/src/com/ctre/phoenix/motorcontrol/can/WPI_VictorSPX.java
@@ -0,0 +1,231 @@
+/**
+ * WPI Compliant motor controller class.
+ * WPILIB's object model requires many interfaces to be implemented to use
+ * the various features.
+ * This includes...
+ * - Software PID loops running in the robot controller
+ * - LiveWindow/Test mode features
+ * - Motor Safety (auto-turn off of motor if Set stops getting called)
+ * - Single Parameter set that assumes a simple motor controller.
+ */
+package com.ctre.phoenix.motorcontrol.can;
+
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import edu.wpi.first.wpilibj.MotorSafety;
+import edu.wpi.first.wpilibj.MotorSafetyHelper;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.hal.HAL;
+
+public class WPI_VictorSPX extends VictorSPX implements SpeedController, Sendable, MotorSafety {
+
+	private String _description;
+	private double _speed;
+	private MotorSafetyHelper _safetyHelper;
+
+	/** Constructor */
+	public WPI_VictorSPX(int deviceNumber) {
+		super(deviceNumber);
+		HAL.report(67, deviceNumber + 1);
+		_description = "Victor SPX " + deviceNumber;
+		/* prep motor safety */
+		_safetyHelper = new MotorSafetyHelper(this);
+		_safetyHelper.setExpiration(0.0);
+		_safetyHelper.setSafetyEnabled(false);
+
+		LiveWindow.add(this);
+		setName("Victor SPX ", deviceNumber);
+	}
+
+	// ------ set/get routines for WPILIB interfaces ------//
+	@Override
+	public void set(double speed) {
+		_speed = speed;
+		set(ControlMode.PercentOutput, _speed);
+		_safetyHelper.feed();
+	}
+
+	@Override
+	public void pidWrite(double output) {
+		set(output);
+	}
+
+	/**
+	 * Common interface for getting the current set speed of a speed controller.
+	 *
+	 * @return The current set speed. Value is between -1.0 and 1.0.
+	 */
+	@Override
+	public double get() {
+		return _speed;
+	}
+
+	// ---------Intercept CTRE calls for motor safety ---------//
+	public void set(ControlMode mode, double value) {
+		/* intercept the advanced Set and feed motor-safety */
+		super.set(mode, value);
+		_safetyHelper.feed();
+	}
+
+	public void set(ControlMode mode, double demand0, double demand1) {
+		/* intercept the advanced Set and feed motor-safety */
+		super.set(mode, demand0, demand1);
+		_safetyHelper.feed();
+	}
+
+	// ----------------------- Invert routines -------------------//
+	@Override
+	public void setInverted(boolean isInverted) {
+		super.setInverted(isInverted);
+	}
+
+	@Override
+	public boolean getInverted() {
+		return super.getInverted();
+	}
+
+	// ----------------------- turn-motor-off routines-------------------//
+	@Override
+	public void disable() {
+		neutralOutput();
+	}
+
+	/**
+	 * Common interface to stop the motor until Set is called again.
+	 */
+	@Override
+	public void stopMotor() {
+		neutralOutput();
+	}
+	// -------- Motor Safety--------//
+
+	/**
+	 * Set the safety expiration time.
+	 *
+	 * @param timeout
+	 *            The timeout (in seconds) for this motor object
+	 */
+	@Override
+	public void setExpiration(double timeout) {
+		_safetyHelper.setExpiration(timeout);
+	}
+
+	/**
+	 * Return the safety expiration time.
+	 *
+	 * @return The expiration time value.
+	 */
+	@Override
+	public double getExpiration() {
+		return _safetyHelper.getExpiration();
+	}
+
+	/**
+	 * Check if the motor is currently alive or stopped due to a timeout.
+	 *
+	 * @return a bool value that is true if the motor has NOT timed out and
+	 *         should still be running.
+	 */
+	@Override
+	public boolean isAlive() {
+		return _safetyHelper.isAlive();
+	}
+
+	/**
+	 * Check if motor safety is enabled.
+	 *
+	 * @return True if motor safety is enforced for this object
+	 */
+	@Override
+	public boolean isSafetyEnabled() {
+		return _safetyHelper.isSafetyEnabled();
+	}
+
+	@Override
+	public void setSafetyEnabled(boolean enabled) {
+		_safetyHelper.setSafetyEnabled(enabled);
+	}
+
+	// ---- essentially a copy of SendableBase -------//
+	private String m_name = "";
+	private String m_subsystem = "Ungrouped";
+
+	/**
+	 * Free the resources used by this object.
+	 */
+	public void free() {
+		LiveWindow.remove(this);
+	}
+
+	@Override
+	public final synchronized String getName() {
+		return m_name;
+	}
+
+	@Override
+	public final synchronized void setName(String name) {
+		m_name = name;
+	}
+
+	/**
+	 * Sets the name of the sensor with a channel number.
+	 *
+	 * @param moduleType
+	 *            A string that defines the module name in the label for the
+	 *            value
+	 * @param channel
+	 *            The channel number the device is plugged into
+	 */
+	protected final void setName(String moduleType, int channel) {
+		setName(moduleType + "[" + channel + "]");
+	}
+
+	/**
+	 * Sets the name of the sensor with a module and channel number.
+	 *
+	 * @param moduleType
+	 *            A string that defines the module name in the label for the
+	 *            value
+	 * @param moduleNumber
+	 *            The number of the particular module type
+	 * @param channel
+	 *            The channel number the device is plugged into (usually PWM)
+	 */
+	protected final void setName(String moduleType, int moduleNumber, int channel) {
+		setName(moduleType + "[" + moduleNumber + "," + channel + "]");
+	}
+
+	@Override
+	public final synchronized String getSubsystem() {
+		return m_subsystem;
+	}
+
+	@Override
+	public final synchronized void setSubsystem(String subsystem) {
+		m_subsystem = subsystem;
+	}
+
+	/**
+	 * Add a child component.
+	 *
+	 * @param child
+	 *            child component
+	 */
+	protected final void addChild(Object child) {
+		LiveWindow.addChild(this, child);
+	}
+
+	@Override
+	public void initSendable(SendableBuilder builder) {
+		builder.setSmartDashboardType("Speed Controller");
+		builder.setSafeState(this::stopMotor);
+		builder.addDoubleProperty("Value", this::get, this::set);
+	}
+
+	@Override
+	public String getDescription() {
+		return _description;
+	}
+}
diff --git a/java/src/com/ctre/phoenix/schedulers/ConcurrentScheduler.java b/java/src/com/ctre/phoenix/schedulers/ConcurrentScheduler.java
new file mode 100644
index 0000000..bfc0a5d
--- /dev/null
+++ b/java/src/com/ctre/phoenix/schedulers/ConcurrentScheduler.java
@@ -0,0 +1,95 @@
+package com.ctre.phoenix.schedulers;
+import java.util.ArrayList;
+
+import com.ctre.phoenix.ILoopable;
+
+public class ConcurrentScheduler implements ILoopable {
+	ArrayList<ILoopable> _loops = new ArrayList<ILoopable>();
+	ArrayList<Boolean> _enabs = new ArrayList<Boolean>();
+
+	public void add(ILoopable aLoop, boolean enable) {
+		_loops.add(aLoop);
+		_enabs.add(enable);
+	}
+	public void add(ILoopable aLoop) {
+		add(aLoop, true);
+	}
+
+	public void removeAll() {
+		_loops.clear();
+		_enabs.clear();
+	}
+
+	public void start(ILoopable toStart) {
+		for (int i = 0; i < _loops.size(); ++i) {
+			ILoopable lp = (ILoopable) _loops.get(i);
+
+			if (lp == toStart) {
+				_enabs.set(i, true);
+				lp.onStart();
+				return;
+			}
+		}
+
+	}
+
+	public void stop(ILoopable toStop) {
+		for (int i = 0; i < (int) _loops.size(); ++i) {
+			ILoopable lp = (ILoopable) _loops.get(i);
+
+			if (lp == toStop) {
+				_enabs.set(i, false);
+				lp.onStop();
+				return;
+			}
+		}
+	}
+
+	public void startAll() { // All Loops
+
+		for (ILoopable loop : _loops) {
+			loop.onStart();
+		}
+		for (int i = 0; i < _enabs.size(); ++i) {
+			_enabs.set(i,  true);
+		}
+	}
+
+	public void stopAll() { // All Loops
+		for (ILoopable loop : _loops) {
+			loop.onStop();
+		}
+		for (int i = 0; i < _enabs.size(); ++i) {
+			_enabs.set(i,  false);
+		}
+	}
+
+	public void process() {
+		for (int i = 0; i < (int) _loops.size(); ++i) {
+			ILoopable loop = (ILoopable) _loops.get(i);
+			boolean en = (boolean) _enabs.get(i);
+			if (en) {
+				loop.onLoop();
+			} else {
+				/* Current ILoopable is turned off, don't call onLoop for it */
+			}
+		}
+	}
+
+	/* ILoopable */
+	public void onStart() {
+		startAll();
+	}
+
+	public void onLoop() {
+		process();
+	}
+
+	public void onStop() {
+		stopAll();
+	}
+
+	public boolean isDone() {
+		return false;
+	}
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/schedulers/SequentialScheduler.java b/java/src/com/ctre/phoenix/schedulers/SequentialScheduler.java
new file mode 100644
index 0000000..ba2b238
--- /dev/null
+++ b/java/src/com/ctre/phoenix/schedulers/SequentialScheduler.java
@@ -0,0 +1,97 @@
+package com.ctre.phoenix.schedulers;
+import java.util.ArrayList;
+
+public class SequentialScheduler implements com.ctre.phoenix.ILoopable
+{
+	boolean _running = false;
+	ArrayList<com.ctre.phoenix.ILoopable> _loops = new ArrayList<com.ctre.phoenix.ILoopable>();
+	int _periodMs;
+	int _idx;
+	boolean _iterated = false;
+
+	public SequentialScheduler(int periodMs)
+	{
+		_periodMs = periodMs;
+	}
+	public void add(com.ctre.phoenix.ILoopable aLoop)
+	{
+		_loops.add(aLoop);
+	}
+
+	public com.ctre.phoenix.ILoopable getCurrent()
+	{
+		return null;
+	}
+
+	public void removeAll()
+	{
+		_loops.clear();
+	}
+
+	public void start()
+	{
+		_idx = 0;
+		if(_loops.size() > 0)
+			_loops.get(0).onStart();
+		
+		_running = true;
+	}
+	public void stop()
+	{
+		for (int i = 0; i < _loops.size(); i++)
+		{
+			_loops.get(i).onStop();
+		}
+		_running = false;
+	}
+	public void process()
+	{
+		_iterated = false;
+		if (_idx < _loops.size())
+		{
+			if (_running)
+			{
+				com.ctre.phoenix.ILoopable loop = _loops.get(_idx);
+				loop.onLoop();
+				if (loop.isDone())
+				{
+					_iterated = true;
+					++_idx;
+					if (_idx < _loops.size()) _loops.get(_idx).onStart();
+				}
+			}
+		}
+		else
+		{
+			_running = false;
+		}
+	}
+	public boolean iterated()
+	{
+		return _iterated;
+	}
+	//--- Loopable ---/
+	public void onStart()
+	{
+		start();
+	}
+
+	public void onLoop()
+	{
+		process();
+	}
+
+	public boolean isDone()
+	{
+		/* Have to return something to know if we are done */
+		if (_running == false)
+			return true;
+		else
+			return false;
+	}
+
+	public void onStop()
+	{
+		stop();
+	}
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/sensors/PigeonIMU.java b/java/src/com/ctre/phoenix/sensors/PigeonIMU.java
new file mode 100644
index 0000000..981b457
--- /dev/null
+++ b/java/src/com/ctre/phoenix/sensors/PigeonIMU.java
@@ -0,0 +1,908 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ *
+ * Cross The Road Electronics (CTRE) licenses to you the right to
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ *
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+package com.ctre.phoenix.sensors;
+import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix.ParamEnum;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+
+import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.wpilibj.hal.HAL;
+
+/**
+ * Pigeon IMU Class. Class supports communicating over CANbus and over
+ * ribbon-cable (CAN Talon SRX).
+ */
+public class PigeonIMU {
+	private long m_handle;
+
+	/** Data object for holding fusion information. */
+	public static class FusionStatus {
+		public double heading;
+		public boolean bIsValid;
+		public boolean bIsFusing;
+
+		/**
+		 * Same as getLastError()
+		 */
+		public ErrorCode lastError;
+
+		public String toString() {
+			String description;
+			if (lastError != ErrorCode.OK) {
+				description = "Could not receive status frame.  Check wiring and web-config.";
+			} else if (bIsValid == false) {
+				description = "Fused Heading is not valid.";
+			} else if (bIsFusing == false) {
+				description = "Fused Heading is valid.";
+			} else {
+				description = "Fused Heading is valid and is fusing compass.";
+			}
+			return description;
+		}
+	}
+
+	/** Various calibration modes supported by Pigeon. */
+	public enum CalibrationMode {
+		BootTareGyroAccel(0), Temperature(1), Magnetometer12Pt(2), Magnetometer360(3), Accelerometer(5), Unknown(-1);
+		public final int value;
+
+		private CalibrationMode(int initValue) {
+			this.value = initValue;
+		}
+
+		public static CalibrationMode valueOf(int value) {
+			for (CalibrationMode e : CalibrationMode.values()) {
+				if (e.value == value) {
+					return e;
+				}
+			}
+			return Unknown;
+		}
+	};
+
+	/** Overall state of the Pigeon. */
+	public enum PigeonState {
+		NoComm(0), Initializing(1), Ready(2), UserCalibration(3), Unknown(-1);
+		public final int value;
+
+		private PigeonState(int initValue) {
+			this.value = initValue;
+		}
+
+		public static PigeonState valueOf(int value) {
+			for (PigeonState e : PigeonState.values()) {
+				if (e.value == value) {
+					return e;
+				}
+			}
+			return Unknown;
+		}
+	};
+
+	/**
+	 * Data object for status on current calibration and general status.
+	 *
+	 * Pigeon has many calibration modes supported for a variety of uses. The
+	 * modes generally collects and saves persistently information that makes
+	 * the Pigeon signals more accurate. This includes collecting temperature,
+	 * gyro, accelerometer, and compass information.
+	 *
+	 * For FRC use-cases, typically compass and temperature calibration is not
+	 * required.
+	 *
+	 * Additionally when motion driver software in the Pigeon boots, it will
+	 * perform a fast boot calibration to initially bias gyro and setup
+	 * accelerometer.
+	 *
+	 * These modes can be enabled with the EnterCalibration mode.
+	 *
+	 * When a calibration mode is entered, caller can expect...
+	 *
+	 * - PigeonState to reset to Initializing and bCalIsBooting is set to true.
+	 * Pigeon LEDs will blink the boot pattern. This is similar to the normal
+	 * boot cal, however it can an additional ~30 seconds since calibration
+	 * generally requires more information. currentMode will reflect the user's
+	 * selected calibration mode.
+	 *
+	 * - PigeonState will eventually settle to UserCalibration and Pigeon LEDs
+	 * will show cal specific blink patterns. bCalIsBooting is now false.
+	 *
+	 * - Follow the instructions in the Pigeon User Manual to meet the
+	 * calibration specific requirements. When finished calibrationError will
+	 * update with the result. Pigeon will solid-fill LEDs with red (for
+	 * failure) or green (for success) for ~5 seconds. Pigeon then perform
+	 * boot-cal to cleanly apply the newly saved calibration data.
+	 */
+	public static class GeneralStatus {
+		/**
+		 * The current state of the motion driver. This reflects if the sensor
+		 * signals are accurate. Most calibration modes will force Pigeon to
+		 * reinit the motion driver.
+		 */
+		public PigeonState state;
+		/**
+		 * The currently applied calibration mode if state is in UserCalibration
+		 * or if bCalIsBooting is true. Otherwise it holds the last selected
+		 * calibration mode (when calibrationError was updated).
+		 */
+		public CalibrationMode currentMode;
+		/**
+		 * The error code for the last calibration mode. Zero represents a
+		 * successful cal (with solid green LEDs at end of cal) and nonzero is a
+		 * failed calibration (with solid red LEDs at end of cal). Different
+		 * calibration
+		 */
+		public int calibrationError;
+		/**
+		 * After caller requests a calibration mode, pigeon will perform a
+		 * boot-cal before entering the requested mode. During this period, this
+		 * flag is set to true.
+		 */
+		public boolean bCalIsBooting;
+		/**
+		 * Temperature in Celsius
+		 */
+		public double tempC;
+		/**
+		 * Number of seconds Pigeon has been up (since boot). This register is
+		 * reset on power boot or processor reset. Register is capped at 255
+		 * seconds with no wrap around.
+		 */
+		public int upTimeSec;
+		/**
+		 * Number of times the Pigeon has automatically rebiased the gyro. This
+		 * counter overflows from 15 -> 0 with no cap.
+		 */
+		public int noMotionBiasCount;
+		/**
+		 * Number of times the Pigeon has temperature compensated the various
+		 * signals. This counter overflows from 15 -> 0 with no cap.
+		 */
+		public int tempCompensationCount;
+		/**
+		 * Same as getLastError()
+		 */
+		public ErrorCode lastError;
+
+		/**
+		 * general string description of current status
+		 */
+		public String toString() {
+			String description;
+			/* build description string */
+			if (lastError != ErrorCode.OK) { // same as NoComm
+				description = "Status frame was not received, check wired connections and web-based config.";
+			} else if (bCalIsBooting) {
+				description = "Pigeon is boot-caling to properly bias accel and gyro.  Do not move Pigeon.  When finished biasing, calibration mode will start.";
+			} else if (state == PigeonState.UserCalibration) {
+				/* mode specific descriptions */
+				switch (currentMode) {
+				case BootTareGyroAccel:
+					description = "Boot-Calibration: Gyro and Accelerometer are being biased.";
+					break;
+				case Temperature:
+					description = "Temperature-Calibration: Pigeon is collecting temp data and will finish when temp range is reached. \n";
+					description += "Do not move Pigeon.";
+					break;
+				case Magnetometer12Pt:
+					description = "Magnetometer Level 1 calibration: Orient the Pigeon PCB in the 12 positions documented in the User's Manual.";
+					break;
+				case Magnetometer360:
+					description = "Magnetometer Level 2 calibration: Spin robot slowly in 360' fashion.  ";
+					break;
+				case Accelerometer:
+					description = "Accelerometer Calibration: Pigeon PCB must be placed on a level source.  Follow User's Guide for how to level surfacee.  ";
+					break;
+				default:
+				case Unknown:
+					description = "Unknown status";
+					break;
+				}
+			} else if (state == PigeonState.Ready) {
+				/*
+				 * definitely not doing anything cal-related. So just instrument
+				 * the motion driver state
+				 */
+				description = "Pigeon is running normally.  Last CAL error code was ";
+				description += Integer.toString(calibrationError);
+				description += ".";
+			} else if (state == PigeonState.Initializing) {
+				/*
+				 * definitely not doing anything cal-related. So just instrument
+				 * the motion driver state
+				 */
+				description = "Pigeon is boot-caling to properly bias accel and gyro.  Do not move Pigeon.";
+			} else {
+				description = "Not enough data to determine status.";
+			}
+			return description;
+		}
+	};
+
+	private int m_deviceNumber = 0;
+
+	private double[] _generalStatus = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+	private double[] _fusionStatus = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+	/**
+	 * Create a Pigeon object that communicates with Pigeon on CAN Bus.
+	 *
+	 * @param deviceNumber
+	 *            CAN Device Id of Pigeon [0,62]
+	 */
+	public PigeonIMU(int deviceNumber) {
+		m_handle = PigeonImuJNI.JNI_new_PigeonImu(deviceNumber);
+		m_deviceNumber = deviceNumber;
+		HAL.report(tResourceType.kResourceType_PigeonIMU, m_deviceNumber + 1);
+	}
+
+	/**
+	 * Create a Pigeon object that communciates with Pigeon through the
+	 * Gadgeteer ribbon cable connected to a Talon on CAN Bus.
+	 *
+	 * @param talonSrx
+	 *            Object for the TalonSRX connected via ribbon cable.
+	 */
+	public PigeonIMU(TalonSRX talonSrx) {
+		m_deviceNumber = talonSrx.getDeviceID();
+		m_handle = PigeonImuJNI.JNI_new_PigeonImu_Talon(m_deviceNumber);
+		HAL.report(tResourceType.kResourceType_PigeonIMU, m_deviceNumber + 1);
+		HAL.report(64, m_deviceNumber + 1);
+	}
+
+	/**
+	 * Sets the Yaw register to the specified value.
+	 *
+	 * @param angleDeg  Degree of Yaw [+/- 23040 degrees]
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setYaw(double angleDeg, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_SetYaw(m_handle, angleDeg, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Atomically add to the Yaw register.
+	 *
+	 * @param angleDeg  Degrees to add to the Yaw register.
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode addYaw(double angleDeg, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_AddYaw(m_handle, angleDeg, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Sets the Yaw register to match the current compass value.
+	 *
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setYawToCompass(int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_SetYawToCompass(m_handle, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Sets the Fused Heading to the specified value.
+	 *
+	 * @param angleDeg  Degree of heading [+/- 23040 degrees]
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setFusedHeading(double angleDeg, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_SetFusedHeading(m_handle, angleDeg, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Atomically add to the Fused Heading register.
+	 *
+	 * @param angleDeg  Degrees to add to the Fused Heading register.
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode addFusedHeading(double angleDeg, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_AddFusedHeading(m_handle, angleDeg, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Sets the Fused Heading register to match the current compass value.
+	 *
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setFusedHeadingToCompass(int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_SetFusedHeadingToCompass(m_handle, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Sets the AccumZAngle.
+	 *
+	 * @param angleDeg  Degrees to set AccumZAngle to.
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setAccumZAngle(double angleDeg, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_SetAccumZAngle(m_handle, angleDeg, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Enable/Disable Temp compensation. Pigeon defaults with this on at boot.
+	 *
+	 * @param bTempCompEnable Set to "True" to enable temperature compensation.
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configTemperatureCompensationEnable(boolean bTempCompEnable, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_ConfigTemperatureCompensationEnable(m_handle, bTempCompEnable ? 1 : 0, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Set the declination for compass. Declination is the difference between
+	 * Earth Magnetic north, and the geographic "True North".
+	 *
+	 * @param angleDegOffset  Degrees to set Compass Declination to.
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setCompassDeclination(double angleDegOffset, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_SetCompassDeclination(m_handle, angleDegOffset, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Sets the compass angle. Although compass is absolute [0,360) degrees, the
+	 * continuous compass register holds the wrap-arounds.
+	 *
+	 * @param angleDeg  Degrees to set continuous compass angle to.
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setCompassAngle(double angleDeg, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_SetCompassAngle(m_handle, angleDeg, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	// ----------------------- Calibration routines -----------------------//
+	/**
+	 * Enters the Calbration mode.  See the Pigeon IMU documentation for More
+	 * information on Calibration.
+	 *
+	 * @param calMode  Calibration to execute
+	 * @param timeoutMs
+   *            Timeout value in ms. If nonzero, function will wait for
+   *            config success and report an error if it times out.
+   *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode enterCalibrationMode(CalibrationMode calMode, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_EnterCalibrationMode(m_handle, calMode.value, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Get the status of the current (or previousley complete) calibration.
+	 *
+	 * @param toFill Container for the status information.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode getGeneralStatus(GeneralStatus toFill) {
+		int retval = PigeonImuJNI.JNI_GetGeneralStatus(m_handle, _generalStatus);
+		toFill.state = PigeonState.valueOf((int) _generalStatus[0]);
+		toFill.currentMode = CalibrationMode.valueOf((int) _generalStatus[1]);
+		toFill.calibrationError = (int) _generalStatus[2];
+		toFill.bCalIsBooting = _generalStatus[3] != 0;
+		toFill.tempC = _generalStatus[4];
+		toFill.upTimeSec = (int) _generalStatus[5];
+		toFill.noMotionBiasCount = (int) _generalStatus[6];
+		toFill.tempCompensationCount = (int) _generalStatus[7];
+		toFill.lastError = ErrorCode.valueOf(retval);
+		return toFill.lastError;
+	}
+
+	// ----------------------- General Error status -----------------------//
+	/**
+	 * Call GetLastError() generated by this object.
+	 * Not all functions return an error code but can
+	 * potentially report errors.
+	 *
+	 * This function can be used to retrieve those error codes.
+	 *
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode getLastError() {
+		int retval = PigeonImuJNI.JNI_GetLastError(m_handle);
+		return ErrorCode.valueOf(retval);
+	}
+
+	// ----------------------- Strongly typed Signal decoders
+	// -----------------------//
+	/**
+	 * Get 6d Quaternion data.
+	 *
+	 * @param wxyz Array to fill with quaternion data w[0], x[1], y[2], z[3]
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode get6dQuaternion(double[] wxyz) {
+		int retval = PigeonImuJNI.JNI_Get6dQuaternion(m_handle, wxyz);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Get Yaw, Pitch, and Roll data.
+	 *
+	 * @param ypr_deg Array to fill with yaw[0], pitch[1], and roll[2] data
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode getYawPitchRoll(double[] ypr_deg) {
+		int retval = PigeonImuJNI.JNI_GetYawPitchRoll(m_handle, ypr_deg);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Get AccumGyro data.
+	 * AccumGyro is the integrated gyro value on each axis.
+	 *
+	 * @param xyz_deg Array to fill with x[0], y[1], and z[2] AccumGyro data
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode getAccumGyro(double[] xyz_deg) {
+		int retval = PigeonImuJNI.JNI_GetAccumGyro(m_handle, xyz_deg);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Get the absolute compass heading.
+	 * @return compass heading [0,360) degrees.
+	 */
+	public double getAbsoluteCompassHeading() {
+		double retval = PigeonImuJNI.JNI_GetAbsoluteCompassHeading(m_handle);
+		return retval;
+	}
+
+	/**
+	 * Get the continuous compass heading.
+	 * @return continuous compass heading [-23040, 23040) degrees. Use
+	 *         SetCompassHeading to modify the wrap-around portion.
+	 */
+	public double getCompassHeading() {
+		double retval = PigeonImuJNI.JNI_GetCompassHeading(m_handle);
+		return retval;
+	}
+
+	/**
+	 * Gets the compass' measured magnetic field strength.
+	 * @return field strength in Microteslas (uT).
+	 */
+	public double getCompassFieldStrength() {
+		double retval = PigeonImuJNI.JNI_GetCompassFieldStrength(m_handle);
+		return retval;
+	}
+	/**
+	 * Gets the temperature of the pigeon.
+	 *
+	 * @return Temperature in ('C)
+	 */
+	public double getTemp() {
+		double retval = PigeonImuJNI.JNI_GetTemp(m_handle);
+		return retval;
+	}
+
+	/**
+	 * Gets the current Pigeon state
+	 *
+	 * @return PigeonState enum
+	 */
+	public PigeonState getState() {
+		int retval = PigeonImuJNI.JNI_GetState(m_handle);
+		return PigeonState.valueOf(retval);
+	}
+
+	/**
+	 * Gets the current Pigeon uptime.
+	 *
+	 * @return How long has Pigeon been running in whole seconds. Value caps at
+	 *         255.
+	 */
+	public int getUpTime() {
+		int retval = PigeonImuJNI.JNI_GetUpTime(m_handle);
+		return retval;
+	}
+	/**
+	 * Get Raw Magnetometer data.
+	 *
+	 * @param rm_xyz Array to fill with x[0], y[1], and z[2] data
+	 * 				Number is equal to 0.6 microTeslas per unit.
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode getRawMagnetometer(short[] rm_xyz) {
+		int retval = PigeonImuJNI.JNI_GetRawMagnetometer(m_handle, rm_xyz);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Get Biased Magnetometer data.
+	 *
+	 * @param bm_xyz Array to fill with x[0], y[1], and z[2] data
+	 * 				Number is equal to 0.6 microTeslas per unit.
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode getBiasedMagnetometer(short[] bm_xyz) {
+		int retval = PigeonImuJNI.JNI_GetBiasedMagnetometer(m_handle, bm_xyz);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Get Biased Accelerometer data.
+	 *
+	 * @param ba_xyz Array to fill with x[0], y[1], and z[2] data.
+	 * 			These are in fixed point notation Q2.14.  eg. 16384 = 1G
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode getBiasedAccelerometer(short[] ba_xyz) {
+		int retval = PigeonImuJNI.JNI_GetBiasedAccelerometer(m_handle, ba_xyz);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Get Raw Gyro data.
+	 *
+	 * @param xyz_dps Array to fill with x[0], y[1], and z[2] data in degrees per second.
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode getRawGyro(double[] xyz_dps) {
+		int retval = PigeonImuJNI.JNI_GetRawGyro(m_handle, xyz_dps);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Get Accelerometer tilt angles.
+	 *
+	 * @param tiltAngles Array to fill with x[0], y[1], and z[2] angles in degrees.
+	 * @return The last ErrorCode generated.
+	 */
+	public ErrorCode getAccelerometerAngles(double[] tiltAngles) {
+		int retval = PigeonImuJNI.JNI_GetAccelerometerAngles(m_handle, tiltAngles);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Get the current Fusion Status (including fused heading)
+	 *
+	 * @param toFill 	object reference to fill with fusion status flags.
+	 *					Caller may pass null if flags are not needed.
+	 * @return The fused heading in degrees.
+	 */
+	public double getFusedHeading(FusionStatus toFill) {
+		int errorCode = PigeonImuJNI.JNI_GetFusedHeading(m_handle, _fusionStatus);
+
+		if (toFill != null) {
+			toFill.heading = _fusionStatus[0];
+			toFill.bIsFusing = (_fusionStatus[1] != 0);
+			toFill.bIsValid = (_fusionStatus[2] != 0);
+			toFill.lastError = ErrorCode.valueOf(errorCode);
+		}
+
+		return _fusionStatus[0];
+	}
+	/**
+	 * Gets the Fused Heading
+	 *
+	 * @return The fused heading in degrees.
+	 */
+	public double getFusedHeading() {
+		PigeonImuJNI.JNI_GetFusedHeading(m_handle, _fusionStatus);
+
+		return _fusionStatus[0];
+	}
+
+	/**
+	 * Gets the firmware version of the device.
+	 *
+	 * @return param holds the firmware version of the device. Device must be powered
+	 * cycled at least once.
+	 */
+	public int getFirmwareVersion() {
+		int k = PigeonImuJNI.JNI_GetFirmwareVersion(m_handle);
+		return k;
+	}
+
+	/**
+	 * @return true iff a reset has occurred since last call.
+	 */
+	public boolean hasResetOccurred() {
+		boolean k = PigeonImuJNI.JNI_HasResetOccurred(m_handle);
+		return k;
+	}
+
+	/**
+	 * Sets the value of a custom parameter. This is for arbitrary use.
+   *
+   * Sometimes it is necessary to save calibration/declination/offset
+   * information in the device. Particularly if the
+   * device is part of a subsystem that can be replaced.
+	 *
+	 * @param newValue
+	 *            Value for custom parameter.
+	 * @param paramIndex
+	 *            Index of custom parameter. [0-1]
+	 * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSetCustomParam(int newValue, int paramIndex, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_ConfigSetCustomParam(m_handle, newValue, paramIndex, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Gets the value of a custom parameter. This is for arbitrary use.
+   *
+   * Sometimes it is necessary to save calibration/declination/offset
+   * information in the device. Particularly if the
+   * device is part of a subsystem that can be replaced.
+	 *
+	 * @param paramIndex
+	 *            Index of custom parameter. [0-1]
+	 * @param timoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 * @return Value of the custom param.
+	 */
+	public int configGetCustomParam(int paramIndex, int timoutMs) {
+		int retval = PigeonImuJNI.JNI_ConfigGetCustomParam(m_handle, paramIndex, timoutMs);
+		return retval;
+	}
+
+	/**
+	 * Sets a parameter. Generally this is not used.
+   * This can be utilized in
+   * - Using new features without updating API installation.
+   * - Errata workarounds to circumvent API implementation.
+   * - Allows for rapid testing / unit testing of firmware.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param value
+	 *            Value of parameter.
+	 * @param subValue
+	 *            Subvalue for parameter. Maximum value of 255.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSetParameter(ParamEnum param, double value, int subValue, int ordinal, int timeoutMs) {
+		return configSetParameter(param.value, value, subValue, ordinal, timeoutMs);
+	}
+	/**
+	 * Sets a parameter. Generally this is not used.
+   * This can be utilized in
+   * - Using new features without updating API installation.
+   * - Errata workarounds to circumvent API implementation.
+   * - Allows for rapid testing / unit testing of firmware.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param value
+	 *            Value of parameter.
+	 * @param subValue
+	 *            Subvalue for parameter. Maximum value of 255.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode configSetParameter(int param, double value, int subValue, int ordinal, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_ConfigSetParameter(m_handle, param, value, subValue, ordinal,
+				timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Gets a parameter. Generally this is not used.
+   * This can be utilized in
+   * - Using new features without updating API installation.
+   * - Errata workarounds to circumvent API implementation.
+   * - Allows for rapid testing / unit testing of firmware.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 * @return Value of parameter.
+	 */
+	public double configGetParameter(ParamEnum param, int ordinal, int timeoutMs) {
+		return configGetParameter(param.value, ordinal, timeoutMs);
+	}
+	/**
+	 * Gets a parameter.
+	 *
+	 * @param param
+	 *            Parameter enumeration.
+	 * @param ordinal
+	 *            Ordinal of parameter.
+	 * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 * @return Value of parameter.
+	 */
+	public double configGetParameter(int param, int ordinal, int timeoutMs) {
+		return PigeonImuJNI.JNI_ConfigGetParameter(m_handle, param, ordinal, timeoutMs);
+	}
+	/**
+	 * Sets the period of the given status frame.
+	 *
+	 * @param statusFrame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setStatusFramePeriod(PigeonIMU_StatusFrame statusFrame, int periodMs, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_SetStatusFramePeriod(m_handle, statusFrame.value, periodMs, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Sets the period of the given status frame.
+	 *
+	 * @param statusFrame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setStatusFramePeriod(int statusFrame, int periodMs, int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_SetStatusFramePeriod(m_handle, statusFrame, periodMs, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * Gets the period of the given status frame.
+	 *
+	 * @param frame
+	 *            Frame to get the period of.
+	 * @param timeoutMs
+ *            Timeout value in ms. If nonzero, function will wait for
+ *            config success and report an error if it times out.
+ *            If zero, no blocking or checking is performed.
+	 * @return Period of the given status frame.
+	 */
+	public int getStatusFramePeriod(PigeonIMU_StatusFrame frame, int timeoutMs) {
+		return PigeonImuJNI.JNI_GetStatusFramePeriod(m_handle, frame.value, timeoutMs);
+	}
+	/**
+	 * Sets the period of the given control frame.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setControlFramePeriod(PigeonIMU_ControlFrame frame, int periodMs) {
+		int retval = PigeonImuJNI.JNI_SetControlFramePeriod(m_handle, frame.value, periodMs);
+		return ErrorCode.valueOf(retval);
+	}
+	/**
+	 * Sets the period of the given control frame.
+	 *
+	 * @param frame
+	 *            Frame whose period is to be changed.
+	 * @param periodMs
+	 *            Period in ms for the given frame.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode setControlFramePeriod(int frame, int periodMs) {
+		int retval = PigeonImuJNI.JNI_SetControlFramePeriod(m_handle, frame, periodMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	// ------ Faults ----------//
+	/**
+	 * Gets the fault status
+	 *
+	 * @param toFill
+	 *            Container for fault statuses.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode getFaults(PigeonIMU_Faults toFill) {
+		int bits = PigeonImuJNI.JNI_GetFaults(m_handle);
+		toFill.update(bits);
+		return getLastError();
+	}
+	/**
+	 * Gets the sticky fault status
+	 *
+	 * @param toFill
+	 *            Container for sticky fault statuses.
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode getStickyFaults(PigeonIMU_StickyFaults toFill) {
+		int bits = PigeonImuJNI.JNI_GetStickyFaults(m_handle);
+		toFill.update(bits);
+		return getLastError();
+	}
+	/**
+	 * Clears the Sticky Faults
+	 *
+	 * @return Error Code generated by function. 0 indicates no error.
+	 */
+	public ErrorCode clearStickyFaults(int timeoutMs) {
+		int retval = PigeonImuJNI.JNI_ClearStickyFaults(m_handle, timeoutMs);
+		return ErrorCode.valueOf(retval);
+	}
+
+	/**
+	 * @return The Device Number
+	 */
+	public int getDeviceID(){
+		return m_deviceNumber;
+	}
+}
diff --git a/java/src/com/ctre/phoenix/sensors/PigeonIMU_ControlFrame.java b/java/src/com/ctre/phoenix/sensors/PigeonIMU_ControlFrame.java
new file mode 100644
index 0000000..21ee565
--- /dev/null
+++ b/java/src/com/ctre/phoenix/sensors/PigeonIMU_ControlFrame.java
@@ -0,0 +1,21 @@
+package com.ctre.phoenix.sensors;
+
+/** Enumerated type for control frame types. */
+public enum PigeonIMU_ControlFrame {
+	Control_1(0x00042800);
+
+	public final int value;
+
+	PigeonIMU_ControlFrame(int initValue) {
+		this.value = initValue;
+	}
+
+	public static PigeonIMU_ControlFrame valueOf(int value) {
+		for (PigeonIMU_ControlFrame mode : values()) {
+			if (mode.value == value) {
+				return mode;
+			}
+		}
+		return null;
+	}
+}
diff --git a/java/src/com/ctre/phoenix/sensors/PigeonIMU_Faults.java b/java/src/com/ctre/phoenix/sensors/PigeonIMU_Faults.java
new file mode 100644
index 0000000..8628608
--- /dev/null
+++ b/java/src/com/ctre/phoenix/sensors/PigeonIMU_Faults.java
@@ -0,0 +1,16 @@
+package com.ctre.phoenix.sensors;
+
+public class PigeonIMU_Faults {
+	//!< True iff any of the above flags are true.
+	public boolean hasAnyFault() {
+		return false;
+	}
+	public int toBitfield() {
+		int retval = 0;
+		return retval;
+	}
+	public void update(int bits) {
+	}
+	public PigeonIMU_Faults() {
+	}
+};
diff --git a/java/src/com/ctre/phoenix/sensors/PigeonIMU_StatusFrame.java b/java/src/com/ctre/phoenix/sensors/PigeonIMU_StatusFrame.java
new file mode 100644
index 0000000..cec0b94
--- /dev/null
+++ b/java/src/com/ctre/phoenix/sensors/PigeonIMU_StatusFrame.java
@@ -0,0 +1,33 @@
+package com.ctre.phoenix.sensors;
+
+/**
+ * Enumerated types for frame rate ms.
+ */
+public enum PigeonIMU_StatusFrame {
+	CondStatus_1_General(0x042000), 
+	CondStatus_9_SixDeg_YPR(0x042200), 
+	CondStatus_6_SensorFusion(0x042140), 
+	CondStatus_11_GyroAccum(0x042280), 
+	CondStatus_2_GeneralCompass(0x042040), 
+	CondStatus_3_GeneralAccel(0x042080), 
+	CondStatus_10_SixDeg_Quat(0x042240), 
+	RawStatus_4_Mag(0x041CC0), 
+	BiasedStatus_2_Gyro(0x041C40), 
+	BiasedStatus_4_Mag(0x041CC0), 
+	BiasedStatus_6_Accel(0x41D40);
+
+	public final int value;
+
+	PigeonIMU_StatusFrame(int initValue) {
+		this.value = initValue;
+	}
+
+	public static PigeonIMU_StatusFrame valueOf(int value) {
+		for (PigeonIMU_StatusFrame mode : values()) {
+			if (mode.value == value) {
+				return mode;
+			}
+		}
+		return null;
+	}
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/sensors/PigeonIMU_StickyFaults.java b/java/src/com/ctre/phoenix/sensors/PigeonIMU_StickyFaults.java
new file mode 100644
index 0000000..77a99e2
--- /dev/null
+++ b/java/src/com/ctre/phoenix/sensors/PigeonIMU_StickyFaults.java
@@ -0,0 +1,16 @@
+package com.ctre.phoenix.sensors;
+
+public class PigeonIMU_StickyFaults {
+	//!< True iff any of the above flags are true.
+	public boolean hasAnyFault() {
+		return false;
+	}
+	public int toBitfield() {
+		int retval = 0;
+		return retval;
+	}
+	public void update(int bits) {
+	}
+	public PigeonIMU_StickyFaults() {
+	}
+};
diff --git a/java/src/com/ctre/phoenix/sensors/PigeonImuJNI.java b/java/src/com/ctre/phoenix/sensors/PigeonImuJNI.java
new file mode 100644
index 0000000..11932d8
--- /dev/null
+++ b/java/src/com/ctre/phoenix/sensors/PigeonImuJNI.java
@@ -0,0 +1,118 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ * 
+ * Cross The Road Electronics (CTRE) licenses to you the right to 
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files ( *.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ * 
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, 
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+package com.ctre.phoenix.sensors;
+import com.ctre.phoenix.CTREJNIWrapper;
+
+
+public class PigeonImuJNI extends CTREJNIWrapper {
+  
+	public static native long JNI_new_PigeonImu_Talon(int talonID);
+
+	public static native long JNI_new_PigeonImu(int deviceNumber);
+
+	public static native int JNI_ConfigSetCustomParam(long handle, int newValue, int paramIndex, int timeoutMs);
+
+	public static native int JNI_ConfigGetCustomParam(long handle, int paramIndex, int timoutMs);
+
+	public static native int JNI_ConfigSetParameter(long handle, int param, double value, int subValue, int ordinal,
+			int timeoutMs);
+
+	public static native double JNI_ConfigGetParameter(long handle, int param, int ordinal, int timeoutMs);
+
+	public static native int JNI_SetStatusFramePeriod(long handle, int statusFrame, int periodMs, int timeoutMs);
+
+	public static native int JNI_SetYaw(long handle, double angleDeg, int timeoutMs);
+
+	public static native int JNI_AddYaw(long handle, double angleDeg, int timeoutMs);
+	
+	public static native int JNI_SetYawToCompass(long handle, int timeoutMs);
+	
+	public static native int JNI_SetFusedHeading(long handle, double angleDeg, int timeoutMs);
+	
+	public static native int JNI_AddFusedHeading(long handle, double angleDeg, int timeoutMs);
+	
+	public static native int JNI_SetFusedHeadingToCompass(long handle, int timeoutMs);
+	
+	public static native int JNI_SetAccumZAngle(long handle, double angleDeg, int timeoutMs);
+	
+	public static native int JNI_ConfigTemperatureCompensationEnable(long handle, int bTempCompEnable, int timeoutMs);
+	
+	public static native int JNI_SetCompassDeclination(long handle, double angleDegOffset, int timeoutMs);
+	
+	public static native int JNI_SetCompassAngle(long handle, double angleDeg, int timeoutMs);
+	
+	public static native int JNI_EnterCalibrationMode(long handle, int calMode, int timeoutMs);
+	  
+	public static native int JNI_GetGeneralStatus(long handle, double [] params);
+	
+	public static native int JNI_Get6dQuaternion(long handle, double [] wxyz );
+	
+	public static native int JNI_GetYawPitchRoll(long handle, double [] ypr);
+	
+	public static native int JNI_GetAccumGyro(long handle, double [] xyz_deg);
+	
+	public static native double JNI_GetAbsoluteCompassHeading(long handle);
+	
+	public static native double JNI_GetCompassHeading(long handle);
+	
+	public static native double JNI_GetCompassFieldStrength(long handle);
+	
+	public static native double JNI_GetTemp(long handle);
+	
+	public static native int JNI_GetUpTime(long handle);
+	
+	public static native int JNI_GetRawMagnetometer(long handle, short [] rm_xyz);
+	
+	public static native int JNI_GetBiasedMagnetometer(long handle, short [] bm_xyz);
+	
+	public static native int JNI_GetBiasedAccelerometer(long handle, short [] ba_xyz);
+	
+	public static native int JNI_GetRawGyro(long handle, double [] xyz_dps);
+	
+	public static native int JNI_GetAccelerometerAngles(long handle, double [] tiltAngles);
+	  
+	public static native int JNI_GetFusedHeading(long handle, double [] params);
+	  
+	public static native int JNI_GetState(long handle);
+	
+	public static native int JNI_GetResetCount(long handle);
+	
+	public static native int JNI_GetResetFlags(long handle);
+	
+	public static native int JNI_GetFirmwareVersion(long handle);
+	  
+	public static native int JNI_GetLastError(long handle);
+	  
+	public static native boolean JNI_HasResetOccurred(long handle);
+	
+	public static native int JNI_GetStatusFramePeriod(long handle, int frame, int timeoutMs);
+
+	public static native int JNI_SetControlFramePeriod(long handle, int frame, int periodMs);
+
+	public static native int JNI_GetFaults(long handle);
+
+	public static native int JNI_GetStickyFaults(long handle);
+
+	public static native int JNI_ClearStickyFaults(long handle, int timeoutMs);
+	
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/signals/IInvertable.java b/java/src/com/ctre/phoenix/signals/IInvertable.java
new file mode 100644
index 0000000..e30a48d
--- /dev/null
+++ b/java/src/com/ctre/phoenix/signals/IInvertable.java
@@ -0,0 +1,7 @@
+package com.ctre.phoenix.signals;
+
+public interface IInvertable
+{
+	void setInverted(boolean invert);
+	boolean getInverted();
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/signals/IOutputSignal.java b/java/src/com/ctre/phoenix/signals/IOutputSignal.java
new file mode 100644
index 0000000..13e6ced
--- /dev/null
+++ b/java/src/com/ctre/phoenix/signals/IOutputSignal.java
@@ -0,0 +1,6 @@
+package com.ctre.phoenix.signals;
+
+public interface IOutputSignal
+{
+	//void set(double value);
+}
\ No newline at end of file
diff --git a/java/src/com/ctre/phoenix/time/StopWatch.java b/java/src/com/ctre/phoenix/time/StopWatch.java
new file mode 100644
index 0000000..fc41340
--- /dev/null
+++ b/java/src/com/ctre/phoenix/time/StopWatch.java
@@ -0,0 +1,25 @@
+package com.ctre.phoenix.time;
+
+public class StopWatch
+{
+	private long _t0 = 0;
+	private long _t1 = 0;
+	
+	public void start()
+	{
+		_t0 = System.currentTimeMillis();
+	}
+	
+	public double getDuration()
+	{
+		return (double)getDurationMs() / 1000;
+	}
+	public int getDurationMs()
+	{
+		_t1 = System.currentTimeMillis();
+		long retval = _t1 - _t0;
+		if(retval < 0)
+			retval = 0;
+		return (int)retval;
+	}
+}
\ No newline at end of file
diff --git a/java/sysProps.xml b/java/sysProps.xml
new file mode 100644
index 0000000..0ceba2a
--- /dev/null
+++ b/java/sysProps.xml
Binary files differ
diff --git a/libraries/driver/include/ctre/phoenix/CANifierControlFrame.h b/libraries/driver/include/ctre/phoenix/CANifierControlFrame.h
new file mode 100644
index 0000000..e5adbf8
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/CANifierControlFrame.h
@@ -0,0 +1,13 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+
+/** Enumerated type for status frame types. */
+enum CANifierControlFrame {
+	CANifier_Control_1_General = 0x03040000,
+	CANifier_Control_2_PwmOutput = 0x03040040,
+};
+
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/CANifierFaults.h b/libraries/driver/include/ctre/phoenix/CANifierFaults.h
new file mode 100644
index 0000000..c663dc9
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/CANifierFaults.h
@@ -0,0 +1,24 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+
+struct CANifierFaults {
+	//!< True iff any of the above flags are true.
+	bool HasAnyFault() const {
+		return false;
+	}
+	int ToBitfield() const {
+		int retval = 0;
+		return retval;
+	}
+	CANifierFaults(int bits) {
+		(void)bits;
+	}
+	CANifierFaults() {
+	}
+};
+
+} // phoenix
+} // ctre
+
diff --git a/libraries/driver/include/ctre/phoenix/CANifierStatusFrame.h b/libraries/driver/include/ctre/phoenix/CANifierStatusFrame.h
new file mode 100644
index 0000000..240fc88
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/CANifierStatusFrame.h
@@ -0,0 +1,18 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+
+/** Enumerated type for status frame types. */
+enum CANifierStatusFrame {
+	CANifierStatusFrame_Status_1_General = 0x041400,
+	CANifierStatusFrame_Status_2_General = 0x041440,
+	CANifierStatusFrame_Status_3_PwmInputs0 = 0x041480,
+	CANifierStatusFrame_Status_4_PwmInputs1 = 0x0414C0,
+	CANifierStatusFrame_Status_5_PwmInputs2 = 0x041500,
+	CANifierStatusFrame_Status_6_PwmInputs3 = 0x041540,
+	CANifierStatusFrame_Status_8_Misc = 0x0415C0,
+};
+
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/CANifierStickyFaults.h b/libraries/driver/include/ctre/phoenix/CANifierStickyFaults.h
new file mode 100644
index 0000000..5246208
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/CANifierStickyFaults.h
@@ -0,0 +1,23 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+
+struct CANifierStickyFaults {
+	//!< True iff any of the above flags are true.
+	bool HasAnyFault() const {
+		return false;
+	}
+	int ToBitfield() const {
+		int retval = 0;
+		return retval;
+	}
+	CANifierStickyFaults(int bits) {
+		(void)bits;
+	}
+	CANifierStickyFaults() {
+	}
+};
+
+} // phoenix
+} // ctre
diff --git a/libraries/driver/include/ctre/phoenix/CANifierVelocityMeasPeriod.h b/libraries/driver/include/ctre/phoenix/CANifierVelocityMeasPeriod.h
new file mode 100644
index 0000000..46fb2c5
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/CANifierVelocityMeasPeriod.h
@@ -0,0 +1,18 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+
+enum CANifierVelocityMeasPeriod {
+	Period_1Ms = 1,
+	Period_2Ms = 2,
+	Period_5Ms = 5,
+	Period_10Ms = 10,
+	Period_20Ms = 20,
+	Period_25Ms = 25,
+	Period_50Ms = 50,
+	Period_100Ms = 100,
+};
+
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/CCI/CANifier_CCI.h b/libraries/driver/include/ctre/phoenix/CCI/CANifier_CCI.h
new file mode 100644
index 0000000..6228a0d
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/CCI/CANifier_CCI.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include "ctre/phoenix/ErrorCode.h"
+#include <set>
+
+namespace CANifier_CCI{
+	enum GeneralPin{
+	QUAD_IDX = 0,
+	QUAD_B = 1,
+	QUAD_A = 2,
+	LIMR = 3,
+	LIMF = 4,
+	SDA = 5,
+	SCL = 6,
+	SPI_CS = 7,
+	SPI_MISO_PWM2P = 8,
+	SPI_MOSI_PWM1P = 9,
+	SPI_CLK_PWM0P = 10,
+	};
+}
+
+extern "C"{
+	void *c_CANifier_Create1(int deviceNumber);
+	ctre::phoenix::ErrorCode c_CANifier_GetDescription(void *handle, char * toFill, int toFillByteSz, int * numBytesFilled);
+	ctre::phoenix::ErrorCode c_CANifier_SetLEDOutput(void *handle,  uint32_t  dutyCycle,  uint32_t  ledChannel);
+	ctre::phoenix::ErrorCode c_CANifier_SetGeneralOutputs(void *handle,  uint32_t  outputsBits,  uint32_t  isOutputBits);
+	ctre::phoenix::ErrorCode c_CANifier_SetGeneralOutput(void *handle,  uint32_t  outputPin,  bool  outputValue,  bool  outputEnable);
+	ctre::phoenix::ErrorCode c_CANifier_SetPWMOutput(void *handle,  uint32_t  pwmChannel,  uint32_t  dutyCycle);
+	ctre::phoenix::ErrorCode c_CANifier_EnablePWMOutput(void *handle, uint32_t pwmChannel, bool bEnable);
+	ctre::phoenix::ErrorCode c_CANifier_GetGeneralInputs(void *handle, bool allPins[], uint32_t capacity);
+	ctre::phoenix::ErrorCode c_CANifier_GetGeneralInput(void *handle, uint32_t inputPin, bool * measuredInput);
+	ctre::phoenix::ErrorCode c_CANifier_GetPWMInput(void *handle,  uint32_t  pwmChannel,  double dutyCycleAndPeriod [2]);
+	ctre::phoenix::ErrorCode c_CANifier_GetLastError(void *handle);
+	ctre::phoenix::ErrorCode c_CANifier_GetBusVoltage(void *handle, double * batteryVoltage);
+	ctre::phoenix::ErrorCode c_CANifier_GetQuadraturePosition(void *handle, int * pos);
+	ctre::phoenix::ErrorCode c_CANifier_SetQuadraturePosition(void *handle, int pos, int timeoutMs);
+	ctre::phoenix::ErrorCode c_CANifier_GetQuadratureVelocity(void *handle, int * vel);
+	ctre::phoenix::ErrorCode c_CANifier_GetQuadratureSensor(void *handle, int * pos, int * vel);
+	ctre::phoenix::ErrorCode c_CANifier_ConfigVelocityMeasurementPeriod(void *handle, int period, int timeoutMs);
+	ctre::phoenix::ErrorCode c_CANifier_ConfigVelocityMeasurementWindow(void *handle, int window, int timeoutMs);
+	void c_CANifier_SetLastError(void *handle, int error);
+	ctre::phoenix::ErrorCode c_CANifier_ConfigSetParameter(void *handle, int param, double value, int subValue, int ordinal, int timeoutMs);
+	ctre::phoenix::ErrorCode c_CANifier_ConfigGetParameter(void *handle, int param, double *value, int ordinal, int timeoutMs);
+	ctre::phoenix::ErrorCode c_CANifier_ConfigSetCustomParam(void *handle, int newValue, int paramIndex, int timeoutMs);
+	ctre::phoenix::ErrorCode c_CANifier_ConfigGetCustomParam(void *handle, int *readValue, int paramIndex, int timoutMs);
+	ctre::phoenix::ErrorCode c_CANifier_GetFaults(void *handle, int * param) ;
+	ctre::phoenix::ErrorCode c_CANifier_GetStickyFaults(void *handle, int * param) ;
+	ctre::phoenix::ErrorCode c_CANifier_ClearStickyFaults(void *handle, int timeoutMs) ;
+	ctre::phoenix::ErrorCode c_CANifier_GetFirmwareVersion(void *handle, int *firmwareVers);
+	ctre::phoenix::ErrorCode c_CANifier_HasResetOccurred(void *handle, bool * hasReset);
+	ctre::phoenix::ErrorCode c_CANifier_SetStatusFramePeriod(void *handle, int frame, int periodMs, int timeoutMs);
+	ctre::phoenix::ErrorCode c_CANifier_GetStatusFramePeriod(void *handle, int frame, int *periodMs, int timeoutMs);
+	ctre::phoenix::ErrorCode c_CANifier_SetControlFramePeriod(void *handle, int frame,	int periodMs) ;
+}
diff --git a/libraries/driver/include/ctre/phoenix/CCI/Logger_CCI.h b/libraries/driver/include/ctre/phoenix/CCI/Logger_CCI.h
new file mode 100644
index 0000000..14a6c0e
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/CCI/Logger_CCI.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include "ctre/phoenix/ErrorCode.h"
+#include <string>
+
+extern "C" {
+	void c_Logger_Close();
+	void c_Logger_Open(int language, bool logDriverStation);
+	ctre::phoenix::ErrorCode c_Logger_Log(ctre::phoenix::ErrorCode code, const char* origin, int hierarchy, const char *stacktrace);
+	void c_Logger_Description(ctre::phoenix::ErrorCode code, std::string & shortDescripToFill, std::string & longDescripToFill);
+}
diff --git a/libraries/driver/include/ctre/phoenix/CCI/MotController_CCI.h b/libraries/driver/include/ctre/phoenix/CCI/MotController_CCI.h
new file mode 100644
index 0000000..a66f9bd
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/CCI/MotController_CCI.h
@@ -0,0 +1,132 @@
+#include "ctre/phoenix/ErrorCode.h"
+
+extern "C"{
+	void* c_MotController_Create1(int baseArbId);
+
+	ctre::phoenix::ErrorCode c_MotController_GetDeviceNumber(void *handle, int *deviceNumber);
+	ctre::phoenix::ErrorCode c_MotController_GetDescription(void *handle, char * toFill, int toFillByteSz, int * numBytesFilled);
+	ctre::phoenix::ErrorCode c_MotController_SetDemand(void *handle, int mode, int demand0, int demand1);
+	ctre::phoenix::ErrorCode c_MotController_Set_4(void *handle, int mode, double demand0, double demand1, int demand1Type);
+	void c_MotController_SetNeutralMode(void *handle, int neutralMode);
+	void c_MotController_SetSensorPhase(void *handle, bool PhaseSensor);
+	void c_MotController_SetInverted(void *handle, bool invert);
+	ctre::phoenix::ErrorCode c_MotController_ConfigOpenLoopRamp(void *handle, double secondsFromNeutralToFull, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigClosedLoopRamp(void *handle, double secondsFromNeutralToFull, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigPeakOutputForward(void *handle, double percentOut, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigPeakOutputReverse(void *handle, double percentOut, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigNominalOutputForward(void *handle, double percentOut, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigNominalOutputReverse(void *handle, double percentOut, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigNeutralDeadband(void *handle, double percentDeadband, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigVoltageCompSaturation(void *handle, double voltage, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigVoltageMeasurementFilter(void *handle, int filterWindowSamples, int timeoutMs);
+	void c_MotController_EnableVoltageCompensation(void *handle, bool enable);
+	ctre::phoenix::ErrorCode c_MotController_GetBusVoltage(void *handle, double *voltage);
+	ctre::phoenix::ErrorCode c_MotController_GetMotorOutputPercent(void *handle, double *percentOutput);
+	ctre::phoenix::ErrorCode c_MotController_GetOutputCurrent(void *handle, double *current);
+	ctre::phoenix::ErrorCode c_MotController_GetTemperature(void *handle, double *temperature);
+	ctre::phoenix::ErrorCode c_MotController_ConfigSelectedFeedbackSensor(void *handle, int feedbackDevice, int pidIdx, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigSelectedFeedbackCoefficient(void *handle, double coefficient, int pidIdx, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigRemoteFeedbackFilter(void *handle, int deviceID, int remoteSensorSource, int remoteOrdinal, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigSensorTerm(void *handle, int sensorTerm, int feedbackDevice, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_GetSelectedSensorPosition(void *handle, int *param, int pidIdx);
+	ctre::phoenix::ErrorCode c_MotController_GetSelectedSensorVelocity(void *handle, int *param, int pidIdx);
+	ctre::phoenix::ErrorCode c_MotController_SetSelectedSensorPosition(void *handle, int sensorPos, int pidIdx,int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_SetControlFramePeriod(void *handle, int frame, int periodMs);
+	ctre::phoenix::ErrorCode c_MotController_SetStatusFramePeriod(void *handle, int frame, int periodMs, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_GetStatusFramePeriod(void *handle, int frame, int *periodMs, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigVelocityMeasurementPeriod(void *handle, int period, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigVelocityMeasurementWindow(void *handle, int windowSize, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigForwardLimitSwitchSource(void *handle, int type, int normalOpenOrClose, int deviceID, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigReverseLimitSwitchSource(void *handle, int type, int normalOpenOrClose, int deviceID, int timeoutMs);
+	void c_MotController_OverrideLimitSwitchesEnable(void *handle, bool enable);
+	ctre::phoenix::ErrorCode c_MotController_ConfigForwardSoftLimitThreshold(void *handle, int forwardSensorLimit, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigReverseSoftLimitThreshold(void *handle, int reverseSensorLimit, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigForwardSoftLimitEnable(void *handle, bool enable, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigReverseSoftLimitEnable(void *handle, bool enable, int timeoutMs);
+	void c_MotController_OverrideSoftLimitsEnable(void *handle, bool enable);
+	ctre::phoenix::ErrorCode c_MotController_Config_kP(void *handle, int slotIdx, double value, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_Config_kI(void *handle, int slotIdx, double value, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_Config_kD(void *handle, int slotIdx, double value, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_Config_kF(void *handle, int slotIdx, double value, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_Config_IntegralZone(void *handle, int slotIdx, double izone, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigAllowableClosedloopError(void *handle, int slotIdx, int allowableClosedLoopError, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigMaxIntegralAccumulator(void *handle, int slotIdx, double iaccum, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigClosedLoopPeakOutput(void *handle, int slotIdx, double percentOut, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigClosedLoopPeriod(void *handle, int slotIdx, int loopTimeMs, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_SetIntegralAccumulator(void *handle, double iaccum, int pidIdx, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_GetClosedLoopError(void *handle, int *closedLoopError, int pidIdx);
+	ctre::phoenix::ErrorCode c_MotController_GetIntegralAccumulator(void *handle, double *iaccum, int pidIdx);
+	ctre::phoenix::ErrorCode c_MotController_GetErrorDerivative(void *handle, double *derror, int pidIdx);
+	ctre::phoenix::ErrorCode c_MotController_SelectProfileSlot(void *handle, int slotIdx, int pidIdx);
+	ctre::phoenix::ErrorCode c_MotController_GetActiveTrajectoryPosition(void *handle, int *param);
+	ctre::phoenix::ErrorCode c_MotController_GetActiveTrajectoryVelocity(void *handle, int *param);
+	ctre::phoenix::ErrorCode c_MotController_GetActiveTrajectoryHeading(void *handle, double *param);
+	ctre::phoenix::ErrorCode c_MotController_GetActiveTrajectoryAll(void *handle, int * vel, int * pos, double *heading);
+	ctre::phoenix::ErrorCode c_MotController_ConfigMotionCruiseVelocity(void *handle, int sensorUnitsPer100ms, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigMotionAcceleration(void *handle, int sensorUnitsPer100msPerSec, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ClearMotionProfileTrajectories(void *handle);
+	ctre::phoenix::ErrorCode c_MotController_GetMotionProfileTopLevelBufferCount(void *handle, int * value);
+	ctre::phoenix::ErrorCode c_MotController_PushMotionProfileTrajectory(void *handle, double position,
+			double velocity, double headingDeg, int profileSlotSelect, bool isLastPoint, bool zeroPos);
+ctre::phoenix::ErrorCode c_MotController_PushMotionProfileTrajectory_2(
+		void *handle, double position, double velocity, double headingDeg,
+		int profileSlotSelect0, int profileSlotSelect1, bool isLastPoint, bool zeroPos, int durationMs);
+	ctre::phoenix::ErrorCode c_MotController_IsMotionProfileTopLevelBufferFull(void *handle, bool * value);
+	ctre::phoenix::ErrorCode c_MotController_ProcessMotionProfileBuffer(void *handle);
+	ctre::phoenix::ErrorCode c_MotController_GetMotionProfileStatus(void *handle,
+			int *topBufferRem, int *topBufferCnt, int *btmBufferCnt,
+			bool *hasUnderrun, bool *isUnderrun, bool *activePointValid,
+			bool *isLast, int *profileSlotSelect, int *outputEnable);
+	ctre::phoenix::ErrorCode c_MotController_GetMotionProfileStatus_2(void *handle,
+			int *topBufferRem, int *topBufferCnt, int *btmBufferCnt,
+			bool *hasUnderrun, bool *isUnderrun, bool *activePointValid,
+			bool *isLast, int *profileSlotSelect, int *outputEnable, int *timeDurMs,
+			int *profileSlotSelect1);
+	ctre::phoenix::ErrorCode c_MotController_ClearMotionProfileHasUnderrun(void *handle,
+			int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ChangeMotionControlFramePeriod(void *handle,
+			int periodMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigMotionProfileTrajectoryPeriod(
+			void *handle, int durationMs, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_GetLastError(void *handle);
+	ctre::phoenix::ErrorCode c_MotController_GetFirmwareVersion(void *handle, int *);
+	ctre::phoenix::ErrorCode c_MotController_HasResetOccurred(void *handle,bool *);
+	ctre::phoenix::ErrorCode c_MotController_ConfigSetCustomParam(void *handle, int newValue, int paramIndex, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigGetCustomParam(void *handle, int *readValue, int paramIndex, int timoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigSetParameter(void *handle, int param, double value, int subValue, int ordinal, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigGetParameter(void *handle, int param, double *value, int ordinal, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigPeakCurrentLimit(void *handle, int amps, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigPeakCurrentDuration(void *handle, int milliseconds, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_ConfigContinuousCurrentLimit(void *handle, int amps, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_EnableCurrentLimit(void *handle, bool enable);
+	ctre::phoenix::ErrorCode c_MotController_SetLastError(void *handle, int error);
+	ctre::phoenix::ErrorCode c_MotController_GetAnalogIn(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_SetAnalogPosition(void *handle,int newPosition, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_GetAnalogInRaw(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetAnalogInVel(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetQuadraturePosition(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_SetQuadraturePosition(void *handle,int newPosition, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_GetQuadratureVelocity(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetPulseWidthPosition(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_SetPulseWidthPosition(void *handle,int newPosition, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_GetPulseWidthVelocity(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetPulseWidthRiseToFallUs(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetPulseWidthRiseToRiseUs(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetPinStateQuadA(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetPinStateQuadB(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetPinStateQuadIdx(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_IsFwdLimitSwitchClosed(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_IsRevLimitSwitchClosed(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetFaults(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_GetStickyFaults(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_MotController_ClearStickyFaults(void *handle, int timeoutMs);
+	ctre::phoenix::ErrorCode c_MotController_SelectDemandType(void *handle, bool enable);
+	ctre::phoenix::ErrorCode c_MotController_SetMPEOutput(void *handle, int MpeOutput);
+	ctre::phoenix::ErrorCode c_MotController_EnableHeadingHold(void *handle, bool enable);
+	ctre::phoenix::ErrorCode c_MotController_GetAnalogInAll(void *handle, int * withOv, int * raw, int * vel);
+	ctre::phoenix::ErrorCode c_MotController_GetQuadratureSensor(void *handle, int * pos, int * vel);
+	ctre::phoenix::ErrorCode c_MotController_GetPulseWidthAll(void *handle, int * pos, int * vel, int * riseToRiseUs, int * riseToFallUs);
+	ctre::phoenix::ErrorCode c_MotController_GetQuadPinStates(void *handle, int * quadA, int * quadB, int * quadIdx);
+	ctre::phoenix::ErrorCode c_MotController_GetLimitSwitchState(void *handle, int * isFwdClosed, int * isRevClosed);
+	ctre::phoenix::ErrorCode c_MotController_GetClosedLoopTarget(void *handle, int * value, int pidIdx);
+}
diff --git a/libraries/driver/include/ctre/phoenix/CCI/PigeonIMU_CCI.h b/libraries/driver/include/ctre/phoenix/CCI/PigeonIMU_CCI.h
new file mode 100644
index 0000000..f95b6ae
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/CCI/PigeonIMU_CCI.h
@@ -0,0 +1,84 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ * 
+ * Cross The Road Electronics (CTRE) licenses to you the right to 
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ * 
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, 
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+ 
+ #pragma once
+ 
+#include "ctre/phoenix/ErrorCode.h"
+ #include <map>
+ 
+ static std::map<void *, bool> pigeonPresent;
+ 
+ extern "C"{
+	void *c_PigeonIMU_Create2(int talonDeviceID);
+	void *c_PigeonIMU_Create1(int deviceNumber);
+	// void c_PigeonIMU_Destroy(void *handle);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetDescription(void *handle, char * toFill, int toFillByteSz, int * numBytesFilled);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ConfigSetParameter(void *handle, int param, double paramValue, int subValue, int ordinal, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ConfigGetParameter(void *handle, int param, double *value, int ordinal, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ConfigSetCustomParam(void *handle, int newValue, int paramIndex, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ConfigGetCustomParam(void *handle, int *readValue, int paramIndex, int timoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetYaw(void *handle, double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_AddYaw(void *handle, double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetYawToCompass(void *handle, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetFusedHeading(void *handle, double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_AddFusedHeading(void *handle, double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetFusedHeadingToCompass(void *handle, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetAccumZAngle(void *handle, double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ConfigTemperatureCompensationEnable(void *handle, int bTempCompEnable, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetCompassDeclination(void *handle, double angleDegOffset, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetCompassAngle(void *handle, double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_EnterCalibrationMode(void *handle, int calMode, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetGeneralStatus(void *handle, int *state, int *currentMode, int *calibrationError, int *bCalIsBooting, double *tempC, int *upTimeSec, int *noMotionBiasCount, int *tempCompensationCount, int *lastError);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetLastError(void *handle);
+	ctre::phoenix::ErrorCode c_PigeonIMU_Get6dQuaternion(void *handle, double wxyz[4]);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetYawPitchRoll(void *handle, double ypr[3]);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetAccumGyro(void *handle, double xyz_deg[3]);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetAbsoluteCompassHeading(void *handle, double *value);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetCompassHeading(void *handle, double *value);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetCompassFieldStrength(void *handle, double *value);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetTemp(void *handle, double *value);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetState(void *handle, int *state);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetUpTime(void *handle, int *value);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetRawMagnetometer(void *handle, short rm_xyz[3]);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetBiasedMagnetometer(void *handle, short bm_xyz[3]);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetBiasedAccelerometer(void *handle, short ba_xyz[3]);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetRawGyro(void *handle, double xyz_dps[3]);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetAccelerometerAngles(void *handle, double tiltAngles[3]);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetFusedHeading2(void *handle, int *bIsFusing, int *bIsValid, double *value, int *lastError);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetFusedHeading1(void *handle, double *value);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetResetCount(void *handle, int *value);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetResetFlags(void *handle, int *value);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetFirmwareVersion(void *handle, int * firmwareVers);
+	ctre::phoenix::ErrorCode c_PigeonIMU_HasResetOccurred(void *handle, bool * hasReset);
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetLastError(void *handle, int value);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ConfigSetCustomParam(void *handle, int newValue, int paramIndex, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ConfigGetCustomParam(void *handle, int *readValue, int paramIndex, int timoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ConfigSetParameter(void *handle, int param, double value, int subValue, int ordinal, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ConfigGetParameter(void *handle, int param, double *value, int ordinal, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetFaults(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetStickyFaults(void *handle, int * param);
+	ctre::phoenix::ErrorCode c_PigeonIMU_ClearStickyFaults(void *handle, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetStatusFramePeriod(void *handle, int frame, int periodMs, int timeoutMs);
+	ctre::phoenix::ErrorCode c_PigeonIMU_GetStatusFramePeriod(void *handle, int frame, int *periodMs, int timeoutMs) ;
+	ctre::phoenix::ErrorCode c_PigeonIMU_SetControlFramePeriod(void *handle, int frame, int periodMs) ;
+}
diff --git a/libraries/driver/include/ctre/phoenix/ErrorCode.h b/libraries/driver/include/ctre/phoenix/ErrorCode.h
new file mode 100644
index 0000000..889f436
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/ErrorCode.h
@@ -0,0 +1,86 @@
+#pragma once
+#include <stdint.h>
+
+namespace ctre {
+namespace phoenix {
+
+enum ErrorCode
+: int32_t
+{
+	OK = 0, 
+	OKAY = 0,		//!< No Error - Function executed as expected
+
+	//CAN-Related
+	CAN_MSG_STALE = 1,
+	CAN_TX_FULL = -1,
+	TxFailed = -1,				//!< Could not transmit the CAN frame.
+	InvalidParamValue = -2, 	//!< Caller passed an invalid param
+	CAN_INVALID_PARAM = -2,
+	RxTimeout = -3,	//!< CAN frame has not been received within specified period of time.
+	CAN_MSG_NOT_FOUND = -3,
+	TxTimeout = -4,				//!< Not used.
+	CAN_NO_MORE_TX_JOBS = -4,
+	UnexpectedArbId = -5,		//!< Specified CAN Id is invalid.
+	CAN_NO_SESSIONS_AVAIL = -5,
+	BufferFull = +6,//!< Caller attempted to insert data into a buffer that is full.
+	CAN_OVERFLOW = -6,
+	SensorNotPresent = -7,		//!< Sensor is not present
+	FirmwareTooOld = -8,
+	CouldNotChangePeriod = -9,
+
+
+	//General
+	GeneralError = -100,		//!< User Specified General Error
+	GENERAL_ERROR = -100,
+
+	//Signal
+	SIG_NOT_UPDATED = -200,
+	SigNotUpdated = -200,	//!< Have not received an value response for signal.
+	NotAllPIDValuesUpdated = -201,
+
+	//Gadgeteer Port Error Codes
+	//These include errors between ports and modules
+	GEN_PORT_ERROR = -300,
+	PORT_MODULE_TYPE_MISMATCH = -301,
+
+	//Gadgeteer Module Error Codes
+	//These apply only to the module units themselves
+	GEN_MODULE_ERROR = -400,
+	MODULE_NOT_INIT_SET_ERROR = -401,
+	MODULE_NOT_INIT_GET_ERROR = -402,
+
+	//API
+	WheelRadiusTooSmall = -500,
+	TicksPerRevZero = -501,
+	DistanceBetweenWheelsTooSmall = -502,
+	GainsAreNotSet = -503,
+
+	//Higher Level
+	IncompatibleMode = -600,
+	InvalidHandle = -601,		//!< Handle does not match stored map of handles
+	
+	//Firmware Versions
+	FeatureRequiresHigherFirm = -700,
+	TalonFeatureRequiresHigherFirm = -701,
+
+	//CAN Related
+	PulseWidthSensorNotPresent = 10,	//!< Special Code for "isSensorPresent"
+
+	//General
+	GeneralWarning = 100,
+	FeatureNotSupported = 101, // feature not implement in the API or firmware
+	NotImplemented = 102, // feature not implement in the API
+	FirmVersionCouldNotBeRetrieved = 103,
+	FeaturesNotAvailableYet = 104, // feature will be release in an upcoming release
+	ControlModeNotValid = 105, // Current control mode of motor controller not valid for this call
+
+	ControlModeNotSupportedYet = 106,
+	CascadedPIDNotSupporteYet= 107,
+	AuxiliaryPIDNotSupportedYet= 107,
+	RemoteSensorsNotSupportedYet= 108,
+	MotProfFirmThreshold= 109,
+	MotProfFirmThreshold2 = 110,
+};
+
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/LowLevel/CANBusAddressable.h b/libraries/driver/include/ctre/phoenix/LowLevel/CANBusAddressable.h
new file mode 100644
index 0000000..407bd24
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/LowLevel/CANBusAddressable.h
@@ -0,0 +1,20 @@
+#pragma once
+#include <stdint.h>
+/**
+ * Simple address holder.  This will be removed TBD.
+ */
+class CANBusAddressable {
+
+public:
+	CANBusAddressable(uint8_t deviceNumber) {
+		_deviceNum = deviceNumber;
+	}
+
+	uint8_t GetDeviceNumber() {
+		return _deviceNum;
+	}
+protected:
+private:
+	uint32_t _deviceNum;
+};
+
diff --git a/libraries/driver/include/ctre/phoenix/LowLevel/CANifier_LowLevel.h b/libraries/driver/include/ctre/phoenix/LowLevel/CANifier_LowLevel.h
new file mode 100644
index 0000000..f8a5622
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/LowLevel/CANifier_LowLevel.h
@@ -0,0 +1,109 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ * 
+ * Cross The Road Electronics (CTRE) licenses to you the right to 
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ * 
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, 
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+
+#pragma once
+
+#include "Device_LowLevel.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/CANifierControlFrame.h"
+#include "ctre/phoenix/CANifierFaults.h"
+#include "ctre/phoenix/CANifierStatusFrame.h"
+#include "ctre/phoenix/CANifierStickyFaults.h"
+#include "ctre/phoenix/CANifierVelocityMeasPeriod.h"
+#include <FRC_NetworkCommunication/CANSessionMux.h>  //CAN Comm
+#include <map>
+
+/** 
+ * CANifier Class.
+ * Class supports communicating over CANbus.
+ */
+class LowLevelCANifier : public Device_LowLevel {
+
+public:
+	enum GeneralPin{
+		QUAD_IDX = 0,
+		QUAD_B = 1,
+		QUAD_A = 2,
+		LIMR = 3,
+		LIMF = 4,
+		SDA = 5,
+		SCL = 6,
+		SPI_CS = 7,
+		SPI_MISO_PWM2P = 8,
+		SPI_MOSI_PWM1P = 9,
+		SPI_CLK_PWM0P = 10,
+	};
+
+	explicit LowLevelCANifier(int deviceNumber = 0);
+
+	ctre::phoenix::ErrorCode SetLEDOutput( int  dutyCycle,  int  ledChannel);
+	ctre::phoenix::ErrorCode SetGeneralOutputs( int  outputsBits,  int  isOutputBits);
+	ctre::phoenix::ErrorCode SetGeneralOutput(GeneralPin outputPin, bool bOutputValue, bool bOutputEnable);
+	ctre::phoenix::ErrorCode SetPWMOutput( int  pwmChannel,  int  dutyCycle);
+	ctre::phoenix::ErrorCode EnablePWMOutput( int  pwmChannel,  bool  bEnable);
+	ctre::phoenix::ErrorCode GetGeneralInputs(bool allPins[], uint32_t capacity);
+	ctre::phoenix::ErrorCode GetGeneralInput(GeneralPin inputPin, bool * measuredInput);
+	ctre::phoenix::ErrorCode GetPWMInput( int  pwmChannel,  double dutyCycleAndPeriod[2]);
+	ctre::phoenix::ErrorCode GetLastError();
+	ctre::phoenix::ErrorCode GetBatteryVoltage(double * batteryVoltage);
+	ctre::phoenix::ErrorCode SetLastError(ctre::phoenix::ErrorCode error);
+	ctre::phoenix::ErrorCode GetQuadraturePosition(int * pos);
+	ctre::phoenix::ErrorCode SetQuadraturePosition(int newPosition, int timeoutMs);
+	ctre::phoenix::ErrorCode GetQuadratureVelocity(int * vel);
+	ctre::phoenix::ErrorCode GetQuadratureSensor(int * pos, int * vel);
+	ctre::phoenix::ErrorCode ConfigVelocityMeasurementPeriod(
+			ctre::phoenix::CANifierVelocityMeasPeriod period, int timeoutMs);
+	ctre::phoenix::ErrorCode ConfigVelocityMeasurementWindow(int windowSize, int timeoutMs);
+
+	ctre::phoenix::ErrorCode GetFaults(ctre::phoenix::CANifierFaults & toFill);
+	ctre::phoenix::ErrorCode GetStickyFaults(ctre::phoenix::CANifierStickyFaults & toFill) ;
+	ctre::phoenix::ErrorCode ClearStickyFaults(int timeoutMs) ;
+	ctre::phoenix::ErrorCode SetStatusFramePeriod(ctre::phoenix::CANifierStatusFrame frame, int periodMs, int timeoutMs) ;
+	ctre::phoenix::ErrorCode GetStatusFramePeriod(ctre::phoenix::CANifierStatusFrame frame,
+			int & periodMs, int timeoutMs) ;
+	ctre::phoenix::ErrorCode SetControlFramePeriod(ctre::phoenix::CANifierControlFrame frame,
+			int periodMs);
+
+	const static int kMinFirmwareVersionMajor = 0;
+	const static int kMinFirmwareVersionMinor = 40;
+
+private:
+
+	static const int kDefaultControlPeriodMs = 20;
+	static const int kDefaultPwmOutputPeriodMs = 20;
+
+	bool _SendingPwmOutput = false;
+
+    uint32_t _regInput = 0; //!< Decoded inputs
+    uint32_t _regLat = 0; //!< Decoded output latch
+    uint32_t _regIsOutput = 0; //!< Decoded data direction register
+
+	ctre::phoenix::ErrorCode _lastError = ctre::phoenix::OKAY;
+
+	void CheckFirmVers(int minMajor = kMinFirmwareVersionMajor, int minMinor = kMinFirmwareVersionMinor, 
+							ctre::phoenix::ErrorCode failCode = ctre::phoenix::ErrorCode::FirmwareTooOld);
+	void EnsurePwmOutputFrameIsTransmitting();
+	void EnableFirmStatusFrame(bool enable);
+};
+
diff --git a/libraries/driver/include/ctre/phoenix/LowLevel/CTRE_Native_CAN.h b/libraries/driver/include/ctre/phoenix/LowLevel/CTRE_Native_CAN.h
new file mode 100644
index 0000000..207e6c9
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/LowLevel/CTRE_Native_CAN.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include <FRC_NetworkCommunication/CANSessionMux.h>  //CAN Comm
+
+int CTRE_Native_CAN_GetSendBuffer(int arbId, uint64_t & data);
+int CTRE_Native_CAN_Receive(int arbId, uint64_t & data, int & len, bool allowStale = true);
+int CTRE_Native_CAN_Send(int arbId, uint64_t data, int len, int periodMs);
diff --git a/libraries/driver/include/ctre/phoenix/LowLevel/Device_LowLevel.h b/libraries/driver/include/ctre/phoenix/LowLevel/Device_LowLevel.h
new file mode 100644
index 0000000..a20b551
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/LowLevel/Device_LowLevel.h
@@ -0,0 +1,122 @@
+#pragma once
+
+#include <map>
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/LowLevel/ResetStats.h"
+#include <FRC_NetworkCommunication/CANSessionMux.h>  // tCANStreamMessage
+
+class Device_LowLevel {
+
+protected:
+	int32_t _baseArbId;
+	/** child class has to provide a way to enable/disable firm status */
+	virtual void EnableFirmStatusFrame(bool enable) = 0;
+	virtual ctre::phoenix::ErrorCode SetLastError(ctre::phoenix::ErrorCode enable) = 0;
+
+	/**
+	 * Change the periodMs of a TALON's status frame.  See kStatusFrame_* enums for
+	 * what's available.
+	 */
+	ctre::phoenix::ErrorCode SetStatusFramePeriod_(int32_t statusArbID, int32_t periodMs,
+			int32_t timeoutMs);
+	ctre::phoenix::ErrorCode GetStatusFramePeriod_(int32_t statusArbID, int32_t &periodMs,
+			int32_t timeoutMs);
+
+	int32_t GetStartupStatus();
+
+	void CheckFirmVers(int minMajor, int minMinor,
+			ctre::phoenix::ErrorCode failCode =
+					ctre::phoenix::ErrorCode::FirmwareTooOld);
+
+	ctre::phoenix::ErrorCode ConfigSetParameter(ctre::phoenix::ParamEnum paramEnum, int32_t value,
+			uint8_t subValue, int32_t ordinal, int32_t timeoutMs);
+
+	ctre::phoenix::ErrorCode ConfigGetParameter(ctre::phoenix::ParamEnum paramEnum, int32_t valueToSend,
+			int32_t & valueReceived, int32_t & subValue, int32_t ordinal,
+			int32_t timeoutMs);
+
+	ctre::phoenix::ErrorCode ConfigGetParameter(ctre::phoenix::ParamEnum paramEnum, int32_t &value,
+			int32_t ordinal, int32_t timeoutMs);
+
+	/** child class should call this once to set the description */
+	void SetDescription(const std::string & description);
+private:
+	std::string _description;
+	int32_t _deviceNumber;
+
+	int32_t _arbIdStartupFrame;
+	int32_t _arbIdFrameApiStatus;
+
+	uint64_t _cache = 0;
+	int32_t _len = 0;
+
+	uint32_t _arbIdParamRequest;
+	uint32_t _arbIdParamResp;
+	uint32_t _arbIdParamSet;
+	int32_t kParamArbIdMask;
+
+	uint32_t _can_h = 0;
+	int32_t _can_stat = 0;
+	struct tCANStreamMessage _msgBuff[20];
+	static const uint32_t kMsgCapacity = 20;
+
+	ResetStats _resetStats;
+	int32_t _firmVers = -1; /* invalid */
+
+	std::map<uint32_t, int32_t> _sigs_Value;
+	std::map<uint32_t, int32_t> _sigs_SubValue;
+	std::map<uint32_t, int32_t> _sigs_Ordinal;
+
+	const int32_t kFullMessageIDMask = 0x1fffffff;
+	const double FLOAT_TO_FXP_10_22 = (double) 0x400000;
+	const double FXP_TO_FLOAT_10_22 = 0.0000002384185791015625f;
+	const double FLOAT_TO_FXP_0_8 = (double) 0x100;
+	const double FXP_TO_FLOAT_0_8 = 0.00390625f;
+
+	int _failedVersionChecks = 0;
+	void OpenSessionIfNeedBe();
+	void ProcessStreamMessages();
+
+
+	ctre::phoenix::ErrorCode RequestParam(ctre::phoenix::ParamEnum paramEnum, int32_t value, uint8_t subValue,
+			int32_t ordinal);
+
+	ctre::phoenix::ErrorCode PollForParamResponse(ctre::phoenix::ParamEnum paramEnum, int32_t & value, int32_t & subValue, int32_t & ordinal);
+
+public:
+
+	Device_LowLevel(int32_t baseArbId, int32_t arbIdStartupFrame,
+			int32_t paramReqId, int32_t paramRespId, int32_t paramSetId,
+			int32_t arbIdFrameApiStatus);
+	Device_LowLevel(const Device_LowLevel &) = delete;
+	virtual ~Device_LowLevel() {}
+
+	int GetDeviceNumber();
+	ctre::phoenix::ErrorCode GetDeviceNumber(int & deviceNumber);
+	int32_t GetFirmStatus();
+
+	ctre::phoenix::ErrorCode GetResetCount(int & param);
+	ctre::phoenix::ErrorCode GetResetFlags(int &param);
+	/** return -1 if not available, return 0xXXYY format if available */
+	ctre::phoenix::ErrorCode GetFirmwareVersion(int &param);
+	int GetFirmwareVersion();
+	/**
+	 * @return true iff a reset has occurred since last call.
+	 */
+	ctre::phoenix::ErrorCode HasResetOccurred(bool & param);
+	bool HasResetOccurred();
+
+	ctre::phoenix::ErrorCode ConfigSetParameter(ctre::phoenix::ParamEnum paramEnum, double value,
+			uint8_t subValue, int32_t ordinal, int32_t timeoutMs);
+
+	ctre::phoenix::ErrorCode ConfigGetParameter(ctre::phoenix::ParamEnum paramEnum, double &value,
+			int32_t ordinal, int32_t timeoutMs);
+
+	ctre::phoenix::ErrorCode ConfigSetCustomParam(int value, int paramIndex, int timeoutMs);
+	ctre::phoenix::ErrorCode ConfigGetCustomParam(int & value, int paramIndex, int timeoutMs);
+
+	const std::string & ToString() const;
+
+	void GetDescription(char * toFill, int toFillByteSz, int & numBytesFilled);
+};
diff --git a/libraries/driver/include/ctre/phoenix/LowLevel/Logger_LowLevel.h b/libraries/driver/include/ctre/phoenix/LowLevel/Logger_LowLevel.h
new file mode 100644
index 0000000..552c1f2
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/LowLevel/Logger_LowLevel.h
@@ -0,0 +1,60 @@
+#pragma once
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include <string.h>
+#include <iostream>
+#include <fstream>
+#include <map>
+#include <sys/stat.h>
+#include <vector>
+#include <dirent.h>
+#include <mutex>
+#include <thread>
+
+/* forward prototype */
+namespace ctre {
+namespace phoenix {
+namespace logger {
+class TimestampMsgMap;
+}
+}
+}
+
+class LoggerDriver {
+public:
+	void Looping();
+	void Open(int language, bool logDriverStation);
+	void Close();
+
+	static const int kHierarchyInternal = 0;
+	static const int kHierarchyCCI = 1;
+	static const int kHierarchyJava = 2;
+	static const int kHierarchyAPI = 3;
+
+
+	ctre::phoenix::ErrorCode Log(ctre::phoenix::ErrorCode code, const std::string & origin, int hierarchy, const char *stacktrace);
+	void GetDescription(ctre::phoenix::ErrorCode code, std::string & shrtError, std::string & longError);
+
+	static LoggerDriver & GetInstance();
+private:
+	LoggerDriver();
+	~LoggerDriver();
+	static unsigned long GetDirSize(DIR *t);
+	static std::vector<std::string> OrderedFiles(DIR *directory);
+	
+	ctre::phoenix::logger::TimestampMsgMap * _msgMap;
+
+//	unsigned long startOfSeconds = 0;
+//	//std::map<std::string, unsigned long> timeStamps;
+//	bool writing = false;
+//	bool errorF = false;
+//	bool threadStarted = false;
+//	bool closeThread = false;
+//	std::string buf = "";
+//	std::mutex thisMutex;
+//	std::thread t;
+//	FILE *file = NULL;
+
+	static LoggerDriver *_instance;
+	
+};
diff --git a/libraries/driver/include/ctre/phoenix/LowLevel/MotControllerWithBuffer_LowLevel.h b/libraries/driver/include/ctre/phoenix/LowLevel/MotControllerWithBuffer_LowLevel.h
new file mode 100644
index 0000000..221cb3e
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/LowLevel/MotControllerWithBuffer_LowLevel.h
@@ -0,0 +1,112 @@
+#pragma once
+
+#include "ctre/phoenix/LowLevel/MotController_LowLevel.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+#include "ctre/phoenix/Motion/MotionProfileStatus.h"
+#include <string>
+#include <stdint.h>
+#include <mutex>
+
+struct _Control_6_MotProfAddTrajPoint_t;
+
+namespace ctre {
+namespace phoenix {
+namespace platform {
+namespace can {
+
+template<typename T> class txTask;
+//class txTask;
+}
+}
+}
+}
+
+
+namespace ctre {
+namespace phoenix {
+namespace motion {
+namespace can {
+class txJob_t;
+}
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace motion {
+class TrajectoryBuffer;
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace lowlevel {
+
+class MotControllerWithBuffer_LowLevel: public MotController_LowLevel {
+
+public:
+	MotControllerWithBuffer_LowLevel(int baseArbId);
+	virtual ctre::phoenix::ErrorCode ClearMotionProfileTrajectories();
+	virtual ctre::phoenix::ErrorCode GetMotionProfileTopLevelBufferCount(int & count);
+	virtual ctre::phoenix::ErrorCode PushMotionProfileTrajectory(double position, double velocity, double headingDeg, int profileSlotSelect0, int profileSlotSelect1, bool isLastPoint, bool zeroPos, int durationMs);
+	virtual ctre::phoenix::ErrorCode IsMotionProfileTopLevelBufferFull(bool & param);
+	virtual ctre::phoenix::ErrorCode ProcessMotionProfileBuffer();
+	virtual ctre::phoenix::ErrorCode GetMotionProfileStatus(int &topBufferRem,
+			int &topBufferCnt,
+			int &btmBufferCnt,
+			bool &hasUnderrun,
+			bool &isUnderrun,
+			bool &activePointValid,
+			bool &isLast,
+			int &profileSlotSelect0,
+			int &outputEnable,
+			int &timeDurMs,
+			int &profileSlotSelect1);
+	virtual ctre::phoenix::ErrorCode ClearMotionProfileHasUnderrun(int timeoutMs);
+	virtual ctre::phoenix::ErrorCode ChangeMotionControlFramePeriod(int periodMs);
+	virtual ctre::phoenix::ErrorCode ConfigMotionProfileTrajectoryPeriod(int durationMs, int timeoutMs);
+private:
+
+#if 1
+	/**
+	 * To keep buffers from getting out of control, place a cap on the top level buffer.  Calling
+	 * application can stream addition points as they are fed to Talon. Approx memory footprint is
+	 * this capacity X 8 bytes.
+	 */
+
+	/** Buffer for mot prof top data. */
+	ctre::phoenix::motion::TrajectoryBuffer * _motProfTopBuffer;
+	/** Flow control for streaming trajectories. */
+	int32_t _motProfFlowControl = -1;
+
+	/** The mut mot prof. */
+	std::mutex _mutMotProf; // use this std::unique_lock<std::mutex> lck (_mutMotProf);
+
+	/** Frame Period of the motion profile control6 frame. */
+	uint _control6PeriodMs = kDefaultControl6PeriodMs;
+
+	/**
+	 * @return the tx task that transmits Control6 (motion profile control).
+	 *         If it's not scheduled, then schedule it.  This is part
+	 *         of making the lazy-framing that only peforms MotionProf framing
+	 *         when needed to save bandwidth.
+	 */
+	void GetControl6(ctre::phoenix::platform::can::txTask<uint64_t> & toFill);
+	/**
+	 * Caller is either pushing a new motion profile point, or is
+	 * calling the Process buffer routine.  In either case check our
+	 * flow control to see if we need to start sending control6.
+	 */
+	void ReactToMotionProfileCall();
+
+#endif
+};
+
+} // LowLevel
+} // MotorControl
+} // Phoenix
+} // CTRE
+
diff --git a/libraries/driver/include/ctre/phoenix/LowLevel/MotController_LowLevel.h b/libraries/driver/include/ctre/phoenix/LowLevel/MotController_LowLevel.h
new file mode 100644
index 0000000..542c151
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/LowLevel/MotController_LowLevel.h
@@ -0,0 +1,222 @@
+#pragma once
+
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/LowLevel/Device_LowLevel.h"
+#include "ctre/phoenix/MotorControl/FeedbackDevice.h"
+#include "ctre/phoenix/MotorControl/ControlFrame.h"
+#include "ctre/phoenix/MotorControl/SensorTerm.h"
+#include "ctre/phoenix/MotorControl/RemoteSensorSource.h"
+#include "ctre/phoenix/MotorControl/Faults.h"
+#include "ctre/phoenix/MotorControl/StickyFaults.h"
+#include "ctre/phoenix/MotorControl/NeutralMode.h"
+#include "ctre/phoenix/MotorControl/ControlMode.h"
+#include "ctre/phoenix/MotorControl/LimitSwitchType.h"
+#include "ctre/phoenix/MotorControl/StatusFrame.h"
+#include "ctre/phoenix/MotorControl/VelocityMeasPeriod.h"
+#include <string>
+#include <stdint.h>
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace lowlevel {
+
+class MotController_LowLevel: public Device_LowLevel {
+
+protected:
+	const uint32_t STATUS_01 = 0x041400;
+	const uint32_t STATUS_02 = 0x041440;
+	const uint32_t STATUS_03 = 0x041480;
+	const uint32_t STATUS_04 = 0x0414C0;
+	const uint32_t STATUS_05 = 0x041500;
+	const uint32_t STATUS_06 = 0x041540;
+	const uint32_t STATUS_07 = 0x041580;
+	const uint32_t STATUS_08 = 0x0415C0;
+	const uint32_t STATUS_09 = 0x041600;
+	const uint32_t STATUS_10 = 0x041640;
+	const uint32_t STATUS_11 = 0x041680;
+	const uint32_t STATUS_12 = 0x0416C0;
+	const uint32_t STATUS_13 = 0x041700;
+	const uint32_t STATUS_14 = 0x041740;
+	const uint32_t STATUS_15 = 0x041780;
+
+	const uint32_t CONTROL_1 = 0x040000;
+	//const uint32_t CONTROL_2 = 0x040040;
+	const uint32_t CONTROL_3 = 0x040080;
+	const uint32_t CONTROL_5 = 0x040100;
+	const uint32_t CONTROL_6 = 0x040140;
+
+	const double FLOAT_TO_FXP_10_22 = (double) 0x400000;
+	const double FXP_TO_FLOAT_10_22 = 0.0000002384185791015625f;
+
+	const double FLOAT_TO_FXP_0_8 = (double) 0x100;
+	const double FXP_TO_FLOAT_0_8 = 0.00390625f;
+
+	/* Motion Profile Set Output */
+	// Motor output is neutral, Motion Profile Executer is not running.
+	const int kMotionProf_Disabled = 0;
+	// Motor output is updated from Motion Profile Executer, MPE will
+	// process the buffered points.
+	const int kMotionProf_Enable = 1;
+	// Motor output is updated from Motion Profile Executer, MPE will
+	// stay processing current trajectory point.
+	const int kMotionProf_Hold = 2;
+
+	const int kDefaultControl6PeriodMs = 10;
+
+	void EnableFirmStatusFrame(bool enable);
+	ErrorCode SetLastError(ErrorCode errorCode);
+	ErrorCode SetLastError(int errorCode);
+
+	ErrorCode ConfigSingleLimitSwitchSource(
+			LimitSwitchSource limitSwitchSource, LimitSwitchNormal normalOpenOrClose,
+			int deviceIDIfApplicable, int timeoutMs, bool isForward);
+public:
+	MotController_LowLevel(int baseArbId);
+
+	ErrorCode SetDemand(ctre::phoenix::motorcontrol::ControlMode mode, int demand0, int demand1);
+	ErrorCode Set(ctre::phoenix::motorcontrol::ControlMode mode, double demand0, double demand1, int demand1Type);
+	void SelectDemandType(bool enable);
+	void SetMPEOutput(int MpeOutput);
+	void EnableHeadingHold(bool enable);
+	void SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode neutralMode);
+	void SetSensorPhase(bool PhaseSensor);
+	void SetInverted(bool invert);
+
+	ErrorCode ConfigOpenLoopRamp(double secondsFromNeutralToFull, int timeoutMs);
+	ErrorCode ConfigClosedLoopRamp(double secondsFromNeutralToFull,
+			int timeoutMs);
+	ErrorCode ConfigPeakOutputForward(double percentOut, int timeoutMs);
+	ErrorCode ConfigPeakOutputReverse(double percentOut, int timeoutMs);
+	ErrorCode ConfigNominalOutputForward(double percentOut, int timeoutMs);
+	ErrorCode ConfigNominalOutputReverse(double percentOut, int timeoutMs);
+	ErrorCode ConfigNeutralDeadband(double percentDeadband,
+			int timeoutMs);
+	ErrorCode ConfigVoltageCompSaturation(double voltage, int timeoutMs);
+	ErrorCode ConfigVoltageMeasurementFilter(int filterWindowSamples,
+			int timeoutMs);
+	void EnableVoltageCompensation(bool enable);
+	ErrorCode GetBusVoltage(double & param);
+	ErrorCode GetMotorOutputPercent(double & param);
+	ErrorCode GetOutputCurrent(double & param);
+	ErrorCode GetTemperature(double & param);
+	ErrorCode ConfigSelectedFeedbackSensor(ctre::phoenix::motorcontrol::FeedbackDevice feedbackDevice,
+			int pidIdx, int timeoutMs);
+	ErrorCode ConfigSelectedFeedbackCoefficient(double coefficient, int pidIdx, int timeoutMs);
+	ErrorCode ConfigRemoteFeedbackFilter(int deviceID,
+			RemoteSensorSource remoteSensorSource, int remoteOrdinal, int timeoutMs);
+	ErrorCode ConfigSensorTerm(SensorTerm sensorTerm, FeedbackDevice feedbackDevice, int timeoutMs);
+	ErrorCode GetSelectedSensorPosition(int & param, int pidIdx);
+	ErrorCode GetSelectedSensorVelocity(int & param, int pidIdx);
+	ErrorCode SetSelectedSensorPosition(int sensorPos, int pidIdx, int timeoutMs);
+	ErrorCode SetControlFramePeriod(
+			ctre::phoenix::motorcontrol::ControlFrame frame, int periodMs);
+	ErrorCode SetStatusFramePeriod(
+			ctre::phoenix::motorcontrol::StatusFrame frame, int periodMs,
+			int timeoutMs);
+	ErrorCode SetStatusFramePeriod(
+			ctre::phoenix::motorcontrol::StatusFrameEnhanced frame,
+			int periodMs, int timeoutMs);
+	ErrorCode GetStatusFramePeriod(
+			ctre::phoenix::motorcontrol::StatusFrame frame, int & periodMs,
+			int timeoutMs);
+	ErrorCode GetStatusFramePeriod(
+			ctre::phoenix::motorcontrol::StatusFrameEnhanced frame,
+			int & periodMs, int timeoutMs);
+	ErrorCode ConfigVelocityMeasurementPeriod(
+			ctre::phoenix::motorcontrol::VelocityMeasPeriod period,
+			int timeoutMs);
+	ErrorCode ConfigVelocityMeasurementWindow(int windowSize, int timeoutMs);
+	ErrorCode ConfigForwardLimitSwitchSource(
+			ctre::phoenix::motorcontrol::LimitSwitchSource type,
+			ctre::phoenix::motorcontrol::LimitSwitchNormal normalOpenOrClose,
+			int deviceIDIfApplicable, int timeoutMs);
+	ErrorCode ConfigReverseLimitSwitchSource(
+			ctre::phoenix::motorcontrol::LimitSwitchSource type,
+			ctre::phoenix::motorcontrol::LimitSwitchNormal normalOpenOrClose,
+			int deviceIDIfApplicable, int timeoutMs);
+	void OverrideLimitSwitchesEnable(bool enable);
+	ErrorCode ConfigForwardSoftLimitThreshold(int forwardSensorLimit, int timeoutMs);
+	ErrorCode ConfigReverseSoftLimitThreshold(int reverseSensorLimit, int timeoutMs);
+	ErrorCode ConfigForwardSoftLimitEnable(bool enable, int timeoutMs);
+	ErrorCode ConfigReverseSoftLimitEnable(bool enable, int timeoutMs);
+	void OverrideSoftLimitsEnable(bool enable);
+	ErrorCode ConfigPeakCurrentLimit(int amps, int timeoutMs);
+	ErrorCode ConfigPeakCurrentDuration(int milliseconds, int timeoutMs);
+	ErrorCode ConfigContinuousCurrentLimit(int amps, int timeoutMs);
+	void EnableCurrentLimit(bool enable);
+	ErrorCode Config_kP(int slotIdx, double value, int timeoutMs);
+	ErrorCode Config_kI(int slotIdx, double value, int timeoutMs);
+	ErrorCode Config_kD(int slotIdx, double value, int timeoutMs);
+	ErrorCode Config_kF(int slotIdx, double value, int timeoutMs);
+	ErrorCode Config_IntegralZone(int slotIdx, int izone, int timeoutMs);
+	ErrorCode ConfigAllowableClosedloopError(int slotIdx,
+			int allowableCloseLoopError, int timeoutMs);
+	ErrorCode ConfigMaxIntegralAccumulator(int slotIdx, double iaccum,
+			int timeoutMs);
+	ErrorCode ConfigClosedLoopPeakOutput(int slotIdx, double percentOut, int timeoutMs);
+	ErrorCode ConfigClosedLoopPeriod(int slotIdx, int loopTimeMs, int timeoutMs);
+	ErrorCode SetIntegralAccumulator(double iaccum, int pidIdx, int timeoutMs);
+	ErrorCode GetClosedLoopError(int & error, int pidIdx);
+	ErrorCode GetIntegralAccumulator(double & iaccum, int pidIdx);
+	ErrorCode GetErrorDerivative(double & derivError, int pidIdx);
+	ErrorCode SelectProfileSlot(int slotIdx, int pidIdx);
+
+	ErrorCode GetFaults(ctre::phoenix::motorcontrol::Faults & toFill);
+	ErrorCode GetStickyFaults(ctre::phoenix::motorcontrol::StickyFaults& toFill);
+	ErrorCode ClearStickyFaults(int timeoutMs);
+
+	ErrorCode GetAnalogInWithOv(int & param);
+	ErrorCode GetAnalogInVel(int & param);
+	ErrorCode GetAnalogInAll(int & withOv, int & vRaw, int & vel);
+	ErrorCode GetQuadraturePosition(int & param);
+	ErrorCode GetQuadratureVelocity(int & param);
+	ErrorCode GetQuadratureSensor(int & pos, int & vel);
+	ErrorCode GetPulseWidthPosition(int & param);
+	ErrorCode GetPulseWidthVelocity(int & param);
+	ErrorCode GetPulseWidthRiseToFallUs(int & param);
+	ErrorCode GetPulseWidthRiseToRiseUs(int & param);
+	ErrorCode GetPulseWidthAll(int & pos, int & vel, int & riseToRiseUs, int & riseToFallUs);
+	ErrorCode GetPinStateQuadA(int & param);
+	ErrorCode GetPinStateQuadB(int & param);
+	ErrorCode GetPinStateQuadIdx(int & param);
+	ErrorCode GetQuadPinStates(int & quadA, int & quadB, int & quadIdx);
+	ErrorCode IsFwdLimitSwitchClosed(int & param);
+	ErrorCode IsRevLimitSwitchClosed(int & param);
+	ErrorCode GetLimitSwitchState(int & isFwdClosed, int & isRevClosed);
+	ErrorCode GetLastError();
+
+	ErrorCode ConfigMotionCruiseVelocity(int sensorUnitsPer100ms, int timeoutMs);
+	ErrorCode ConfigMotionAcceleration(int sensorUnitsPer100msPerSec, int timeoutMs);
+
+	ErrorCode GetClosedLoopTarget(int & value, int pidIdx);
+	ErrorCode GetActiveTrajectoryPosition(int & sensorUnits);
+	ErrorCode GetActiveTrajectoryVelocity(int & sensorUnitsPer100ms);
+	ErrorCode GetActiveTrajectoryHeading(double & turnUnits);
+	ErrorCode GetActiveTrajectoryAll(int & vel, int & pos, double & heading);
+
+	ErrorCode SetAnalogPosition(int newPosition, int timeoutMs);
+	ErrorCode SetQuadraturePosition(int newPosition, int timeoutMs);
+	ErrorCode SetPulseWidthPosition(int newPosition, int timeoutMs);
+
+	const static int kMinFirmwareVersionMajor = 3;
+	const static int kMinFirmwareVersionMinor = 0;
+
+private:
+
+	uint64_t _cache = 0;
+	int32_t _len = 0;
+	ErrorCode _lastError = (ErrorCode)0;
+	int32_t _setPoint = 0;
+	ControlMode _appliedMode = ControlMode::Disabled;
+	void CheckFirmVers(int minMajor = kMinFirmwareVersionMajor, int minMinor = kMinFirmwareVersionMinor, 
+			ErrorCode code = ErrorCode::FirmwareTooOld);
+	int _usingAdvancedFeatures = 0;
+
+};
+
+}
+}
+}
+}
diff --git a/libraries/driver/include/ctre/phoenix/LowLevel/PigeonIMU_LowLevel.h b/libraries/driver/include/ctre/phoenix/LowLevel/PigeonIMU_LowLevel.h
new file mode 100644
index 0000000..9f6a3c6
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/LowLevel/PigeonIMU_LowLevel.h
@@ -0,0 +1,273 @@
+/*
+ *  Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics.  All rights
+ * reserved.
+ * 
+ * Cross The Road Electronics (CTRE) licenses to you the right to 
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ * 
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, 
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+
+#pragma once
+
+#include "Device_LowLevel.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_ControlFrame.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_Faults.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_StatusFrame.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_StickyFaults.h"
+#include <string>
+
+/** 
+ * Pigeon IMU Class.
+ * Class supports communicating over CANbus and over ribbon-cable (CAN Talon SRX).
+ */
+class LowLevelPigeonImu : public Device_LowLevel {
+public:
+	/** Data object for holding fusion information. */
+	struct FusionStatus {
+		double heading;
+		bool bIsValid;
+		bool bIsFusing;
+		std::string description;
+		/**
+		 * Same as GetLastError()
+		 */
+		int lastError;
+	};
+	/** Various calibration modes supported by Pigeon. */
+	enum CalibrationMode {
+		BootTareGyroAccel = 0,
+		Temperature = 1,
+		Magnetometer12Pt = 2,
+		Magnetometer360 = 3,
+		Accelerometer = 5,
+	};
+	/** Overall state of the Pigeon. */
+	enum PigeonState {
+		NoComm,
+		Initializing,
+		Ready,
+		UserCalibration,
+	};
+	/**
+	 * Data object for status on current calibration and general status.
+	 *
+	 * Pigeon has many calibration modes supported for a variety of uses.
+	 * The modes generally collects and saves persistently information that makes
+	 * the Pigeon signals more accurate.  This includes collecting temperature, gyro, accelerometer,
+	 * and compass information.
+	 *
+	 * For FRC use-cases, typically compass and temperature calibration is not required.
+	 *
+	 * Additionally when motion driver software in the Pigeon boots, it will perform a fast boot calibration
+	 * to initially bias gyro and setup accelerometer.
+	 *
+	 * These modes can be enabled with the EnterCalibration mode.
+	 *
+	 * When a calibration mode is entered, caller can expect...
+	 *
+	 *  - PigeonState to reset to Initializing and bCalIsBooting is set to true.  Pigeon LEDs will blink the boot pattern.
+	 *  	This is similar to the normal boot cal, however it can an additional ~30 seconds since calibration generally
+	 *  	requires more information.
+	 *  	currentMode will reflect the user's selected calibration mode.
+	 *
+	 *  - PigeonState will eventually settle to UserCalibration and Pigeon LEDs will show cal specific blink patterns.
+	 *  	bCalIsBooting is now false.
+	 *
+	 *  - Follow the instructions in the Pigeon User Manual to meet the calibration specific requirements.
+	 * 		When finished calibrationError will update with the result.
+	 * 		Pigeon will solid-fill LEDs with red (for failure) or green (for success) for ~5 seconds.
+	 * 		Pigeon then perform boot-cal to cleanly apply the newly saved calibration data.
+	 */
+	struct GeneralStatus {
+		/**
+		 * The current state of the motion driver.  This reflects if the sensor signals are accurate.
+		 * Most calibration modes will force Pigeon to reinit the motion driver.
+		 */
+		LowLevelPigeonImu::PigeonState state;
+		/**
+		 * The currently applied calibration mode if state is in UserCalibration or if bCalIsBooting is true.
+		 * Otherwise it holds the last selected calibration mode (when calibrationError was updated).
+		 */
+		LowLevelPigeonImu::CalibrationMode currentMode;
+		/**
+		 * The error code for the last calibration mode.
+		 * Zero represents a successful cal (with solid green LEDs at end of cal)
+		 * and nonzero is a failed calibration (with solid red LEDs at end of cal).
+		 * Different calibration
+		 */
+		int calibrationError;
+		/**
+		 * After caller requests a calibration mode, pigeon will perform a boot-cal before
+		 * entering the requested mode.  During this period, this flag is set to true.
+		 */
+		bool bCalIsBooting;
+		/**
+		 * general string description of current status
+		 */
+		std::string description;
+		/**
+		 * Temperature in Celsius
+		 */
+		double tempC;
+		/**
+		 * Number of seconds Pigeon has been up (since boot).
+		 * This register is reset on power boot or processor reset.
+		 * Register is capped at 255 seconds with no wrap around.
+		 */
+		int upTimeSec;
+		/**
+		 * Number of times the Pigeon has automatically rebiased the gyro.
+		 * This counter overflows from 15 -> 0 with no cap.
+		 */
+		int noMotionBiasCount;
+		/**
+		 * Number of times the Pigeon has temperature compensated the various signals.
+		 * This counter overflows from 15 -> 0 with no cap.
+		 */
+		int tempCompensationCount;
+		/**
+		 * Same as GetLastError()
+		 */
+		int lastError;
+	};
+
+	static LowLevelPigeonImu * CreatePigeon(int deviceNumber, bool talon);
+
+	LowLevelPigeonImu(const LowLevelPigeonImu &) = delete;
+	~LowLevelPigeonImu();
+
+
+	ctre::phoenix::ErrorCode SetLastError(ctre::phoenix::ErrorCode error);
+
+	ctre::phoenix::ErrorCode SetYaw(double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode AddYaw(double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode SetYawToCompass(int timeoutMs);
+
+	ctre::phoenix::ErrorCode SetFusedHeading(double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode AddFusedHeading(double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode SetFusedHeadingToCompass(int timeoutMs);
+	ctre::phoenix::ErrorCode SetAccumZAngle(double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode ConfigTemperatureCompensationEnable(bool bTempCompEnable, int timeoutMs);
+	ctre::phoenix::ErrorCode SetCompassDeclination(double angleDegOffset, int timeoutMs);
+	ctre::phoenix::ErrorCode SetCompassAngle(double angleDeg, int timeoutMs);
+
+	ctre::phoenix::ErrorCode EnterCalibrationMode(CalibrationMode calMode, int timeoutMs);
+	ctre::phoenix::ErrorCode GetGeneralStatus(LowLevelPigeonImu::GeneralStatus & StatusToFill);
+	ctre::phoenix::ErrorCode GetGeneralStatus(int &state, int &currentMode, int &calibrationError, int &bCalIsBooting, double &tempC, int &upTimeSec, int &noMotionBiasCount, int &tempCompensationCount, int &lastError);
+	ctre::phoenix::ErrorCode GetLastError();
+	ctre::phoenix::ErrorCode Get6dQuaternion(double wxyz[4]);
+	ctre::phoenix::ErrorCode GetYawPitchRoll(double ypr[3]);
+	ctre::phoenix::ErrorCode GetAccumGyro(double xyz_deg[3]);
+	ctre::phoenix::ErrorCode GetAbsoluteCompassHeading(double &value);
+	ctre::phoenix::ErrorCode GetCompassHeading(double &value);
+	ctre::phoenix::ErrorCode GetCompassFieldStrength(double &value);
+	ctre::phoenix::ErrorCode GetTemp(double &value);
+	PigeonState GetState();
+	ctre::phoenix::ErrorCode GetState(int &state);
+	ctre::phoenix::ErrorCode GetUpTime(int &value);
+	ctre::phoenix::ErrorCode GetRawMagnetometer(short rm_xyz[3]);
+
+	ctre::phoenix::ErrorCode GetBiasedMagnetometer(short bm_xyz[3]);
+	ctre::phoenix::ErrorCode GetBiasedAccelerometer(short ba_xyz[3]);
+	ctre::phoenix::ErrorCode GetRawGyro(double xyz_dps[3]);
+	ctre::phoenix::ErrorCode GetAccelerometerAngles(double tiltAngles[3]);
+
+	ctre::phoenix::ErrorCode GetFusedHeading(FusionStatus & status, double &value);
+	ctre::phoenix::ErrorCode GetFusedHeading(int &bIsFusing, int &bIsValid,
+			double &value, int &lastError);
+	ctre::phoenix::ErrorCode GetFusedHeading(double &value);
+
+	ctre::phoenix::ErrorCode SetStatusFramePeriod(
+			ctre::phoenix::sensors::PigeonIMU_StatusFrame frame, int periodMs,
+			int timeoutMs);
+	ctre::phoenix::ErrorCode GetStatusFramePeriod(
+			ctre::phoenix::sensors::PigeonIMU_StatusFrame frame, int & periodMs,
+			int timeoutMs);
+	ctre::phoenix::ErrorCode SetControlFramePeriod(
+			ctre::phoenix::sensors::PigeonIMU_ControlFrame frame, int periodMs);
+	ctre::phoenix::ErrorCode GetFaults(
+			ctre::phoenix::sensors::PigeonIMU_Faults & toFill);
+	ctre::phoenix::ErrorCode GetStickyFaults(
+			ctre::phoenix::sensors::PigeonIMU_StickyFaults & toFill);
+	ctre::phoenix::ErrorCode ClearStickyFaults(int timeoutMs);
+
+	static std::string ToString(LowLevelPigeonImu::PigeonState state);
+	static std::string ToString(CalibrationMode cm);
+
+	const static int kMinFirmwareVersionMajor = 0;
+	const static int kMinFirmwareVersionMinor = 40;
+
+protected:
+	virtual void EnableFirmStatusFrame(bool enable);
+private:
+
+	LowLevelPigeonImu(int32_t baseArbId,
+			int32_t arbIdStartupFrame,
+			int32_t paramReqId,
+			int32_t paramRespId,
+			int32_t paramSetId,
+			int32_t arbIdFrameApiStatus,
+			const std::string & description);
+
+	/** firmware state reported over CAN */
+	enum MotionDriverState {
+		Init0 = 0,
+		WaitForPowerOff = 1,
+		ConfigAg = 2,
+		SelfTestAg = 3,
+		StartDMP = 4,
+		ConfigCompass_0 = 5,
+		ConfigCompass_1 = 6,
+		ConfigCompass_2 = 7,
+		ConfigCompass_3 = 8,
+		ConfigCompass_4 = 9,
+		ConfigCompass_5 = 10,
+		SelfTestCompass = 11,
+		WaitForGyroStable = 12,
+		AdditionalAccelAdjust = 13,
+		Idle = 14,
+		Calibration = 15,
+		LedInstrum = 16,
+		Error = 31,
+	};
+	/** sub command for the various Set param enums */
+	enum TareType {
+		SetValue = 0x00, AddOffset = 0x01, MatchCompass = 0x02, SetOffset = 0xFF,
+	};
+
+	/** Portion of the arbID for all status and control frames. */
+	ctre::phoenix::ErrorCode _lastError = ctre::phoenix::OKAY;
+	uint64_t _cache = 0;
+	uint32_t _len = 0;
+
+	void CheckFirmVers(int minMajor = kMinFirmwareVersionMajor, int minMinor = kMinFirmwareVersionMinor);
+	ctre::phoenix::ErrorCode ConfigSetWrapper(ctre::phoenix::ParamEnum paramEnum, TareType tareType, double angleDeg, int timeoutMs);
+	ctre::phoenix::ErrorCode ConfigSetWrapper(ctre::phoenix::ParamEnum paramEnum, double value, int timeoutMs);
+	ctre::phoenix::ErrorCode ReceiveCAN(int arbId);
+	ctre::phoenix::ErrorCode ReceiveCAN(int arbId, bool allowStale);
+	ctre::phoenix::ErrorCode GetTwoParam16(int arbId, short words[2]);
+	ctre::phoenix::ErrorCode GetThreeParam16(int arbId, short words[3]);
+	ctre::phoenix::ErrorCode GetThreeParam16(int arbId, double signals[3], double scalar);
+	int GetThreeFloatAngles(int arbId, double signals[3], double scalar);
+	ctre::phoenix::ErrorCode GetThreeBoundedAngles(int arbId, double boundedAngles[3]);
+	ctre::phoenix::ErrorCode GetFourParam16(int arbId, double params[4], double scalar);
+	ctre::phoenix::ErrorCode GetThreeParam20(int arbId, double param[3], double scalar);
+
+	LowLevelPigeonImu::PigeonState GetState(int errCode, const uint64_t & statusFrame);
+	double GetTemp(const uint64_t & statusFrame);
+};
+
diff --git a/libraries/driver/include/ctre/phoenix/LowLevel/ResetStats.h b/libraries/driver/include/ctre/phoenix/LowLevel/ResetStats.h
new file mode 100644
index 0000000..70912cc
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/LowLevel/ResetStats.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <stdint.h>
+
+struct ResetStats
+{
+	int32_t resetCount;
+	int32_t resetFlags;
+	int32_t firmVers;
+	bool hasReset;
+	ResetStats() {
+		resetCount = 0;
+		resetFlags = 0;
+		firmVers = 0;
+		hasReset = false;
+	}
+};
diff --git a/libraries/driver/include/ctre/phoenix/Motion/MotionProfileStatus.h b/libraries/driver/include/ctre/phoenix/Motion/MotionProfileStatus.h
new file mode 100644
index 0000000..55b1fba
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/Motion/MotionProfileStatus.h
@@ -0,0 +1,72 @@
+#pragma once
+
+#include "ctre/phoenix/Motion/SetValueMotionProfile.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motion {
+
+/**
+ * Motion Profile Status
+ * This is simply a data transer object.
+ */
+struct MotionProfileStatus {
+	/**
+	 * The available empty slots in the trajectory buffer.
+	 *
+	 * The robot API holds a "top buffer" of trajectory points, so your applicaion
+	 * can dump several points at once.  The API will then stream them into the Talon's
+	 * low-level buffer, allowing the Talon to act on them.
+	 */
+	int topBufferRem;
+	/**
+	 * The number of points in the top trajectory buffer.
+	 */
+	int topBufferCnt;
+	/**
+	 * The number of points in the low level Talon buffer.
+	 */
+	int btmBufferCnt;
+	/**
+	 * Set if isUnderrun ever gets set.
+	 * Only is cleared by clearMotionProfileHasUnderrun() to ensure
+	 * robot logic can react or instrument it.
+	 * @see clearMotionProfileHasUnderrun()
+	 */
+	bool hasUnderrun;
+	/**
+	 * This is set if Talon needs to shift a point from its buffer into
+	 * the active trajectory point however the buffer is empty. This gets cleared
+	 * automatically when is resolved.
+	 */
+	bool isUnderrun;
+	/**
+	 * True if the active trajectory point has not empty, false otherwise.
+	 * The members in activePoint are only valid if this signal is set.
+	 */
+	bool activePointValid;
+
+	bool isLast;
+
+	/** Selected slot for PID Loop 0 */
+	int profileSlotSelect0;
+
+	/** Selected slot for PID Loop 0 */
+	int profileSlotSelect1;
+
+	/**
+	 * The current output mode of the motion profile executer (disabled, enabled, or hold).
+	 * When changing the set() value in MP mode, it's important to check this signal to
+	 * confirm the change takes effect before interacting with the top buffer.
+	 */
+	ctre::phoenix::motion::SetValueMotionProfile outputEnable;
+
+	/** The applied duration of the active trajectory point */
+	int timeDurMs;
+};
+
+} // namespace motion
+} // namespace phoenix
+} // namespace ctre
+
diff --git a/libraries/driver/include/ctre/phoenix/Motion/SetValueMotionProfile.h b/libraries/driver/include/ctre/phoenix/Motion/SetValueMotionProfile.h
new file mode 100644
index 0000000..77d4ea2
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/Motion/SetValueMotionProfile.h
@@ -0,0 +1,13 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motion {
+
+enum SetValueMotionProfile {
+	Disable = 0, Enable = 1, Hold = 2,
+};
+
+} // namespace motion
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/Motion/TrajectoryPoint.h b/libraries/driver/include/ctre/phoenix/Motion/TrajectoryPoint.h
new file mode 100644
index 0000000..66a5e59
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/Motion/TrajectoryPoint.h
@@ -0,0 +1,79 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motion {
+
+/**
+ * Duration to apply to a particular trajectory pt.
+ * This time unit is ADDED to the existing base time set by
+ * ConfigMotionProfileTrajectoryPeriod().
+ */
+enum TrajectoryDuration {
+	TrajectoryDuration_0ms = 0,
+	TrajectoryDuration_5ms = 5,
+	TrajectoryDuration_10ms = 10,
+	TrajectoryDuration_20ms = 20,
+	TrajectoryDuration_30ms = 30,
+	TrajectoryDuration_40ms = 40,
+	TrajectoryDuration_50ms = 50,
+	TrajectoryDuration_100ms = 100,
+};
+
+/**
+ * Motion Profile Trajectory Point
+ * This is simply a data transfer object.
+ */
+struct TrajectoryPoint {
+	double position; //!< The position to servo to.
+	double velocity; //!< The velocity to feed-forward.
+	double headingDeg; //!< Not used.  Use auxiliaryPos instead. @see auxiliaryPos
+
+	double auxiliaryPos;  //!< The position for auxiliary PID to target.
+
+	/**
+	 * Which slot to get PIDF gains.
+	 * PID is used for position servo.
+	 * F is used as the Kv constant for velocity feed-forward.
+	 * Typically this is hard-coded
+	 * to a particular slot, but you are free to gain schedule if need be.
+	 * gain schedule if need be.
+	 * Choose from [0,3].
+	 */
+	uint32_t profileSlotSelect0;
+
+	/**
+	 * Which slot to get PIDF gains for auxiliary PID.
+	 * This only has impact during MotionProfileArc Control mode.
+	 * Choose from [0,1].
+	 */
+	uint32_t profileSlotSelect1;
+	/**
+	 * Set to true to signal Talon that this is the final point, so do not
+	 * attempt to pop another trajectory point from out of the Talon buffer.
+	 * Instead continue processing this way point.  Typically the velocity
+	 * member variable should be zero so that the motor doesn't spin indefinitely.
+	 */
+	bool isLastPoint;
+	/**
+	 * Set to true to signal Talon to zero the selected sensor.
+	 * When generating MPs, one simple method is to make the first target position zero,
+	 * and the final target position the target distance from the current position.
+	 * Then when you fire the MP, the current position gets set to zero.
+	 * If this is the intent, you can set zeroPos on the first trajectory point.
+	 *
+	 * Otherwise you can leave this false for all points, and offset the positions
+	 * of all trajectory points so they are correct.
+	 */
+	bool zeroPos;
+
+	/**
+	 * Duration to apply this trajectory pt.
+	 * This time unit is ADDED to the existing base time set by
+	 * ConfigMotionProfileTrajectoryPeriod().
+	 */
+	TrajectoryDuration timeDur;
+};
+} // namespace motion
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/ControlFrame.h b/libraries/driver/include/ctre/phoenix/MotorControl/ControlFrame.h
new file mode 100644
index 0000000..683805a
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/ControlFrame.h
@@ -0,0 +1,29 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+//{
+enum ControlFrame {
+	Control_3_General = 0x040080,
+	Control_4_Advanced = 0x0400C0,
+	Control_6_MotProfAddTrajPoint = 0x040140,
+};
+
+enum ControlFrameEnhanced {
+	Control_3_General_ = 0x040080,
+	Control_4_Advanced_ = 0x0400c0,
+	Control_5_FeedbackOutputOverride_ = 0x040100,
+	Control_6_MotProfAddTrajPoint_ = 0x040140,
+};
+class ControlFrameRoutines {
+	static ControlFrameEnhanced Promote(ControlFrame controlFrame) {
+		return (ControlFrameEnhanced) controlFrame;
+	}
+};
+
+}
+}
+}
+
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/ControlMode.h b/libraries/driver/include/ctre/phoenix/MotorControl/ControlMode.h
new file mode 100644
index 0000000..b30d132
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/ControlMode.h
@@ -0,0 +1,22 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+enum class ControlMode {
+	PercentOutput = 0,
+	Position = 1,
+	Velocity = 2,
+	Current = 3,
+	Follower = 5,
+	MotionProfile = 6,
+	MotionMagic = 7,
+	MotionProfileArc = 10,
+
+	Disabled = 15,
+};
+
+}
+}
+}
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/DemandType.h b/libraries/driver/include/ctre/phoenix/MotorControl/DemandType.h
new file mode 100644
index 0000000..71ef507
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/DemandType.h
@@ -0,0 +1,29 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+/**
+ * How to interpret a demand value.
+ */
+enum DemandType {
+	/**
+	 * Ignore the demand value and apply neutral/no-change.
+	 */
+	DemandType_Neutral = 0,
+	/**
+	 * When closed-looping, set the target of the aux PID loop to the demand value.
+	 *
+	 * When following, follow the processed output of the combined 
+	 * primary/aux PID output.  The demand value is ignored.
+	 */
+	DemandType_AuxPID = 1, //!< Target value of PID loop 1.  When f
+	/**
+	 * When closed-looping, add this arbitrarily to the closed-loop output.
+	 */
+	DemandType_ArbitraryFeedForward = 2, //!< Simply add to the output
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/Faults.h b/libraries/driver/include/ctre/phoenix/MotorControl/Faults.h
new file mode 100644
index 0000000..0b262c2
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/Faults.h
@@ -0,0 +1,95 @@
+#pragma once
+#include <sstream>
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+struct Faults {
+	bool UnderVoltage;
+	bool ForwardLimitSwitch;
+	bool ReverseLimitSwitch;
+	bool ForwardSoftLimit;
+	bool ReverseSoftLimit;
+	bool HardwareFailure;
+	bool ResetDuringEn;
+	bool SensorOverflow;
+	bool SensorOutOfPhase;
+	bool HardwareESDReset;
+	bool RemoteLossOfSignal;
+	//!< True iff any of the above flags are true.
+	bool HasAnyFault() const {
+		return 	UnderVoltage |
+				ForwardLimitSwitch |
+				ReverseLimitSwitch |
+				ForwardSoftLimit |
+				ReverseSoftLimit |
+				HardwareFailure |
+				ResetDuringEn |
+				SensorOverflow |
+				SensorOutOfPhase |
+				HardwareESDReset |
+				RemoteLossOfSignal;
+	}
+	int ToBitfield() const {
+		int retval = 0;
+		int mask = 1;
+		retval |= UnderVoltage ? mask : 0; mask <<= 1;
+		retval |= ForwardLimitSwitch ? mask : 0; mask <<= 1;
+		retval |= ReverseLimitSwitch ? mask : 0; mask <<= 1;
+		retval |= ForwardSoftLimit ? mask : 0; mask <<= 1;
+		retval |= ReverseSoftLimit ? mask : 0; mask <<= 1;
+		retval |= HardwareFailure ? mask : 0; mask <<= 1;
+		retval |= ResetDuringEn ? mask : 0; mask <<= 1;
+		retval |= SensorOverflow ? mask : 0; mask <<= 1;
+		retval |= SensorOutOfPhase ? mask : 0; mask <<= 1;
+		retval |= HardwareESDReset ? mask : 0; mask <<= 1;
+		retval |= RemoteLossOfSignal ? mask : 0; mask <<= 1;
+		return retval;
+	}
+	Faults(int bits) {
+		int mask = 1;
+		UnderVoltage = (bits & mask) ? true : false; mask <<= 1;
+		ForwardLimitSwitch = (bits & mask) ? true : false; mask <<= 1;
+		ReverseLimitSwitch = (bits & mask) ? true : false; mask <<= 1;
+		ForwardSoftLimit = (bits & mask) ? true : false; mask <<= 1;
+		ReverseSoftLimit = (bits & mask) ? true : false; mask <<= 1;
+		HardwareFailure = (bits & mask) ? true : false; mask <<= 1;
+		ResetDuringEn = (bits & mask) ? true : false; mask <<= 1;
+		SensorOverflow = (bits & mask) ? true : false; mask <<= 1;
+		SensorOutOfPhase = (bits & mask) ? true : false; mask <<= 1;
+		HardwareESDReset = (bits & mask) ? true : false; mask <<= 1;
+		RemoteLossOfSignal = (bits & mask) ? true : false; mask <<= 1;
+	}
+	Faults() {
+		UnderVoltage = false;
+		ForwardLimitSwitch = false;
+		ReverseLimitSwitch = false;
+		ForwardSoftLimit = false;
+		ReverseSoftLimit = false;
+		HardwareFailure =false;
+		ResetDuringEn = false;
+		SensorOverflow = false;
+		SensorOutOfPhase = false;
+		HardwareESDReset = false;
+		RemoteLossOfSignal = false;
+	}
+	std::string ToString() {
+		std::stringstream work;
+		work << " UnderVoltage:" << (UnderVoltage ? "1" : "0");
+		work << " ForwardLimitSwitch:" << (ForwardLimitSwitch ? "1" : "0");
+		work << " ReverseLimitSwitch:" << (ReverseLimitSwitch ? "1" : "0");
+		work << " ForwardSoftLimit:" << (ForwardSoftLimit ? "1" : "0");
+		work << " ReverseSoftLimit:" << (ReverseSoftLimit ? "1" : "0");
+		work << " HardwareFailure:" << (HardwareFailure ? "1" : "0");
+		work << " ResetDuringEn:" << (ResetDuringEn ? "1" : "0");
+		work << " SensorOverflow:" << (SensorOverflow ? "1" : "0");
+		work << " SensorOutOfPhase:" << (SensorOutOfPhase ? "1" : "0");
+		work << " HardwareESDReset:" << (HardwareESDReset ? "1" : "0");
+		work << " RemoteLossOfSignal:" << (RemoteLossOfSignal ? "1" : "0");
+		return work.str();
+	}
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/FeedbackDevice.h b/libraries/driver/include/ctre/phoenix/MotorControl/FeedbackDevice.h
new file mode 100644
index 0000000..4262196
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/FeedbackDevice.h
@@ -0,0 +1,46 @@
+#pragma once
+
+#include "ctre/phoenix/ErrorCode.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+/** Motor controller with gadgeteer connector. */
+enum FeedbackDevice {
+	None = -1,
+
+	QuadEncoder = 0,
+	//1
+	Analog = 2,
+	//3
+	Tachometer = 4,
+	PulseWidthEncodedPosition = 8,
+
+	SensorSum = 9,
+	SensorDifference = 10,
+	RemoteSensor0 = 11,
+	RemoteSensor1 = 12,
+	//13
+	//14
+	SoftwareEmulatedSensor  = 15,
+
+	CTRE_MagEncoder_Absolute = PulseWidthEncodedPosition,
+	CTRE_MagEncoder_Relative = QuadEncoder,
+};
+
+enum RemoteFeedbackDevice  {
+	RemoteFeedbackDevice_None = -1,
+
+	RemoteFeedbackDevice_SensorSum = 9,
+	RemoteFeedbackDevice_SensorDifference = 10,
+	RemoteFeedbackDevice_RemoteSensor0 = 11,
+	RemoteFeedbackDevice_RemoteSensor1 = 12,
+	//13
+	//14
+	RemoteFeedbackDevice_SoftwareEmulatedSensor = 15,
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/FollowerType.h b/libraries/driver/include/ctre/phoenix/MotorControl/FollowerType.h
new file mode 100644
index 0000000..221355f
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/FollowerType.h
@@ -0,0 +1,14 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+enum FollowerType {
+	FollowerType_PercentOutput = 0,
+	FollowerType_AuxOutput1,
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/LimitSwitchType.h b/libraries/driver/include/ctre/phoenix/MotorControl/LimitSwitchType.h
new file mode 100644
index 0000000..8bd2722
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/LimitSwitchType.h
@@ -0,0 +1,35 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+enum LimitSwitchSource {
+	LimitSwitchSource_FeedbackConnector = 0, /* default */
+	LimitSwitchSource_RemoteTalonSRX = 1,
+	LimitSwitchSource_RemoteCANifier = 2,
+	LimitSwitchSource_Deactivated = 3,
+};
+
+enum RemoteLimitSwitchSource {
+	RemoteLimitSwitchSource_RemoteTalonSRX = 1,
+	RemoteLimitSwitchSource_RemoteCANifier = 2,
+	RemoteLimitSwitchSource_Deactivated = 3,
+};
+
+enum LimitSwitchNormal {
+	LimitSwitchNormal_NormallyOpen = 0,
+	LimitSwitchNormal_NormallyClosed = 1,
+	LimitSwitchNormal_Disabled = 2
+};
+
+class LimitSwitchRoutines {
+public:
+	static LimitSwitchSource Promote(
+			RemoteLimitSwitchSource limitSwitchSource) {
+		return (LimitSwitchSource) limitSwitchSource;
+	}
+};
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/NeutralMode.h b/libraries/driver/include/ctre/phoenix/MotorControl/NeutralMode.h
new file mode 100644
index 0000000..7f40b0c
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/NeutralMode.h
@@ -0,0 +1,18 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+enum NeutralMode {
+	/** Use the NeutralMode that is set by the jumper wire on the CAN device */
+	EEPROMSetting = 0,
+	/** Stop the motor's rotation by applying a force. */
+    Coast = 1,
+    /** Stop the motor's rotation by applying a force. */
+    Brake = 2,
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/RemoteSensorSource.h b/libraries/driver/include/ctre/phoenix/MotorControl/RemoteSensorSource.h
new file mode 100644
index 0000000..9d2ff77
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/RemoteSensorSource.h
@@ -0,0 +1,25 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+enum class RemoteSensorSource {
+	RemoteSensorSource_Off,
+	RemoteSensorSource_TalonSRX_SelectedSensor,
+	RemoteSensorSource_Pigeon_Yaw,
+	RemoteSensorSource_Pigeon_Pitch,
+	RemoteSensorSource_Pigeon_Roll,
+	RemoteSensorSource_CANifier_Quadrature,
+	RemoteSensorSource_CANifier_PWMInput0,
+	RemoteSensorSource_CANifier_PWMInput1,
+	RemoteSensorSource_CANifier_PWMInput2,
+	RemoteSensorSource_CANifier_PWMInput3,
+	RemoteSensorSource_GadgeteerPigeon_Yaw,
+	RemoteSensorSource_GadgeteerPigeon_Pitch,
+	RemoteSensorSource_GadgeteerPigeon_Roll,
+};
+
+}
+}
+}
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/SensorTerm.h b/libraries/driver/include/ctre/phoenix/MotorControl/SensorTerm.h
new file mode 100644
index 0000000..7e9a744
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/SensorTerm.h
@@ -0,0 +1,16 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+enum class SensorTerm {
+	SensorTerm_Sum0,
+	SensorTerm_Sum1,
+	SensorTerm_Diff0,
+	SensorTerm_Diff1,
+};
+
+}
+}
+}
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/StatusFrame.h b/libraries/driver/include/ctre/phoenix/MotorControl/StatusFrame.h
new file mode 100644
index 0000000..f9a1890
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/StatusFrame.h
@@ -0,0 +1,64 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+enum StatusFrameEnhanced {
+	Status_1_General = 0x1400,
+	Status_2_Feedback0 = 0x1440,
+	Status_4_AinTempVbat = 0x14C0,
+	Status_6_Misc = 0x1540,
+	Status_7_CommStatus = 0x1580,
+	Status_9_MotProfBuffer = 0x1600,
+	/**
+	 * Old name for Status 10 Frame.
+	 * Use Status_10_Targets instead.
+	 */
+	Status_10_MotionMagic = 0x1640,
+	/**
+	 * Correct name for Status 10 Frame.
+	 * Functionally equivalent to Status_10_MotionMagic
+	 */
+	Status_10_Targets = 0x1640,
+	Status_12_Feedback1 = 0x16C0,
+	Status_13_Base_PIDF0 = 0x1700,
+	Status_14_Turn_PIDF1 = 0x1740,
+	Status_15_FirmareApiStatus = 0x1780,
+
+	Status_3_Quadrature = 0x1480,
+	Status_8_PulseWidth = 0x15C0,
+	Status_11_UartGadgeteer = 0x1680,
+};
+
+enum StatusFrame {
+	Status_1_General_ = 0x1400,
+	Status_2_Feedback0_ = 0x1440,
+	Status_4_AinTempVbat_ = 0x14C0,
+	Status_6_Misc_ = 0x1540,
+	Status_7_CommStatus_ = 0x1580,
+	Status_9_MotProfBuffer_ = 0x1600,
+	/**
+	 * Old name for Status 10 Frame.
+	 * Use Status_10_Targets instead.
+	 */
+	Status_10_MotionMagic_ = 0x1640,
+	/**
+	 * Correct name for Status 10 Frame.
+	 * Functionally equivalent to Status_10_MotionMagic
+	 */
+	Status_10_Targets_ = 0x1640,
+	Status_12_Feedback1_ = 0x16C0,
+	Status_13_Base_PIDF0_ = 0x1700,
+	Status_14_Turn_PIDF1_ = 0x1740,
+	Status_15_FirmareApiStatus_ = 0x1780,
+};
+class StatusFrameRoutines {
+public:
+	StatusFrameEnhanced Promote(StatusFrame statusFrame) {
+		return (StatusFrameEnhanced) statusFrame;
+	}
+};
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/StickyFaults.h b/libraries/driver/include/ctre/phoenix/MotorControl/StickyFaults.h
new file mode 100644
index 0000000..f5d7992
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/StickyFaults.h
@@ -0,0 +1,90 @@
+#pragma once
+#include <sstream>
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+struct StickyFaults {
+	bool UnderVoltage;
+	bool ForwardLimitSwitch;
+	bool ReverseLimitSwitch;
+	bool ForwardSoftLimit;
+	bool ReverseSoftLimit;
+	bool ResetDuringEn;
+	bool SensorOverflow;
+	bool SensorOutOfPhase;
+	bool HardwareESDReset;
+	bool RemoteLossOfSignal;
+
+	//!< True iff any of the above flags are true.
+	bool HasAnyFault() const {
+		return 	UnderVoltage |
+				ForwardLimitSwitch |
+				ReverseLimitSwitch |
+				ForwardSoftLimit |
+				ReverseSoftLimit |
+				ResetDuringEn |
+				SensorOverflow |
+				SensorOutOfPhase |
+				HardwareESDReset |
+				RemoteLossOfSignal;
+	}
+	int ToBitfield() const {
+		int retval = 0;
+		int mask = 1;
+		retval |= UnderVoltage ? mask : 0; mask <<= 1;
+		retval |= ForwardLimitSwitch ? mask : 0; mask <<= 1;
+		retval |= ReverseLimitSwitch ? mask : 0; mask <<= 1;
+		retval |= ForwardSoftLimit ? mask : 0; mask <<= 1;
+		retval |= ReverseSoftLimit ? mask : 0; mask <<= 1;
+		retval |= ResetDuringEn ? mask : 0; mask <<= 1;
+		retval |= SensorOverflow ? mask : 0; mask <<= 1;
+		retval |= SensorOutOfPhase ? mask : 0; mask <<= 1;
+		retval |= HardwareESDReset ? mask : 0; mask <<= 1;
+		retval |= RemoteLossOfSignal ? mask : 0; mask <<= 1;
+		return retval;
+	}
+	StickyFaults(int bits) {
+		int mask = 1;
+		UnderVoltage = (bits & mask) ? true : false; mask <<= 1;
+		ForwardLimitSwitch = (bits & mask) ? true : false; mask <<= 1;
+		ReverseLimitSwitch = (bits & mask) ? true : false; mask <<= 1;
+		ForwardSoftLimit = (bits & mask) ? true : false; mask <<= 1;
+		ReverseSoftLimit = (bits & mask) ? true : false; mask <<= 1;
+		ResetDuringEn = (bits & mask) ? true : false; mask <<= 1;
+		SensorOverflow = (bits & mask) ? true : false; mask <<= 1;
+		SensorOutOfPhase = (bits & mask) ? true : false; mask <<= 1;
+		HardwareESDReset = (bits & mask) ? true : false; mask <<= 1;
+		RemoteLossOfSignal = (bits & mask) ? true : false; mask <<= 1;
+	}
+	StickyFaults() {
+		UnderVoltage = false;
+		ForwardLimitSwitch = false;
+		ReverseLimitSwitch = false;
+		ForwardSoftLimit = false;
+		ReverseSoftLimit = false;
+		ResetDuringEn = false;
+		SensorOverflow = false;
+		SensorOutOfPhase = false;
+		HardwareESDReset = false;
+		RemoteLossOfSignal = false;
+	}
+	std::string ToString() {
+		std::stringstream work;
+		work << " UnderVoltage:" << (UnderVoltage ? "1" : "0");
+		work << " ForwardLimitSwitch:" << (ForwardLimitSwitch ? "1" : "0");
+		work << " ReverseLimitSwitch:" << (ReverseLimitSwitch ? "1" : "0");
+		work << " ForwardSoftLimit:" << (ForwardSoftLimit ? "1" : "0");
+		work << " ReverseSoftLimit:" << (ReverseSoftLimit ? "1" : "0");
+		work << " ResetDuringEn:" << (ResetDuringEn ? "1" : "0");
+		work << " SensorOverflow:" << (SensorOverflow ? "1" : "0");
+		work << " SensorOutOfPhase:" << (SensorOutOfPhase ? "1" : "0");
+		work << " HardwareESDReset:" << (HardwareESDReset ? "1" : "0");
+		work << " RemoteLossOfSignal:" << (RemoteLossOfSignal ? "1" : "0");
+		return work.str();
+	}
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/MotorControl/VelocityMeasPeriod.h b/libraries/driver/include/ctre/phoenix/MotorControl/VelocityMeasPeriod.h
new file mode 100644
index 0000000..38cee64
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/MotorControl/VelocityMeasPeriod.h
@@ -0,0 +1,20 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+enum VelocityMeasPeriod {
+	Period_1Ms = 1,
+	Period_2Ms = 2,
+	Period_5Ms = 5,
+	Period_10Ms = 10,
+	Period_20Ms = 20,
+	Period_25Ms = 25,
+	Period_50Ms = 50,
+	Period_100Ms = 100,
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_ControlFrame.h b/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_ControlFrame.h
new file mode 100644
index 0000000..79d79cc
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_ControlFrame.h
@@ -0,0 +1,14 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace sensors {
+
+/** Enumerated type for status frame types. */
+enum PigeonIMU_ControlFrame {
+	PigeonIMU_CondStatus_Control_1 = 0x00042800,
+};
+
+} // namespace sensors
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_Faults.h b/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_Faults.h
new file mode 100644
index 0000000..e9d37b0
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_Faults.h
@@ -0,0 +1,26 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace sensors {
+
+struct PigeonIMU_Faults {
+	//!< True iff any of the above flags are true.
+	bool HasAnyFault() const {
+		return false;
+	}
+	int ToBitfield() const {
+		int retval = 0;
+		return retval;
+	}
+	PigeonIMU_Faults(int bits) {
+		(void)bits;
+	}
+	PigeonIMU_Faults() {
+	}
+};
+
+} // sensors
+} // phoenix
+} // ctre
+
diff --git a/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_StatusFrame.h b/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_StatusFrame.h
new file mode 100644
index 0000000..c381511
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_StatusFrame.h
@@ -0,0 +1,24 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace sensors {
+
+/** Enumerated type for status frame types. */
+enum PigeonIMU_StatusFrame {
+	PigeonIMU_CondStatus_1_General = 0x042000,
+	PigeonIMU_CondStatus_9_SixDeg_YPR = 0x042200,
+	PigeonIMU_CondStatus_6_SensorFusion = 0x042140,
+	PigeonIMU_CondStatus_11_GyroAccum = 0x042280,
+	PigeonIMU_CondStatus_2_GeneralCompass = 0x042040,
+	PigeonIMU_CondStatus_3_GeneralAccel = 0x042080,
+	PigeonIMU_CondStatus_10_SixDeg_Quat = 0x042240,
+	PigeonIMU_RawStatus_4_Mag = 0x041CC0,
+	PigeonIMU_BiasedStatus_2_Gyro = 0x041C40,
+	PigeonIMU_BiasedStatus_4_Mag = 0x041CC0,
+	PigeonIMU_BiasedStatus_6_Accel = 0x41D40,
+};
+
+} // namespace sensors
+} // namespace phoenix
+} // namespace ctre
diff --git a/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_StickyFaults.h b/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_StickyFaults.h
new file mode 100644
index 0000000..5142a72
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/Sensors/PigeonIMU_StickyFaults.h
@@ -0,0 +1,25 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace sensors {
+
+struct PigeonIMU_StickyFaults {
+	//!< True iff any of the above flags are true.
+	bool HasAnyFault() const {
+		return false;
+	}
+	int ToBitfield() const {
+		int retval = 0;
+		return retval;
+	}
+	PigeonIMU_StickyFaults(int bits) {
+		(void)bits;
+	}
+	PigeonIMU_StickyFaults() {
+	}
+};
+
+} // sensors
+} // phoenix
+} // ctre
diff --git a/libraries/driver/include/ctre/phoenix/core/GadgeteerUartClient.h b/libraries/driver/include/ctre/phoenix/core/GadgeteerUartClient.h
new file mode 100644
index 0000000..2308b61
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/core/GadgeteerUartClient.h
@@ -0,0 +1,69 @@
+#ifndef GadgeteerUartClient_h_
+#define GadgeteerUartClient_h_
+#include <stdint.h>
+#include <string>
+
+class IGadgeteerUartClient
+{
+public:
+	IGadgeteerUartClient() {}
+    virtual ~IGadgeteerUartClient() {}
+
+	enum GadgeteerProxyType
+	{
+		General = 0,
+		Pigeon = 1,
+		PC_HERO = 2,
+	};
+	enum GadgeteerConnection
+	{
+		NotConnected = 0,
+		Connecting = 1,
+		Connected = 2,
+	};
+	struct GadgeteerUartStatus {
+		GadgeteerProxyType type;
+		GadgeteerConnection conn;
+		uint32_t bitrate;
+		uint32_t resetCount;
+	};
+
+    virtual int GetGadgeteerStatus(GadgeteerUartStatus & status) = 0;
+
+    static std::string ToString(IGadgeteerUartClient::GadgeteerProxyType gpt) {
+    	std::string retval;
+		switch(gpt) {
+			case General: retval = "General"; break;
+			case Pigeon : retval = "Pigeon"; break;
+			case PC_HERO: retval = "PC_HERO"; break;
+		}
+		return retval;
+    }
+    static std::string ToString(IGadgeteerUartClient::GadgeteerConnection gc) {
+    	std::string retval;
+		switch(gc) {
+			case NotConnected: retval = "NotConnected"; break;
+			case Connecting : retval = "Connecting"; break;
+			case Connected: retval = "Connected"; break;
+		}
+		return retval;
+    }
+
+protected:
+	enum GadgeteerState
+	{
+		GadState_WaitChirp1 = 0,
+		GadState_WaitBLInfo = 1,
+		GadState_WaitBitrateResp = 2,
+		GadState_WaitSwitchDelay = 3,
+		GadState_WaitChirp2 = 4,
+		GadState_Connected_Idle = 5,
+		GadState_Connected_ReqChirp = 6,
+		GadState_Connected_RespChirp = 7,
+		GadState_Connected_ReqCanBus = 8,
+		GadState_Connected_RespCanBus = 9,
+		GadState_Connected_RespIsoThenChirp = 10,
+		GadState_Connected_RespIsoThenCanBus = 11,
+	};
+};
+#endif
diff --git a/libraries/driver/include/ctre/phoenix/defs/signalTypes.h b/libraries/driver/include/ctre/phoenix/defs/signalTypes.h
new file mode 100644
index 0000000..4a90319
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/defs/signalTypes.h
@@ -0,0 +1,87 @@
+#ifndef signalTypes__h_
+#define signalTypes__h_
+
+enum{
+	modeDutyCycleControl = 0,	//!< Demand is signed 16bit 0.16fixedPt. 7FFF is fullfor.  8000 is fullRev.
+	modePosControl = 1,			//!< Demand 24bit position.
+	modeSpeedControl = 2,		//!< Demand is 24 bit speed.
+	modeCurrentControl = 3,		//!< Demand is 24 bit current.
+	modeSlaveFollower = 5,		//!< Demand is the can node to follow.
+	modeMotionProfile = 6,		//!< Demand is unused,could be used in future. Control6 has everything we want.
+	modeMotionMagic = 7,		//!< Reserved
+	motionMagicArc = 8,
+	//9
+	motionProfileArc = 10,
+	//11
+	//12
+	//13
+#ifdef SUPPORT_ONE_SHOT_CONTROL_MODE
+	modeOneShot = 14,
+#endif
+	modeNoDrive = 15,
+};
+
+typedef enum _feedbackDevice_t{
+	kQuadEncoder = 0,
+	//1
+	kAnalog = 2,
+	//3
+	Tachometer= 4,
+	kPulseWidthEncodedPosition = 8,
+	
+	kSensorSum = 9,
+	kSensorDifference = 10,
+	kRemoteSensor0 = 11,
+	kRemoteSensor1 = 12,
+	//13
+	//14
+	kSoftwarEmulatedSensor=15,
+}feedbackDevice_t;
+
+typedef enum _MotProf_OutputType_t {
+	MotProf_OutputType_Disabled = 0,
+	MotProf_OutputType_Enabled,
+	MotProf_OutputType_Hold,
+	MotProf_OutputType_Invalid,
+}MotProf_OutputType_t;
+
+/**
+ * Saved to limitSwitchForward_Source/limitSwitchReverse_Source
+ */
+typedef enum _LimitSwitchSource_t {
+	LSS_Local=0,
+	LSS_RemoteTalon=1,
+	LSS_RemoteCanif=2,
+	LSS_Deactivated=3,
+}LimitSwitchSource_t;
+
+/**
+ * Saved to limitSwitchForward_normClosedAndDis / limitSwitchReverse_normClosedAndDis
+ */
+typedef enum _LimitSwitchNormClosedAndDis_t {
+	LSNCD_NormallyOpen=0,
+	LSNCD_NormallyClosed=1,
+	LSNCD_NormallyDisabled=2,
+}LimitSwitchNormClosedAndDis_t;
+
+typedef enum _RemoteSensorSource_t {
+	RSS_Off,
+	RSS_RemoteTalonSelSensor,
+	RSS_RemotePigeon_Yaw,
+	RSS_RemotePigeon_Pitch,
+	RSS_RemotePigeon_Roll,
+	RSS_RemoteCanif_Quad,
+	RSS_RemoteCanif_PWM0,
+	RSS_RemoteCanif_PWM1,
+	RSS_RemoteCanif_PWM2,
+	RSS_RemoteCanif_PWM3,
+} RemoteSensorSource_t;
+
+typedef enum _SensorTermOrdinal_t {
+	SensorTermOrdinal_Sum0 = 0,
+	SensorTermOrdinal_Sum1 = 0,
+	SensorTermOrdinal_Diff0 = 0,
+	SensorTermOrdinal_Diff1 = 0,
+}SensorTermOrdinal_t;
+
+#endif // signalTypes__h_
diff --git a/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_CANifierJNI.h b/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_CANifierJNI.h
new file mode 100644
index 0000000..deeac53
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_CANifierJNI.h
@@ -0,0 +1,245 @@
+/* DO NOT EDIT THIS FILE - it is machine generated */
+#include <jni.h>
+/* Header for class com_ctre_phoenix_CANifierJNI */
+
+#ifndef _Included_com_ctre_phoenix_CANifierJNI
+#define _Included_com_ctre_phoenix_CANifierJNI
+#ifdef __cplusplus
+extern "C" {
+#endif
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_new_CANifier
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1new_1CANifier
+  (JNIEnv *, jclass, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_SetLEDOutput
+ * Signature: (JII)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1SetLEDOutput
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_SetGeneralOutputs
+ * Signature: (JII)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1SetGeneralOutputs
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_SetGeneralOutput
+ * Signature: (JIZZ)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1SetGeneralOutput
+  (JNIEnv *, jclass, jlong, jint, jboolean, jboolean);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_SetPWMOutput
+ * Signature: (JII)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1SetPWMOutput
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_EnablePWMOutput
+ * Signature: (JIZ)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1EnablePWMOutput
+  (JNIEnv *, jclass, jlong, jint, jboolean);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetGeneralInputs
+ * Signature: (J[Z)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetGeneralInputs
+  (JNIEnv *, jclass, jlong, jbooleanArray);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetGeneralInput
+ * Signature: (JI)Z
+ */
+JNIEXPORT jboolean JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetGeneralInput
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetPWMInput
+ * Signature: (JI[D)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetPWMInput
+  (JNIEnv *, jclass, jlong, jint, jdoubleArray);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetLastError
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetLastError
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetBatteryVoltage
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetBatteryVoltage
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetQuadraturePosition
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetQuadraturePosition
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_SetQuadraturePosition
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1SetQuadraturePosition
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetQuadratureVelocity
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetQuadratureVelocity
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_ConfigVelocityMeasurementPeriod
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1ConfigVelocityMeasurementPeriod
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_ConfigVelocityMeasurementWindow
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1ConfigVelocityMeasurementWindow
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_ConfigSetCustomParam
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1ConfigSetCustomParam
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_ConfigGetCustomParam
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1ConfigGetCustomParam
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_ConfigSetParameter
+ * Signature: (JIDIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1ConfigSetParameter
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_ConfigGetParameter
+ * Signature: (JIII)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1ConfigGetParameter
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_SetStatusFramePeriod
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1SetStatusFramePeriod
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetStatusFramePeriod
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetStatusFramePeriod
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_SetControlFramePeriod
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1SetControlFramePeriod
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetFirmwareVersion
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetFirmwareVersion
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_HasResetOccurred
+ * Signature: (J)Z
+ */
+JNIEXPORT jboolean JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1HasResetOccurred
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetFaults
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetFaults
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetStickyFaults
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetStickyFaults
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_ClearStickyFaults
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1ClearStickyFaults
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CANifierJNI
+ * Method:    JNI_GetBusVoltage
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_CANifierJNI_JNI_1GetBusVoltage
+  (JNIEnv *, jclass, jlong);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_CTRLoggerJNI.h b/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_CTRLoggerJNI.h
new file mode 100644
index 0000000..b4b7ca2
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_CTRLoggerJNI.h
@@ -0,0 +1,53 @@
+/* DO NOT EDIT THIS FILE - it is machine generated */
+#include <jni.h>
+/* Header for class com_ctre_phoenix_CTRLoggerJNI */
+
+#ifndef _Included_com_ctre_phoenix_CTRLoggerJNI
+#define _Included_com_ctre_phoenix_CTRLoggerJNI
+#ifdef __cplusplus
+extern "C" {
+#endif
+/*
+ * Class:     com_ctre_phoenix_CTRLoggerJNI
+ * Method:    JNI_Logger_Close
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_CTRLoggerJNI_JNI_1Logger_1Close
+  (JNIEnv *, jclass);
+
+/*
+ * Class:     com_ctre_phoenix_CTRLoggerJNI
+ * Method:    JNI_Logger_Log
+ * Signature: (ILjava/lang/String;Ljava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_CTRLoggerJNI_JNI_1Logger_1Log
+  (JNIEnv *, jclass, jint, jstring, jstring);
+
+/*
+ * Class:     com_ctre_phoenix_CTRLoggerJNI
+ * Method:    JNI_Logger_Open
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_CTRLoggerJNI_JNI_1Logger_1Open
+  (JNIEnv *, jclass, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CTRLoggerJNI
+ * Method:    JNI_Logger_GetShort
+ * Signature: (I)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL Java_com_ctre_phoenix_CTRLoggerJNI_JNI_1Logger_1GetShort
+  (JNIEnv *, jclass, jint);
+
+/*
+ * Class:     com_ctre_phoenix_CTRLoggerJNI
+ * Method:    JNI_Logger_GetLong
+ * Signature: (I)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL Java_com_ctre_phoenix_CTRLoggerJNI_JNI_1Logger_1GetLong
+  (JNIEnv *, jclass, jint);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_MotorControl_CAN_MotControllerJNI.h b/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_MotorControl_CAN_MotControllerJNI.h
new file mode 100644
index 0000000..a282095
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_MotorControl_CAN_MotControllerJNI.h
@@ -0,0 +1,844 @@
+/* DO NOT EDIT THIS FILE - it is machine generated */
+#include <jni.h>
+/* Header for class com_ctre_phoenix_motorcontrol_can_MotControllerJNI */
+
+#ifndef _Included_com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+#define _Included_com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+#ifdef __cplusplus
+extern "C" {
+#endif
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    Create
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_Create
+  (JNIEnv *, jclass, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetDeviceNumber
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetDeviceNumber
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetDemand
+ * Signature: (JIII)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetDemand
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    Set_4
+ * Signature: (JIDDI)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_Set_14
+  (JNIEnv *, jclass, jlong, jint, jdouble, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetNeutralMode
+ * Signature: (JI)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetNeutralMode
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetSensorPhase
+ * Signature: (JZ)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetSensorPhase
+  (JNIEnv *, jclass, jlong, jboolean);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetInverted
+ * Signature: (JZ)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetInverted
+  (JNIEnv *, jclass, jlong, jboolean);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigOpenLoopRamp
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigOpenLoopRamp
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigClosedLoopRamp
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigClosedLoopRamp
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigPeakOutputForward
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigPeakOutputForward
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigPeakOutputReverse
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigPeakOutputReverse
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigNominalOutputForward
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigNominalOutputForward
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigNominalOutputReverse
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigNominalOutputReverse
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigNeutralDeadband
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigNeutralDeadband
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigVoltageCompSaturation
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigVoltageCompSaturation
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigVoltageMeasurementFilter
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigVoltageMeasurementFilter
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    EnableVoltageCompensation
+ * Signature: (JZ)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_EnableVoltageCompensation
+  (JNIEnv *, jclass, jlong, jboolean);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetBusVoltage
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetBusVoltage
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetMotorOutputPercent
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetMotorOutputPercent
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetOutputCurrent
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetOutputCurrent
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetTemperature
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetTemperature
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigRemoteFeedbackFilter
+ * Signature: (JIIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigRemoteFeedbackFilter
+  (JNIEnv *, jclass, jlong, jint, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigSelectedFeedbackSensor
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigSelectedFeedbackSensor
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigSelectedFeedbackCoefficient
+ * Signature: (JDII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigSelectedFeedbackCoefficient
+  (JNIEnv *, jclass, jlong, jdouble, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetSelectedSensorPosition
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetSelectedSensorPosition
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetSelectedSensorVelocity
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetSelectedSensorVelocity
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetSelectedSensorPosition
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetSelectedSensorPosition
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetControlFramePeriod
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetControlFramePeriod
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetStatusFramePeriod
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetStatusFramePeriod
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetStatusFramePeriod
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetStatusFramePeriod
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigVelocityMeasurementPeriod
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigVelocityMeasurementPeriod
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigVelocityMeasurementWindow
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigVelocityMeasurementWindow
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigForwardLimitSwitchSource
+ * Signature: (JIIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigForwardLimitSwitchSource
+  (JNIEnv *, jclass, jlong, jint, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigReverseLimitSwitchSource
+ * Signature: (JIIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigReverseLimitSwitchSource
+  (JNIEnv *, jclass, jlong, jint, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    EnableLimitSwitches
+ * Signature: (JZ)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_OverrideLimitSwitchesEnable
+  (JNIEnv *, jclass, jlong, jboolean);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigForwardSoftLimit
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigForwardSoftLimitThreshold
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigReverseSoftLimit
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigReverseSoftLimitThreshold
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigForwardSoftLimitEnable
+  (JNIEnv *, jclass, jlong, jboolean, jint);
+
+
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigReverseSoftLimitEnable
+  (JNIEnv *, jclass, jlong, jboolean, jint);
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    EnableSoftLimits
+ * Signature: (JZ)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_OverrideSoftLimitsEnable
+  (JNIEnv *, jclass, jlong, jboolean);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    Config_kP
+ * Signature: (JIDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_Config_1kP
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    Config_kI
+ * Signature: (JIDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_Config_1kI
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    Config_kD
+ * Signature: (JIDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_Config_1kD
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    Config_kF
+ * Signature: (JIDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_Config_1kF
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    Config_IntegralZone
+ * Signature: (JIDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_Config_1IntegralZone
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigAllowableClosedloopError
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigAllowableClosedloopError
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigMaxIntegralAccumulator
+ * Signature: (JIDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigMaxIntegralAccumulator
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigClosedLoopPeakOutput
+ * Signature: (JIDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigClosedLoopPeakOutput
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigClosedLoopPeriod
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigClosedLoopPeriod
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetIntegralAccumulator
+ * Signature: (JDII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetIntegralAccumulator
+  (JNIEnv *, jclass, jlong, jdouble, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetClosedLoopError
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetClosedLoopError
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetIntegralAccumulator
+ * Signature: (JI)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetIntegralAccumulator
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetErrorDerivative
+ * Signature: (JI)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetErrorDerivative
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SelectProfileSlot
+ * Signature: (JII)V
+ */
+JNIEXPORT void JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SelectProfileSlot
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetActiveTrajectoryPosition
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetActiveTrajectoryPosition
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetActiveTrajectoryVelocity
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetActiveTrajectoryVelocity
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetActiveTrajectoryHeading
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetActiveTrajectoryHeading
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigMotionCruiseVelocity
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigMotionCruiseVelocity
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigMotionAcceleration
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigMotionAcceleration
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ClearMotionProfileTrajectories
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ClearMotionProfileTrajectories
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetMotionProfileTopLevelBufferCount
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetMotionProfileTopLevelBufferCount
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    PushMotionProfileTrajectory
+ * Signature: (JDDDIZZ)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_PushMotionProfileTrajectory
+  (JNIEnv *, jclass, jlong, jdouble, jdouble, jdouble, jint, jboolean, jboolean);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    PushMotionProfileTrajectory2
+ * Signature: (JDDDIZZ)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_PushMotionProfileTrajectory2
+  (JNIEnv *, jclass, jlong, jdouble, jdouble, jdouble, jint, jint, jboolean, jboolean, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    IsMotionProfileTopLevelBufferFull
+ * Signature: (J)Z
+ */
+JNIEXPORT jboolean JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_IsMotionProfileTopLevelBufferFull
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ProcessMotionProfileBuffer
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ProcessMotionProfileBuffer
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetMotionProfileStatus
+ * Signature: (J[I)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetMotionProfileStatus
+  (JNIEnv *, jclass, jlong, jintArray);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetMotionProfileStatus2
+ * Signature: (J[I)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetMotionProfileStatus2
+  (JNIEnv *, jclass, jlong, jintArray);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ClearMotionProfileHasUnderrun
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ClearMotionProfileHasUnderrun
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ChangeMotionControlFramePeriod
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ChangeMotionControlFramePeriod
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigMotionProfileTrajectoryPeriod
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigMotionProfileTrajectoryPeriod
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetLastError
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetLastError
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetFirmwareVersion
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetFirmwareVersion
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    HasResetOccurred
+ * Signature: (J)Z
+ */
+JNIEXPORT jboolean JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_HasResetOccurred
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigSetCustomParam
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigSetCustomParam
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigGetCustomParam
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigGetCustomParam
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigSetParameter
+ * Signature: (JIDIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigSetParameter
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigGetParameter
+ * Signature: (JIII)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigGetParameter
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigPeakCurrentLimit
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigPeakCurrentLimit
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigPeakCurrentDuration
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigPeakCurrentDuration
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ConfigContinuousCurrentLimit
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ConfigContinuousCurrentLimit
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    EnableCurrentLimit
+ * Signature: (JZ)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_EnableCurrentLimit
+  (JNIEnv *, jclass, jlong, jboolean);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetAnalogIn
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetAnalogIn
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetAnalogPosition
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetAnalogPosition
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetAnalogInRaw
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetAnalogInRaw
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetAnalogInVel
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetAnalogInVel
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetQuadraturePosition
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetQuadraturePosition
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetQuadraturePosition
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetQuadraturePosition
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetQuadratureVelocity
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetQuadratureVelocity
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetPulseWidthPosition
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetPulseWidthPosition
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetPulseWidthPosition
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetPulseWidthPosition
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetPulseWidthVelocity
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetPulseWidthVelocity
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetPulseWidthRiseToFallUs
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetPulseWidthRiseToFallUs
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetPulseWidthRiseToRiseUs
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetPulseWidthRiseToRiseUs
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetPinStateQuadA
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetPinStateQuadA
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetPinStateQuadB
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetPinStateQuadB
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetPinStateQuadIdx
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetPinStateQuadIdx
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    IsFwdLimitSwitchClosed
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_IsFwdLimitSwitchClosed
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    IsRevLimitSwitchClosed
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_IsRevLimitSwitchClosed
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetFaults
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetFaults
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetStickyFaults
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetStickyFaults
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    ClearStickyFaults
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_ClearStickyFaults
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SelectDemandType
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SelectDemandType
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    SetMPEOutput
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_SetMPEOutput
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    EnableHeadingHold
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_EnableHeadingHold
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_motorcontrol_can_MotControllerJNI
+ * Method:    GetClosedLoopTarget
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_motorcontrol_can_MotControllerJNI_GetClosedLoopTarget
+  (JNIEnv *, jclass, jlong, jint);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_Sensors_PigeonImuJNI.h b/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_Sensors_PigeonImuJNI.h
new file mode 100644
index 0000000..5ba0b5e
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/jni/com_ctre_phoenix_Sensors_PigeonImuJNI.h
@@ -0,0 +1,365 @@
+/* DO NOT EDIT THIS FILE - it is machine generated */
+#include <jni.h>
+/* Header for class com_ctre_phoenix_sensors_PigeonImuJNI */
+
+#ifndef _Included_com_ctre_phoenix_sensors_PigeonImuJNI
+#define _Included_com_ctre_phoenix_sensors_PigeonImuJNI
+#ifdef __cplusplus
+extern "C" {
+#endif
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_new_PigeonImu_Talon
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1new_1PigeonImu_1Talon
+  (JNIEnv *, jclass, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_new_PigeonImu
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1new_1PigeonImu
+  (JNIEnv *, jclass, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_ConfigSetCustomParam
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1ConfigSetCustomParam
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_ConfigGetCustomParam
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1ConfigGetCustomParam
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_ConfigSetParameter
+ * Signature: (JIDIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1ConfigSetParameter
+  (JNIEnv *, jclass, jlong, jint, jdouble, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_ConfigGetParameter
+ * Signature: (JIII)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1ConfigGetParameter
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_SetStatusFramePeriod
+ * Signature: (JIII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1SetStatusFramePeriod
+  (JNIEnv *, jclass, jlong, jint, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_SetYaw
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1SetYaw
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_AddYaw
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1AddYaw
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_SetYawToCompass
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1SetYawToCompass
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_SetFusedHeading
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1SetFusedHeading
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_AddFusedHeading
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1AddFusedHeading
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_SetFusedHeadingToCompass
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1SetFusedHeadingToCompass
+  (JNIEnv *, jclass, jlong, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_SetAccumZAngle
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1SetAccumZAngle
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_ConfigTemperatureCompensationEnable
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1ConfigTemperatureCompensationEnable
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_SetCompassDeclination
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1SetCompassDeclination
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_SetCompassAngle
+ * Signature: (JDI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1SetCompassAngle
+  (JNIEnv *, jclass, jlong, jdouble, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_EnterCalibrationMode
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1EnterCalibrationMode
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetGeneralStatus
+ * Signature: (J[D)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetGeneralStatus
+  (JNIEnv *, jclass, jlong, jdoubleArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_Get6dQuaternion
+ * Signature: (J[D)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1Get6dQuaternion
+  (JNIEnv *, jclass, jlong, jdoubleArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetYawPitchRoll
+ * Signature: (J[D)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetYawPitchRoll
+  (JNIEnv *, jclass, jlong, jdoubleArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetAccumGyro
+ * Signature: (J[D)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetAccumGyro
+  (JNIEnv *, jclass, jlong, jdoubleArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetAbsoluteCompassHeading
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetAbsoluteCompassHeading
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetCompassHeading
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetCompassHeading
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetCompassFieldStrength
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetCompassFieldStrength
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetTemp
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetTemp
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetUpTime
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetUpTime
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetRawMagnetometer
+ * Signature: (J[S)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetRawMagnetometer
+  (JNIEnv *, jclass, jlong, jshortArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetBiasedMagnetometer
+ * Signature: (J[S)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetBiasedMagnetometer
+  (JNIEnv *, jclass, jlong, jshortArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetBiasedAccelerometer
+ * Signature: (J[S)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetBiasedAccelerometer
+  (JNIEnv *, jclass, jlong, jshortArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetRawGyro
+ * Signature: (J[D)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetRawGyro
+  (JNIEnv *, jclass, jlong, jdoubleArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetAccelerometerAngles
+ * Signature: (J[D)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetAccelerometerAngles
+  (JNIEnv *, jclass, jlong, jdoubleArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetFusedHeading
+ * Signature: (J[D)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetFusedHeading
+  (JNIEnv *, jclass, jlong, jdoubleArray);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetState
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetState
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetResetCount
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetResetCount
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetResetFlags
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetResetFlags
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetFirmwareVersion
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetFirmwareVersion
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetLastError
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetLastError
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_HasResetOccurred
+ * Signature: (J)Z
+ */
+JNIEXPORT jboolean JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1HasResetOccurred
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetStatusFramePeriod
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetStatusFramePeriod
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_SetControlFramePeriod
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1SetControlFramePeriod
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetFaults
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetFaults
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_GetStickyFaults
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1GetStickyFaults
+  (JNIEnv *, jclass, jlong);
+
+/*
+ * Class:     com_ctre_phoenix_sensors_PigeonImuJNI
+ * Method:    JNI_ClearStickyFaults
+ * Signature: (JI)I
+ */
+JNIEXPORT jint JNICALL Java_com_ctre_phoenix_sensors_PigeonImuJNI_JNI_1ClearStickyFaults
+  (JNIEnv *, jclass, jlong, jint);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/libraries/driver/include/ctre/phoenix/paramEnum.h b/libraries/driver/include/ctre/phoenix/paramEnum.h
new file mode 100644
index 0000000..473f6a7
--- /dev/null
+++ b/libraries/driver/include/ctre/phoenix/paramEnum.h
@@ -0,0 +1,115 @@
+#pragma once
+#include <stdint.h>
+
+namespace ctre {
+namespace phoenix {
+
+/**
+ * Signal enumeration for generic signal access.
+ */
+enum ParamEnum
+	: uint32_t
+	{
+	eOnBoot_BrakeMode = 31,
+	eQuadFilterEn = 91,
+	eQuadIdxPolarity=108,
+	eClearPositionOnIdx = 100,
+    eMotionProfileHasUnderrunErr = 119,
+    eMotionProfileTrajectoryPointDurationMs = 120,
+	eClearPosOnLimitF = 144,
+	eClearPosOnLimitR = 145,
+
+	eStatusFramePeriod = 300,
+	eOpenloopRamp = 301,
+	eClosedloopRamp = 302,
+	eNeutralDeadband = 303,
+
+	ePeakPosOutput = 305,
+	eNominalPosOutput = 306,
+	ePeakNegOutput = 307,
+	eNominalNegOutput = 308,
+
+	eProfileParamSlot_P = 310,
+	eProfileParamSlot_I = 311,
+	eProfileParamSlot_D = 312,
+	eProfileParamSlot_F = 313,
+	eProfileParamSlot_IZone = 314,
+	eProfileParamSlot_AllowableErr = 315,
+    eProfileParamSlot_MaxIAccum = 316,
+    eProfileParamSlot_PeakOutput = 317,
+
+	eClearPositionOnLimitF = 320,
+	eClearPositionOnLimitR = 321,
+	eClearPositionOnQuadIdx = 322,
+
+	eSampleVelocityPeriod = 325,
+	eSampleVelocityWindow = 326,
+
+	eFeedbackSensorType = 330, // feedbackDevice_t
+	eSelectedSensorPosition = 331,
+	eFeedbackNotContinuous = 332,
+	eRemoteSensorSource = 333, // RemoteSensorSource_t
+	eRemoteSensorDeviceID = 334, // [0,62] DeviceID
+	eSensorTerm = 335, // feedbackDevice_t (ordinal is the register)
+	eRemoteSensorClosedLoopDisableNeutralOnLOS = 336,
+	ePIDLoopPolarity = 337,
+	ePIDLoopPeriod = 338,
+	eSelectedSensorCoefficient = 339,
+
+	eForwardSoftLimitThreshold = 340,
+	eReverseSoftLimitThreshold = 341,
+	eForwardSoftLimitEnable = 342,
+	eReverseSoftLimitEnable = 343,
+
+	eNominalBatteryVoltage = 350,
+	eBatteryVoltageFilterSize = 351,
+
+	eContinuousCurrentLimitAmps = 360,
+	ePeakCurrentLimitMs = 361,
+	ePeakCurrentLimitAmps = 362,
+
+	eClosedLoopIAccum = 370,
+
+	eCustomParam = 380,
+
+	eStickyFaults = 390,
+
+	eAnalogPosition = 400,
+	eQuadraturePosition = 401,
+	ePulseWidthPosition = 402,
+
+	eMotMag_Accel = 410,
+	eMotMag_VelCruise = 411,
+
+	eLimitSwitchSource = 421, // ordinal (fwd=0,reverse=1), @see LimitSwitchSource_t
+	eLimitSwitchNormClosedAndDis = 422, // ordinal (fwd=0,reverse=1). @see LimitSwitchNormClosedAndDis_t
+	eLimitSwitchDisableNeutralOnLOS = 423,
+	eLimitSwitchRemoteDevID = 424,
+	eSoftLimitDisableNeutralOnLOS = 425,
+
+	ePulseWidthPeriod_EdgesPerRot = 430,
+	ePulseWidthPeriod_FilterWindowSz = 431,
+
+	eYawOffset	=160,
+	eCompassOffset	= 161,
+	eBetaGain =	162,
+	eEnableCompassFusion =	163,
+	eGyroNoMotionCal =	164,
+	eEnterCalibration =	165,
+	eFusedHeadingOffset	= 166,
+	eStatusFrameRate	= 169,
+	eAccumZ	= 170,
+	eTempCompDisable	= 171,
+	eMotionMeas_tap_threshX = 172,
+	eMotionMeas_tap_threshY = 173,
+	eMotionMeas_tap_threshZ = 174,
+	eMotionMeas_tap_count = 175,
+	eMotionMeas_tap_time = 176,
+	eMotionMeas_tap_time_multi = 177,
+	eMotionMeas_shake_reject_thresh = 178,
+	eMotionMeas_shake_reject_time = 179,
+	eMotionMeas_shake_reject_timeout = 180,
+};
+
+}
+}
diff --git a/libraries/driver/lib/libCTRE_PhoenixCCI.a b/libraries/driver/lib/libCTRE_PhoenixCCI.a
new file mode 100644
index 0000000..71b326a
--- /dev/null
+++ b/libraries/driver/lib/libCTRE_PhoenixCCI.a
Binary files differ
diff --git a/libraries/java/lib/libCTRE_PhoenixCCI.so b/libraries/java/lib/libCTRE_PhoenixCCI.so
new file mode 100644
index 0000000..35e11eb
--- /dev/null
+++ b/libraries/java/lib/libCTRE_PhoenixCCI.so
Binary files differ
diff --git a/locations.gradle b/locations.gradle
new file mode 100644
index 0000000..3ea68b5
--- /dev/null
+++ b/locations.gradle
@@ -0,0 +1,34 @@
+// Include and src directories for building the wpilibc library
+ext.cppInclude = "${rootDir}/cpp/include"
+ext.cppSrc = "${rootDir}/cpp/src"
+
+// Include and src directories for building the driver library
+ext.driverInclude = "${rootDir}/driver/include"
+ext.driverSrc = "${rootDir}/driver/src"
+
+// Include and lib directories for including in the wpilibc library
+ext.cppLibraryInclude = "${rootDir}/libraries/cpp/include"
+ext.cppLibraryLib = "${rootDir}/libraries/cpp/lib"
+ext.cppLibrarySrc = "${rootDir}/libraries/cpp/src"
+
+// Include and lib directories for including in the driver library
+ext.driverLibraryInclude = "${rootDir}/libraries/driver/include"
+ext.driverLibraryLib = "${rootDir}/libraries/driver/lib"
+ext.driverLibrarySrc = "${rootDir}/libraries/driver/src"
+
+// Locations for Java and JNI source files
+ext.javaJNISrc = "${rootDir}/java/lib"
+ext.javaSrc = "${rootDir}/java/src"
+
+// Locations for libraries to include in the Java library
+ext.javaLibraryLoc = "${rootDir}/libraries/java/lib"
+
+// Locations for the JNI java files to generate headers from
+ext.jniDefinitions = []
+
+// Location for user provided java library source code if it is not already in a jar
+ext.userJavaSrc = "${rootDir}/libraries/java/src"
+
+// Folder to put release zips into
+ext.releaseDir = file("${rootDir}/release")
+ext.archiveReleaseDir = file("${rootDir}/build/tmp/archive")
diff --git a/options.gradle b/options.gradle
new file mode 100644
index 0000000..8959504
--- /dev/null
+++ b/options.gradle
@@ -0,0 +1,53 @@
+//input parameters
+def beta = project.hasProperty('beta') ? project.beta : false
+
+//defaults
+ext.wpiDepYear = 2018
+ext.wpiDeps = "wpi-dependencies-2018.gradle"
+ext.FRCToolChainYear = 2018
+ext.wpilibjDep = 'edu.wpi.first.wpilibj:wpilibj-java:+'
+ext.javaNetTablesDep = 'edu.wpi.first.ntcore:ntcore-java:+'
+
+if(beta)
+{
+
+}
+
+
+
+//Choose which dependencies gradle to use
+if(wpiDepYear == 2017)
+{
+	wpiDeps = "wpi-dependencies-2017.gradle"
+	wpilibjDep = 'edu.wpi.first.wpilibj:athena:+'
+	javaNetTablesDep = 'edu.wpi.first.wpilib.networktables.java:NetworkTables:+:arm'
+	println "[INFO] Using 2017 WPI dependencies"
+}
+else if (wpiDepYear == 2018)
+{
+	wpiDeps = "wpi-dependencies-2018.gradle"
+	wpilibjDep = 'edu.wpi.first.wpilibj:wpilibj-java:+'
+	javaNetTablesDep = 'edu.wpi.first.ntcore:ntcore-java:+'
+	println "[INFO] Using 2018 WPI dependencies"
+}
+
+ext.toolChainVers = null
+ext.instToolChain = null
+
+toolChainVers = new File('C:/frc/version.txt')
+if(toolChainVers.exists()){instToolChain = toolChainVers.text}
+else {println "[INFO] Toolchain version not found"}
+
+
+//Choose the path for your FRC Toolchain
+if(FRCToolChainYear == 2018)
+{
+	if(instToolChain != null && instToolChain != '2018'){throw new GradleException('2018 FRC Toolchain Not Installed')}
+	println "[INFO] Using 2018 FRC Toolchain"
+}
+else if(FRCToolChainYear == 2017)
+{
+	if(instToolChain != null && instToolChain != '2017'){throw new GradleException('2017 FRC Toolchain Not Installed')}
+	println "[INFO] Using 2017 FRC Toolchain"
+}
+//default to PATH otherwise
\ No newline at end of file
diff --git a/properties.gradle b/properties.gradle
new file mode 100644
index 0000000..cd5688c
--- /dev/null
+++ b/properties.gradle
@@ -0,0 +1,24 @@
+// Name of the library used for changing zip outputs and jar name
+// Note will not change native library names
+ext.libraryName = "CTRE_Phoenix"
+
+// Set to true to embed included java libraries in the output jar
+ext.embedJavaLibraries = false
+
+// Set to true to combine the driver and cpp static libraries for wpilibc use
+ext.combineStaticLibs = false
+
+// Set to true to include the java sources in the user zip
+ext.includeJavaSources = true
+
+// Set to true to include the java javadocs in the user zip
+ext.includeJavaJavadoc = true
+
+// Set to true to include the cpp sources in the user zip (does not include the driver sources)
+ext.includeCppSources = false
+
+// Set to true to include the driver sources in the user zip
+ext.includeDriverSources = false
+
+// Set to true to automatically copy to eclipse on build
+ext.setCopyToEclipse = true
\ No newline at end of file
diff --git a/releases.gradle b/releases.gradle
new file mode 100644
index 0000000..47c34ac
--- /dev/null
+++ b/releases.gradle
@@ -0,0 +1,288 @@
+task javaSourceJar(type: Jar) {
+    description = 'Generates the source jar for java'
+    group = ''
+    baseName = libraryName
+    classifier = "sources"
+    duplicatesStrategy = 'exclude'
+    destinationDir = archiveReleaseDir
+    
+	dependsOn project(':arm:cpp').classes
+	from project(':arm:cpp').sourceSets.main.allJava
+}
+
+task javaJavadocJar(type: Jar) {
+    description = 'Generates the javadoc jar for java'
+    group = ''
+    baseName = libraryName
+    classifier = "javadoc"
+    duplicatesStrategy = 'exclude'
+    destinationDir = archiveReleaseDir
+    
+	dependsOn project(':arm:cpp').javadoc
+	from project(':arm:cpp').javadoc.destinationDir
+}
+
+task cppSources(type: Zip) {
+    description = 'Creates a zip of cpp sources.'
+    group = ''
+    destinationDir = archiveReleaseDir
+    baseName = libraryName
+    classifier = 'cppsources'
+    duplicatesStrategy = 'exclude'
+    
+    from(cppSrc) {
+        into 'src'
+    }
+
+    from(cppInclude) {
+        into 'include'
+    }
+}
+
+task copyToEclipse(type: Copy) {
+    description = 'Creates user zip of libraries, with shared c++ libs.'
+    group = ''
+    def userDir = System.getProperty("user.home")
+    destinationDir = file("${userDir}/wpilib/user/")
+    
+    // Copy include files from cpp project
+    from(file(cppInclude)) {
+        include '**/*.h'
+        into '/cpp/include'
+    }
+
+    // Copy static binaries from cpp project
+    project(':arm:cpp').model {
+        binaries {
+            withType(StaticLibraryBinarySpec) { binary ->
+                from(binary.staticLibraryFile) {
+                    include '*.a'
+                    into '/cpp/lib'
+                }
+            }
+        }
+    }
+
+    // copy driver include
+    from (file(driverInclude)) {
+        include '**/*.h'
+        into '/cpp/include'
+    }
+    
+    // Copy included driver library headers
+    from(file(driverLibraryInclude)) {
+        include '**/*.h'
+        into '/cpp/include'
+    }
+    
+    // Copy included driver library binaries
+    from(file(driverLibraryLib)) {
+        include '*.so*'
+        include '*.a*'
+        into '/cpp/lib'
+    }
+    
+    // Copy included driver library headers
+    from(file(cppLibraryInclude)) {
+        include '**/*.h'
+        into '/cpp/include'
+    }
+    
+    // Copy included driver library binaries
+    from(file(cppLibraryLib)) {
+        include '*.so*'
+        include '*.a*'
+        into '/cpp/lib'
+    }
+
+	def javaProject = project(':arm:cpp')
+	dependsOn javaProject.jar
+	// Copy project java binary
+	from (file(javaProject.jar.archivePath)) {
+		into '/java/lib'
+	}
+    
+    
+    // If not embedded java, include java libs
+    if (!embedJavaLibraries) {
+        from(file(javaLibraryLoc)) {
+            include '*.jar'
+            include '*.so'
+            include '*.so.debug'
+            into '/java/lib'
+        }
+    }
+
+    // Include java sources if set
+    if (includeJavaSources) {
+      dependsOn javaSourceJar
+        from (file(javaSourceJar.archivePath)) {
+            into '/java/lib'
+        }
+    }
+    
+    // Include java javadoc if set
+    if (includeJavaJavadoc) {
+        dependsOn javaJavadocJar
+        from (file(javaJavadocJar.archivePath)) {
+            into '/java/docs'
+        }
+    }
+    
+    // Include cpp sources if set
+    if (includeCppSources) {
+        from(file(cppSrc)) {
+            include '**/*.cpp'
+            include '**/*.h'
+            into "/cpp/src/$libraryName"
+        }
+    }
+    
+    // Include driver sources if set
+    if (includeDriverSources) {
+        from(file(driverSrc)) {
+            include '**/*.cpp'
+            include '**/*.h'
+            into "/cpp/src/$libraryName"
+        }
+    }
+}
+
+
+task userStaticArtifacts(type: Copy) {
+    description = 'Creates user zip of libraries, with static c++ libs.'
+    group = ''
+    destinationDir = releaseDir
+    duplicatesStrategy = 'exclude'
+    
+    // Copy include files from cpp project
+    from(file(cppInclude)) {
+        include '**/*.h'
+        into '/cpp/include'
+    }
+
+    // Copy static binaries from cpp project
+    project(':arm:cpp').model {
+        binaries {
+            withType(StaticLibraryBinarySpec) { binary ->
+                from(binary.staticLibraryFile) {
+                    include '*.a'
+                    into '/cpp/lib'
+                }
+            }
+        }
+    }
+
+    // copy driver include
+    from (file(driverInclude)) {
+        include '**/*.h'
+        into '/cpp/include'
+    }
+    
+    // Copy included driver library headers
+    from(file(driverLibraryInclude)) {
+        include '**/*.h'
+        into '/cpp/include'
+    }
+    
+    // Copy included driver library binaries
+    from(file(driverLibraryLib)) {
+        include '*.so*'
+        include '*.a*'
+        into '/cpp/lib'
+    }
+    
+    // Copy included driver library headers
+    from(file(cppLibraryInclude)) {
+        include '**/*.h'
+        into '/cpp/include'
+    }
+    
+    // Copy included driver library binaries
+    from(file(cppLibraryLib)) {
+        include '*.so*'
+        include '*.a*'
+        into '/cpp/lib'
+    }
+
+	def javaProject = project(':arm:cpp')
+	dependsOn javaProject.jar
+	// Copy project java binary
+	from (file(javaProject.jar.archivePath)) {
+		into '/java/lib'
+	}
+    
+    // If not embedded java, include java libs
+    if (!embedJavaLibraries) {
+        from(file(javaLibraryLoc)) {
+            include '*.jar'
+            include '*.so'
+            include '*.so.debug'
+            into '/java/lib'
+        }
+    }
+
+    // Include java sources if set
+    if (includeJavaSources) {
+      dependsOn javaSourceJar
+        from (file(javaSourceJar.archivePath)) {
+            into '/java/lib'
+        }
+    }
+    
+    // Include java javadoc if set
+    if (includeJavaJavadoc) {
+        dependsOn javaJavadocJar
+        from (file(javaJavadocJar.archivePath)) {
+            into '/java/docs'
+        }
+    }
+    
+    // Include cpp sources if set
+    if (includeCppSources) {
+        from(file(cppSrc)) {
+            include '**/*.cpp'
+            include '**/*.h'
+            into "/cpp/src/$libraryName"
+        }
+    }
+    
+    // Include driver sources if set
+    if (includeDriverSources) {
+        from(file(driverSrc)) {
+            include '**/*.cpp'
+            include '**/*.h'
+            into "/cpp/src/$libraryName"
+        }
+    }
+}
+
+task doc(type: Exec){
+	doFirst{
+		mkdir "${releaseDir}/cpp/docs"
+	}
+	commandLine 'powershell', 'doxygen ctrlib.doxy'
+}
+
+
+project(':arm:cpp').tasks.whenTaskAdded { task ->
+    def name = task.name.toLowerCase()
+    if (name.contains("sharedlibrary") || name.contains("staticlibrary")) {
+		userStaticArtifacts.dependsOn task
+        copyToEclipse.dependsOn task
+    }
+}
+
+
+//build.dependsOn javaSourceJar
+//build.dependsOn javaJavadocJar
+//build.dependsOn cppSources
+
+build.dependsOn userStaticArtifacts
+build.dependsOn doc
+
+doc.mustRunAfter userStaticArtifacts
+
+if (setCopyToEclipse) {
+    build.dependsOn copyToEclipse
+}
\ No newline at end of file
diff --git a/settings.gradle b/settings.gradle
new file mode 100644
index 0000000..07c9a77
--- /dev/null
+++ b/settings.gradle
@@ -0,0 +1 @@
+include 'arm:cpp'
\ No newline at end of file
diff --git a/toolchains/arm.gradle b/toolchains/arm.gradle
new file mode 100644
index 0000000..4fe9f86
--- /dev/null
+++ b/toolchains/arm.gradle
@@ -0,0 +1,97 @@
+ext.isArm = true
+ext.buildPlatform = 'arm'
+
+def compilerPrefix = project.hasProperty('compilerPrefix') ? project.compilerPrefix : 'arm-frc-linux-gnueabi-'
+def toolChainPath = project.hasProperty('toolChainPath') ? project.toolChainPath : null
+model {
+    platforms {
+        arm {
+            architecture 'arm'
+            operatingSystem 'linux'
+        }
+    }
+    toolChains {
+        armGcc(Gcc) {
+            if (toolChainPath != null) path(toolChainPath)
+            target("arm") {
+                // We use a custom-built cross compiler with the prefix arm-frc-linux-gnueabi-<util name>
+                // If this ever changes, the prefix will need to be changed here
+                cCompiler.executable = compilerPrefix + cCompiler.executable
+                cppCompiler.executable = compilerPrefix + cppCompiler.executable
+                linker.executable = compilerPrefix + linker.executable
+                assembler.executable = compilerPrefix + assembler.executable
+                // Gradle auto-adds the -m32 argument to the linker and compiler. Our compiler only supports
+                // arm, and doesn't understand this flag, so it is removed from both
+                cppCompiler.withArguments { args ->
+					args << '-std=c++1y' << '-Wformat=2' << '-Wall' << '-Wextra' << '-Werror' << '-pedantic'
+                    args << '-fPIC' << '-rdynamic'
+                    args << '-pthread'
+                    args.remove('-m32')
+                }
+                linker.withArguments { args ->
+                    args << '-rdynamic' << '-pthread'
+                    args.remove('-m32')
+                }
+                staticLibArchiver.executable = compilerPrefix + staticLibArchiver.executable
+            }
+        }
+    }
+}
+
+ext.binTools = { tool ->
+    if (toolChainPath != null) return "${toolChainPath}/${compilerPrefix}${tool}"
+    return "${compilerPrefix}${tool}"
+}
+
+ext.setupReleaseDefines = { cppCompiler, linker ->
+    cppCompiler.args '-O2', '-g'
+}
+
+ext.setupDebugDefines = { cppCompiler, linker ->
+    cppCompiler.args '-g', '-O0'
+}
+
+// Used only on Windows.
+ext.setupDef = { linker, deffile -> }
+
+ext.debugStripSetup = {
+    if (!project.hasProperty('debug')) {
+        project.tasks.whenObjectAdded { task ->
+            if (task.name.contains('link') && task.name.contains('SharedLibrary')) {
+                def library = task.outputFile.absolutePath
+                def debugLibrary = task.outputFile.absolutePath + ".debug"
+                task.doLast {
+                    exec { commandLine binTools('objcopy'), '--only-keep-debug', library, debugLibrary }
+                    exec { commandLine binTools('strip'), '-g', library }
+                    exec { commandLine binTools('objcopy'), "--add-gnu-debuglink=$debugLibrary", library }
+                }
+            }
+        }
+    }
+}
+
+ext.checkNativeSymbols = { getSymbolFunc ->
+    project.tasks.whenObjectAdded { task ->
+        if (task.name.contains('link') && task.name.contains('SharedLibrary')) {
+            def library = task.outputFile.absolutePath
+            task.doLast {
+                def nmOutput = new ByteArrayOutputStream()
+                exec { 
+                    commandLine binTools('nm'), library
+                    standardOutput nmOutput
+                }
+                // Remove '\r' so we can check for full string contents
+                String nmSymbols = nmOutput.toString().replace('\r', '')
+
+                def symbolList = getSymbolFunc()
+                symbolList.each {
+                    //Add \n so we can check for the exact symbol
+                    def found = nmSymbols.contains(it + '\n')
+                    if (!found) {
+                        throw new GradleException("Found a definition that does not have a matching symbol ${it}")
+                    }
+                }
+            }
+        }
+    }
+}
diff --git a/wpi-dependencies-2017.gradle b/wpi-dependencies-2017.gradle
new file mode 100644
index 0000000..f2b06d3
--- /dev/null
+++ b/wpi-dependencies-2017.gradle
@@ -0,0 +1,284 @@
+task downloadWpiUtil() {
+    description = 'Downloads the C++ ARM wpiutil maven dependency.'
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def utilZip = file("$depFolder/wpiutil.zip")
+    outputs.file(utilZip)
+    def armWpiUtil
+
+    doFirst {
+        def armWpiUtilDependency = project.dependencies.create("edu.wpi.first.wpilib:wpiutil:+:arm@zip")
+        def armWpiUtilConfig = project.configurations.detachedConfiguration(armWpiUtilDependency)
+        armWpiUtilConfig.setTransitive(false)
+        armWpiUtil = armWpiUtilConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armWpiUtil
+            rename 'wpiutil(.+)', 'wpiutil.zip'
+            into depFolder
+        }
+    }
+}
+
+def wpiUtilUnzipLocation = "$buildDir/wpiutil"
+
+// Create a task that will unzip the wpiutil files into a temporary build directory
+task unzipWpiUtil(type: Copy) {
+    description = 'Unzips the wpiutil maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadWpiUtil
+
+    from zipTree(downloadWpiUtil.outputs.files.singleFile)
+    into wpiUtilUnzipLocation
+}
+
+ext.defineWpiUtilProperties = {
+    ext.wpiUtil = wpiUtilUnzipLocation
+    ext.wpiUtilInclude = "$wpiUtilUnzipLocation/include"
+    ext.wpiUtilLibArmLocation = "$wpiUtilUnzipLocation/Linux/arm"
+    ext.wpiUtilSharedLib = "$wpiUtilLibArmLocation/libwpiutil.so"
+    ext.wpiUtilSharedLibDebug = "$wpiUtilLibArmLocation/libwpiutil.so.debug"
+    ext.addWpiUtilLibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipWpiUtil
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+            linker.args wpiUtilSharedLib
+        }
+    }
+}
+
+ext.NIInclude = "$buildDir/NI/include"
+
+def halUnzipLocation = "$buildDir/hal"
+
+task downloadHAL() {
+    description = 'Downloads the C++ ARM HAL maven dependency.'
+
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def libZip = file("$depFolder/hal.zip")
+    outputs.file(libZip)
+    def armHal
+    
+
+    doFirst {
+        def armHALDependency = project.dependencies.create("edu.wpi.first.wpilib:hal:+@zip")
+        def armHALConfig = project.configurations.detachedConfiguration(armHALDependency)
+        armHALConfig.setTransitive(false)
+        armHal = armHALConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armHal
+            rename 'hal(.+)', 'hal.zip'
+            into depFolder
+        }
+    }
+}
+
+// Create a task that will unzip the hal files into a temporary build directory
+task unzipHAL(type: Copy) {
+    description = 'Unzips the hal maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadHAL
+    
+    from zipTree(downloadHAL.outputs.files.singleFile)
+    into halUnzipLocation
+}
+
+ext.defineHALProperties = {
+    ext.hal = halUnzipLocation
+    ext.halInclude = "$halUnzipLocation/include"
+    ext.halLocation = "$halUnzipLocation/lib"
+    ext.halSharedLib = "$halLocation/libHALAthena.so"
+  
+    ext.addHalLibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipHAL
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+            // Grab all the shared libraries and link them
+            linker.args halSharedLib      
+            linker.args "$halLocation/libnilibraries.so"
+
+            def libraryPath = halLocation
+
+            linker.args << '-L' + libraryPath
+        }
+    }
+}
+
+task downloadNetworkTables() {
+    description = 'Downloads the C++ ARM NetworkTables maven dependency.'
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def ntZip = file("$depFolder/ntcore.zip")
+    outputs.file(ntZip)
+    def armNetTables
+
+    doFirst {
+        def armNtDependency = project.dependencies.create('edu.wpi.first.wpilib.networktables.cpp:NetworkTables:+:arm@zip')
+        def armConfig = project.configurations.detachedConfiguration(armNtDependency)
+        armConfig.setTransitive(false)
+        armNetTables = armConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armNetTables
+            rename 'NetworkTables(.+)', 'ntcore.zip'
+            into depFolder
+        }
+    }
+}
+
+def netTablesUnzipLocation = "$buildDir/networktables"
+
+// Create a task that will unzip the networktables files into a temporary build directory
+task unzipNetworkTables(type: Copy) {
+    description = 'Unzips the networktables maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadNetworkTables
+
+    from zipTree(downloadNetworkTables.outputs.files.singleFile)
+    into netTablesUnzipLocation
+}
+
+// This defines a project property that projects depending on network tables can use to setup that dependency.
+ext.defineNetworkTablesProperties = {
+    ext.netTables = netTablesUnzipLocation
+    ext.netTablesInclude = "$netTablesUnzipLocation/include"
+    ext.netLibArmLocation = "$netTablesUnzipLocation/Linux/arm"
+    ext.netSharedLib = "$netLibArmLocation/libntcore.so"
+    ext.netSharedLibDebug = "$netLibArmLocation/libntcore.so.debug"
+
+    ext.addNetworkTablesLibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipNetworkTables
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+            linker.args netSharedLib
+        }
+        addWpiUtilLibraryLinks(compileTask, linker, targetPlatform)
+    }
+}
+
+
+def wpilibUnzipLocation = "$buildDir/wpilib"
+
+task downloadWpilib() {
+    description = 'Downloads the C++ ARM wpilib maven dependency.'
+
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def libZip = file("$depFolder/athena-wpilibc.zip")
+    outputs.file(libZip)
+    def armWPILib
+    
+
+    doFirst {
+        def armWpiLibDependency = project.dependencies.create("edu.wpi.first.wpilibc:athena:+@zip")
+        def armWpiLibConfig = project.configurations.detachedConfiguration(armWpiLibDependency)
+        armWpiLibConfig.setTransitive(false)
+        armWPILib = armWpiLibConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armWPILib
+            rename 'athena(.+)', 'athena-wpilibc.zip'
+            into depFolder
+        }
+    }
+}
+
+// Create a task that will unzip the wpilib files into a temporary build directory
+task unzipWpilib(type: Copy) {
+    description = 'Unzips the wpilib maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadWpilib
+    
+    from zipTree(downloadWpilib.outputs.files.singleFile)
+    into wpilibUnzipLocation
+}
+
+ext.defineWpiLibProperties = {
+    ext.wpilib = wpilibUnzipLocation
+    ext.wpilibInclude = "$wpilibUnzipLocation/include"
+    ext.wpilibLocation = "$wpilibUnzipLocation/lib"
+    ext.wpilibSharedLib = "$wpilibLocation/libwpilibc.so"
+    
+    ext.addWpilibLibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipWpilib
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+            // Grab all the shared libraries and link them
+            linker.args wpilibSharedLib
+
+            def libraryPath = wpilibLocation
+
+            linker.args << '-L' + libraryPath
+        }
+    }
+}
+
+def cscoreUnzipLocation = "$buildDir/cscore"
+
+task downloadCsCore() {
+    description = 'Downloads the C++ ARM CsCore maven dependency.'
+
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def libZip = file("$depFolder/cscore.zip")
+    outputs.file(libZip)
+    def armCsCore
+    
+
+    doFirst {
+        def armCsDependency = project.dependencies.create("edu.wpi.cscore.cpp:cscore:+:athena-uberzip@zip")
+        def armCsConfig = project.configurations.detachedConfiguration(armCsDependency)
+        armCsConfig.setTransitive(false)
+        armCsCore = armCsConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armCsCore
+            rename 'cscore(.+)', 'cscore.zip'
+            into depFolder
+        }
+    }
+}
+
+// Create a task that will unzip the cscore files into a temporary build directory
+task unzipCsCore(type: Copy) {
+    description = 'Unzips the cscore maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadCsCore
+    
+    from zipTree(downloadCsCore.outputs.files.singleFile)
+    into cscoreUnzipLocation
+}
+
+ext.defineCsCoreProperties = {
+    ext.cscore = cscoreUnzipLocation
+    ext.cscoreInclude = "$cscoreUnzipLocation/include"
+    ext.cscoreLocation = "$cscoreUnzipLocation/lib"
+    ext.opencvSharedLib = "$cscoreLocation/libopencv.so"
+	ext.cscoreSharedLib = "$cscoreLocation/libcscore.so"
+    
+    ext.addCsCoreLibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipCsCore
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+            // Grab all the shared libraries and link them
+            linker.args opencvSharedLib
+			linker.args cscoreSharedLib
+
+            def libraryPath = cscoreLocation
+
+            linker.args << '-L' + libraryPath
+        }
+    }
+}
diff --git a/wpi-dependencies-2018.gradle b/wpi-dependencies-2018.gradle
new file mode 100644
index 0000000..b38190e
--- /dev/null
+++ b/wpi-dependencies-2018.gradle
@@ -0,0 +1,472 @@
+task downloadWpiUtil() {
+    description = 'Downloads the C++ ARM wpiutil maven dependency.'
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def utilZip = file("$depFolder/wpiutil.zip")
+    outputs.file(utilZip)
+    def armWpiUtil
+
+    doFirst {
+        def armWpiUtilDependency = project.dependencies.create("edu.wpi.first.wpiutil:wpiutil-cpp:+:linuxathena@zip")
+        def armWpiUtilConfig = project.configurations.detachedConfiguration(armWpiUtilDependency)
+        armWpiUtilConfig.setTransitive(false)
+        armWpiUtil = armWpiUtilConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armWpiUtil
+            rename 'wpiutil(.+)', 'wpiutil.zip'
+            into depFolder
+        }
+    }
+}
+
+task downloadWpiUtilHeaders() {
+    description = 'Downloads the C++ ARM wpiutil maven dependency.'
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def utilZip = file("$depFolder/wpiutilHeaders.zip")
+    outputs.file(utilZip)
+    def armWpiUtilHeaders
+
+    doFirst {
+        def armWpiUtilHeadersDependency = project.dependencies.create("edu.wpi.first.wpiutil:wpiutil-cpp:+:headers@zip")
+        def armWpiUtilHeadersConfig = project.configurations.detachedConfiguration(armWpiUtilHeadersDependency)
+        armWpiUtilHeadersConfig.setTransitive(false)
+        armWpiUtilHeaders = armWpiUtilHeadersConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armWpiUtilHeaders
+            rename 'wpiutil(.+)', 'wpiutilHeaders.zip'
+            into depFolder
+        }
+    }
+}
+
+def wpiUtilUnzipLocation = "$buildDir/wpiutil"
+def wpiUtilUnzipHeadersLocation = "$buildDir/wpiutil/include"
+
+// Create a task that will unzip the wpiutil files into a temporary build directory
+task unzipWpiUtil(type: Copy) {
+    description = 'Unzips the wpiutil maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadWpiUtil
+
+    from zipTree(downloadWpiUtil.outputs.files.singleFile)
+    into wpiUtilUnzipLocation
+}
+
+// Create a task that will unzip the wpiutil files into a temporary build directory
+task unzipWpiUtilHeaders(type: Copy) {
+    description = 'Unzips the wpiutil maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadWpiUtilHeaders
+
+    from zipTree(downloadWpiUtilHeaders.outputs.files.singleFile)
+    into wpiUtilUnzipHeadersLocation
+}
+
+ext.defineWpiUtilProperties = {
+    ext.wpiUtil = wpiUtilUnzipLocation
+    ext.wpiUtilInclude = "$wpiUtilUnzipLocation/include"
+    ext.wpiUtilLibArmLocation = "$wpiUtilUnzipLocation/linux/athena/shared"
+    ext.wpiUtilSharedLib = "$wpiUtilLibArmLocation/libwpiutil.so"
+    ext.wpiUtilSharedLibDebug = "$wpiUtilLibArmLocation/libwpiutil.so.debug"
+    ext.addWpiUtilLibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipWpiUtil
+		compileTask.dependsOn project(':').unzipWpiUtilHeaders
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+//            linker.args wpiUtilSharedLib
+        }
+    }
+}
+
+
+
+def NIUnzipHeadersLocation = "$buildDir/NI/include"
+
+task downloadNIHeaders() {
+    description = 'Downloads the NI headers.'
+
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def libZip = file("$depFolder/NIHeaders.zip")
+    outputs.file(libZip)
+    def NIHeaders
+    
+
+    doFirst {
+        def NIHeadersDependency = project.dependencies.create("edu.wpi.first.ni-libraries:ni-libraries:+:headers@zip")
+        def NIHeadersConfig = project.configurations.detachedConfiguration(NIHeadersDependency)
+        NIHeadersConfig.setTransitive(false)
+        NIHeaders = NIHeadersConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from NIHeaders
+            rename 'ni-libraries(.+)', 'NIHeaders.zip'
+            into depFolder
+        }
+    }
+}
+
+task unzipNIHeaders(type: Copy) {
+    description = 'Unzips the NI include files so they can be used'
+    group = 'WPILib'
+    dependsOn downloadNIHeaders
+    
+    from zipTree(downloadNIHeaders.outputs.files.singleFile)
+    into NIUnzipHeadersLocation
+}
+
+ext.defineNIProperties = {
+    ext.NI = NIUnzipHeadersLocation
+    ext.NIInclude = NIUnzipHeadersLocation
+  
+    ext.addNILibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipNIHeaders
+    }
+}
+
+
+
+def halUnzipLocation = "$buildDir/hal"
+def halUnzipHeadersLocation = "$buildDir/hal/include"
+
+task downloadHAL() {
+    description = 'Downloads the C++ ARM HAL maven dependency.'
+
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def libZip = file("$depFolder/hal.zip")
+    outputs.file(libZip)
+    def armHal
+    
+
+    doFirst {
+        def armHALDependency = project.dependencies.create("edu.wpi.first.hal:hal:+:linuxathena@zip")
+        def armHALConfig = project.configurations.detachedConfiguration(armHALDependency)
+        armHALConfig.setTransitive(false)
+        armHal = armHALConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armHal
+            rename 'hal(.+)', 'hal.zip'
+            into depFolder
+        }
+    }
+}
+
+task downloadHALHeaders() {
+    description = 'Downloads the C++ ARM HAL maven dependency.'
+
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def libZip = file("$depFolder/halHeaders.zip")
+    outputs.file(libZip)
+    def armHalHeaders
+    
+
+    doFirst {
+        def armHALHeadersDependency = project.dependencies.create("edu.wpi.first.hal:hal:+:headers@zip")
+        def armHALHeadersConfig = project.configurations.detachedConfiguration(armHALHeadersDependency)
+        armHALHeadersConfig.setTransitive(false)
+        armHalHeaders = armHALHeadersConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armHalHeaders
+            rename 'hal(.+)', 'halHeaders.zip'
+            into depFolder
+        }
+    }
+}
+
+// Create a task that will unzip the hal files into a temporary build directory
+task unzipHAL(type: Copy) {
+    description = 'Unzips the hal maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadHAL
+    
+    from zipTree(downloadHAL.outputs.files.singleFile)
+    into halUnzipLocation
+}
+task unzipHALHeaders(type: Copy) {
+    description = 'Unzips the hal maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadHALHeaders
+    
+    from zipTree(downloadHALHeaders.outputs.files.singleFile)
+    into halUnzipHeadersLocation
+}
+
+ext.defineHALProperties = {
+    ext.hal = halUnzipLocation
+    ext.halInclude = "$halUnzipLocation/include"
+    ext.halLocation = "$halUnzipLocation/linux/athena/shared"
+    ext.halSharedLib = "$halLocation/libwpiHal.so"
+  
+    ext.addHalLibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipHAL
+		compileTask.dependsOn project(':').unzipHALHeaders
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+            // Grab all the shared libraries and link them
+            linker.args halSharedLib      
+//            linker.args "$halLocation/libnilibraries.so"
+
+            def libraryPath = halLocation
+
+            linker.args << '-L' + libraryPath
+        }
+    }
+}
+
+task downloadNetworkTables() {
+    description = 'Downloads the C++ ARM NetworkTables maven dependency.'
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def ntZip = file("$depFolder/ntcore.zip")
+    outputs.file(ntZip)
+    def armNetTables
+
+    doFirst {
+        def armNtDependency = project.dependencies.create('edu.wpi.first.ntcore:ntcore-cpp:+:linuxathena@zip')
+        def armConfig = project.configurations.detachedConfiguration(armNtDependency)
+        armConfig.setTransitive(false)
+        armNetTables = armConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armNetTables
+            rename 'ntcore(.+)', 'ntcore.zip'
+            into depFolder
+        }
+    }
+}
+task downloadNetworkTablesHeaders() {
+    description = 'Downloads the C++ ARM NetworkTables maven dependency.'
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def ntZip = file("$depFolder/ntcoreHeaders.zip")
+    outputs.file(ntZip)
+    def armNetTablesHeaders
+
+    doFirst {
+        def armNtHeadersDependency = project.dependencies.create('edu.wpi.first.ntcore:ntcore-cpp:+:headers@zip')
+        def armHeadersConfig = project.configurations.detachedConfiguration(armNtHeadersDependency)
+        armHeadersConfig.setTransitive(false)
+        armNetTablesHeaders = armHeadersConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armNetTablesHeaders
+            rename 'ntcore(.+)', 'ntcoreHeaders.zip'
+            into depFolder
+        }
+    }
+}
+
+def netTablesUnzipLocation = "$buildDir/ntcore"
+def netTablesHeadersUnzipLocation = "$buildDir/ntcore/include"
+
+// Create a task that will unzip the networktables files into a temporary build directory
+task unzipNetworkTables(type: Copy) {
+    description = 'Unzips the networktables maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadNetworkTables
+
+    from zipTree(downloadNetworkTables.outputs.files.singleFile)
+    into netTablesUnzipLocation
+}
+task unzipNetworkTablesHeaders(type: Copy) {
+    description = 'Unzips the networktables maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadNetworkTablesHeaders
+
+    from zipTree(downloadNetworkTablesHeaders.outputs.files.singleFile)
+    into netTablesHeadersUnzipLocation
+}
+
+// This defines a project property that projects depending on network tables can use to setup that dependency.
+ext.defineNetworkTablesProperties = {
+    ext.netTables = netTablesUnzipLocation
+    ext.netTablesInclude = "$netTablesUnzipLocation/include"
+    ext.netLibArmLocation = "$netTablesUnzipLocation/linux/athena/shared"
+    ext.netSharedLib = "$netLibArmLocation/libntcore.so"
+    ext.netSharedLibDebug = "$netLibArmLocation/libntcore.so.debug"
+
+    ext.addNetworkTablesLibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipNetworkTables
+		compileTask.dependsOn project(':').unzipNetworkTablesHeaders
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+            linker.args netSharedLib
+        }
+        addWpiUtilLibraryLinks(compileTask, linker, targetPlatform)
+    }
+}
+
+
+def wpilibUnzipLocation = "$buildDir/wpilib"
+def wpilibHeadersUnzipLocation = "$buildDir/wpilib/include"
+
+task downloadWpilib() {
+    description = 'Downloads the C++ ARM wpilib maven dependency.'
+
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def libZip = file("$depFolder/wpilibc.zip")
+    outputs.file(libZip)
+    def armWPILib
+    
+
+    doFirst {
+        def armWpiLibDependency = project.dependencies.create("edu.wpi.first.wpilibc:wpilibc:+:linuxathena@zip")
+        def armWpiLibConfig = project.configurations.detachedConfiguration(armWpiLibDependency)
+        armWpiLibConfig.setTransitive(false)
+        armWPILib = armWpiLibConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armWPILib
+            rename 'wpilibc(.+)', 'wpilibc.zip'
+            into depFolder
+        }
+    }
+}
+task downloadWpilibHeaders() {
+    description = 'Downloads the C++ ARM wpilib maven dependency.'
+
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def libZip = file("$depFolder/wpilibcHeaders.zip")
+    outputs.file(libZip)
+    def armWPILibHeaders
+    
+
+    doFirst {
+        def armWpiLibHeadersDependency = project.dependencies.create("edu.wpi.first.wpilibc:wpilibc:+:headers@zip")
+        def armWpiLibHeadersConfig = project.configurations.detachedConfiguration(armWpiLibHeadersDependency)
+        armWpiLibHeadersConfig.setTransitive(false)
+        armWPILibHeaders = armWpiLibHeadersConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armWPILibHeaders
+            rename 'wpilibc(.+)', 'wpilibcHeaders.zip'
+            into depFolder
+        }
+    }
+}
+
+// Create a task that will unzip the wpilib files into a temporary build directory
+task unzipWpilib(type: Copy) {
+    description = 'Unzips the wpilib maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadWpilib
+    
+    from zipTree(downloadWpilib.outputs.files.singleFile)
+    into wpilibUnzipLocation
+}
+task unzipWpilibHeaders(type: Copy) {
+    description = 'Unzips the wpilib maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadWpilibHeaders
+    
+    from zipTree(downloadWpilibHeaders.outputs.files.singleFile)
+    into wpilibHeadersUnzipLocation
+}
+
+ext.defineWpiLibProperties = {
+    ext.wpilib = wpilibUnzipLocation
+    ext.wpilibInclude = "$wpilibUnzipLocation/include"
+    ext.wpilibLocation = "$wpilibUnzipLocation/linux/athena/shared"
+    ext.wpilibSharedLib = "$wpilibLocation/libwpilibc.so"
+    
+    ext.addWpilibLibraryLinks = { compileTask, linker, targetPlatform ->
+        compileTask.dependsOn project(':').unzipWpilib
+		compileTask.dependsOn project(':').unzipWpilibHeaders
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+            // Grab all the shared libraries and link them
+            linker.args wpilibSharedLib
+
+            def libraryPath = wpilibLocation
+
+            linker.args << '-L' + libraryPath
+        }
+    }
+}
+
+def cscoreUnzipLocation = "$buildDir/cscore"
+
+task downloadCsCore() {
+    description = 'Downloads the C++ ARM CsCore maven dependency.'
+
+    group = 'WPILib'
+    def depFolder = "$buildDir/dependencies"
+    def libZip = file("$depFolder/cscore.zip")
+    outputs.file(libZip)
+    def armCsCore
+    
+
+    doFirst {
+        def armCsDependency = project.dependencies.create("edu.wpi.cscore.cpp:cscore:+:athena-uberzip@zip")
+        def armCsConfig = project.configurations.detachedConfiguration(armCsDependency)
+        armCsConfig.setTransitive(false)
+        armCsCore = armCsConfig.files[0].canonicalFile
+    }
+
+    doLast {
+        copy {
+            from armCsCore
+            rename 'cscore(.+)', 'cscore.zip'
+            into depFolder
+        }
+    }
+}
+
+// Create a task that will unzip the cscore files into a temporary build directory
+task unzipCsCore(type: Copy) {
+    description = 'Unzips the cscore maven dependency so that the include files and libraries can be used'
+    group = 'WPILib'
+    dependsOn downloadCsCore
+    
+    from zipTree(downloadCsCore.outputs.files.singleFile)
+    into cscoreUnzipLocation
+}
+
+ext.defineCsCoreProperties = {
+    ext.cscore = cscoreUnzipLocation
+    ext.cscoreInclude = "$cscoreUnzipLocation/include"
+    ext.cscoreLocation = "$cscoreUnzipLocation/lib"
+    ext.opencvSharedLib = "$cscoreLocation/libopencv.so"
+	ext.cscoreSharedLib = "$cscoreLocation/libcscore.so"
+    
+    ext.addCsCoreLibraryLinks = { compileTask, linker, targetPlatform ->
+//        compileTask.dependsOn project(':').unzipCsCore
+        String architecture = targetPlatform.architecture
+        if (architecture.contains('arm')) {
+            // Grab all the shared libraries and link them
+            linker.args opencvSharedLib
+			linker.args cscoreSharedLib
+
+            def libraryPath = cscoreLocation
+
+            linker.args << '-L' + libraryPath
+        }
+    }
+}