diff --git a/.classpath b/.classpath
deleted file mode 100644
index b786107..0000000
--- a/.classpath
+++ /dev/null
@@ -1,15 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<classpath>
-	<classpathentry kind="src" path="src"/>
-	<classpathentry kind="src" path="Pathfinder/Pathfinder-Java/src/main/java"/>
-	<classpathentry kind="var" path="wpilib" sourcepath="wpilib.sources"/>
-	<classpathentry kind="var" path="networktables" sourcepath="ntcore.sources"/>
-	<classpathentry kind="var" path="opencv" sourcepath="opencv.sources"/>
-	<classpathentry kind="var" path="cscore" sourcepath="cscore.sources"/>
-	<classpathentry kind="var" path="wpiutil" sourcepath="wpiutil.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="USERLIBS_DIR/CTRE_Phoenix-sources.jar"/>
-	<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix.jar"/>
-	<classpathentry kind="lib" path="C:/Users/Josh/Downloads/Pathfinder-Java.jar"/>
-	<classpathentry kind="output" path="bin"/>
-</classpath>
diff --git a/.gitignore b/.gitignore
index 0ba3f73..8b755a8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -26,6 +26,49 @@
 *.tar.gz
 *.rar
 
+.gradle/*
+
 # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
 hs_err_pid*
 /bin/
+
+# WPILib
+bin/
+build/
+dist/
+sysProps.xml
+workbench.xmi
+
+# IDE
+*.ipr
+*.iml
+*.iws
+out/
+tmp/
+.settings/
+.loadpath
+.project
+/.idea
+.classpath
+
+# Toast/GradleRIO
+run/
+.gradle/
+gradle/.launch/
+
+# OS Files
+.DS_Store
+.DS_Store?
+._*
+.Spotlight-V100
+.AppleDouble
+.LSOverride
+.Trashes
+ehthumbs.db
+Thumbs.db
+
+# backup files
+*~
+
+# directory for XML generated after running junit tests through ant
+junit/
diff --git a/.idea/NU18.iml b/.idea/NU18.iml
deleted file mode 100644
index 98abcfb..0000000
--- a/.idea/NU18.iml
+++ /dev/null
@@ -1,48 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module type="JAVA_MODULE" version="4">
-  <component name="FacetManager">
-    <facet type="FRC_FACET" name="FRC">
-      <configuration />
-    </facet>
-  </component>
-  <component name="NewModuleRootManager" inherit-compiler-output="true">
-    <exclude-output />
-    <content url="file://$MODULE_DIR$">
-      <sourceFolder url="file://$MODULE_DIR$/src" isTestSource="false" />
-    </content>
-    <orderEntry type="inheritedJdk" />
-    <orderEntry type="sourceFolder" forTests="false" />
-    <orderEntry type="module-library">
-      <library name="WPILib Libraries">
-        <CLASSES>
-          <root url="file://$USER_HOME$/wpilib/java/current/lib" />
-        </CLASSES>
-        <JAVADOC>
-          <root url="file://$USER_HOME$/wpilib/java/current/lib" />
-        </JAVADOC>
-        <SOURCES>
-          <root url="file://$USER_HOME$/wpilib/java/current/lib" />
-        </SOURCES>
-        <jarDirectory url="file://$USER_HOME$/wpilib/java/current/lib" recursive="true" />
-        <jarDirectory url="file://$USER_HOME$/wpilib/java/current/lib" recursive="true" type="JAVADOC" />
-        <jarDirectory url="file://$USER_HOME$/wpilib/java/current/lib" recursive="true" type="SOURCES" />
-      </library>
-    </orderEntry>
-    <orderEntry type="module-library">
-      <library name="WPILib User Lib Directory">
-        <CLASSES>
-          <root url="file://$USER_HOME$/wpilib/user/java/lib" />
-        </CLASSES>
-        <JAVADOC>
-          <root url="file://$USER_HOME$/wpilib/user/java/lib" />
-        </JAVADOC>
-        <SOURCES>
-          <root url="file://$USER_HOME$/wpilib/user/java/lib" />
-        </SOURCES>
-        <jarDirectory url="file://$USER_HOME$/wpilib/user/java/lib" recursive="true" />
-        <jarDirectory url="file://$USER_HOME$/wpilib/user/java/lib" recursive="true" type="JAVADOC" />
-        <jarDirectory url="file://$USER_HOME$/wpilib/user/java/lib" recursive="true" type="SOURCES" />
-      </library>
-    </orderEntry>
-  </component>
-</module>
\ No newline at end of file
diff --git a/.idea/libraries/CTRE_Phoenix_sources.xml b/.idea/libraries/CTRE_Phoenix_sources.xml
deleted file mode 100644
index 7b5d862..0000000
--- a/.idea/libraries/CTRE_Phoenix_sources.xml
+++ /dev/null
@@ -1,15 +0,0 @@
-<component name="libraryTable">
-  <library name="CTRE_Phoenix-sources">
-    <CLASSES>
-      <root url="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix.jar!/" />
-      <root url="jar://$USER_HOME$/wpilib/java/lib/Pathfinder-Java.jar!/" />
-    </CLASSES>
-    <JAVADOC />
-    <NATIVE>
-      <root url="file://$USER_HOME$/wpilib/java/lib" />
-    </NATIVE>
-    <SOURCES>
-      <root url="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/" />
-    </SOURCES>
-  </library>
-</component>
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
deleted file mode 100644
index 825cfcf..0000000
--- a/.idea/misc.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_8" default="false" project-jdk-name="1.8" project-jdk-type="JavaSDK" />
-    <output url="file://$PROJECT_DIR$/classes" />
-  </component>
-</project>
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
deleted file mode 100644
index 3711aed..0000000
--- a/.idea/modules.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/NU18.iml" filepath="$PROJECT_DIR$/NU18.iml" />
-    </modules>
-  </component>
-</project>
\ No newline at end of file
diff --git a/.idea/uiDesigner.xml b/.idea/uiDesigner.xml
deleted file mode 100644
index e96534f..0000000
--- a/.idea/uiDesigner.xml
+++ /dev/null
@@ -1,124 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="Palette2">
-    <group name="Swing">
-      <item class="com.intellij.uiDesigner.HSpacer" tooltip-text="Horizontal Spacer" icon="/com/intellij/uiDesigner/icons/hspacer.png" removable="false" auto-create-binding="false" can-attach-label="false">
-        <default-constraints vsize-policy="1" hsize-policy="6" anchor="0" fill="1" />
-      </item>
-      <item class="com.intellij.uiDesigner.VSpacer" tooltip-text="Vertical Spacer" icon="/com/intellij/uiDesigner/icons/vspacer.png" removable="false" auto-create-binding="false" can-attach-label="false">
-        <default-constraints vsize-policy="6" hsize-policy="1" anchor="0" fill="2" />
-      </item>
-      <item class="javax.swing.JPanel" icon="/com/intellij/uiDesigner/icons/panel.png" removable="false" auto-create-binding="false" can-attach-label="false">
-        <default-constraints vsize-policy="3" hsize-policy="3" anchor="0" fill="3" />
-      </item>
-      <item class="javax.swing.JScrollPane" icon="/com/intellij/uiDesigner/icons/scrollPane.png" removable="false" auto-create-binding="false" can-attach-label="true">
-        <default-constraints vsize-policy="7" hsize-policy="7" anchor="0" fill="3" />
-      </item>
-      <item class="javax.swing.JButton" icon="/com/intellij/uiDesigner/icons/button.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="0" hsize-policy="3" anchor="0" fill="1" />
-        <initial-values>
-          <property name="text" value="Button" />
-        </initial-values>
-      </item>
-      <item class="javax.swing.JRadioButton" icon="/com/intellij/uiDesigner/icons/radioButton.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="0" hsize-policy="3" anchor="8" fill="0" />
-        <initial-values>
-          <property name="text" value="RadioButton" />
-        </initial-values>
-      </item>
-      <item class="javax.swing.JCheckBox" icon="/com/intellij/uiDesigner/icons/checkBox.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="0" hsize-policy="3" anchor="8" fill="0" />
-        <initial-values>
-          <property name="text" value="CheckBox" />
-        </initial-values>
-      </item>
-      <item class="javax.swing.JLabel" icon="/com/intellij/uiDesigner/icons/label.png" removable="false" auto-create-binding="false" can-attach-label="false">
-        <default-constraints vsize-policy="0" hsize-policy="0" anchor="8" fill="0" />
-        <initial-values>
-          <property name="text" value="Label" />
-        </initial-values>
-      </item>
-      <item class="javax.swing.JTextField" icon="/com/intellij/uiDesigner/icons/textField.png" removable="false" auto-create-binding="true" can-attach-label="true">
-        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1">
-          <preferred-size width="150" height="-1" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JPasswordField" icon="/com/intellij/uiDesigner/icons/passwordField.png" removable="false" auto-create-binding="true" can-attach-label="true">
-        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1">
-          <preferred-size width="150" height="-1" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JFormattedTextField" icon="/com/intellij/uiDesigner/icons/formattedTextField.png" removable="false" auto-create-binding="true" can-attach-label="true">
-        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1">
-          <preferred-size width="150" height="-1" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JTextArea" icon="/com/intellij/uiDesigner/icons/textArea.png" removable="false" auto-create-binding="true" can-attach-label="true">
-        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
-          <preferred-size width="150" height="50" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JTextPane" icon="/com/intellij/uiDesigner/icons/textPane.png" removable="false" auto-create-binding="true" can-attach-label="true">
-        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
-          <preferred-size width="150" height="50" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JEditorPane" icon="/com/intellij/uiDesigner/icons/editorPane.png" removable="false" auto-create-binding="true" can-attach-label="true">
-        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
-          <preferred-size width="150" height="50" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JComboBox" icon="/com/intellij/uiDesigner/icons/comboBox.png" removable="false" auto-create-binding="true" can-attach-label="true">
-        <default-constraints vsize-policy="0" hsize-policy="2" anchor="8" fill="1" />
-      </item>
-      <item class="javax.swing.JTable" icon="/com/intellij/uiDesigner/icons/table.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
-          <preferred-size width="150" height="50" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JList" icon="/com/intellij/uiDesigner/icons/list.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="6" hsize-policy="2" anchor="0" fill="3">
-          <preferred-size width="150" height="50" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JTree" icon="/com/intellij/uiDesigner/icons/tree.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
-          <preferred-size width="150" height="50" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JTabbedPane" icon="/com/intellij/uiDesigner/icons/tabbedPane.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="3" hsize-policy="3" anchor="0" fill="3">
-          <preferred-size width="200" height="200" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JSplitPane" icon="/com/intellij/uiDesigner/icons/splitPane.png" removable="false" auto-create-binding="false" can-attach-label="false">
-        <default-constraints vsize-policy="3" hsize-policy="3" anchor="0" fill="3">
-          <preferred-size width="200" height="200" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JSpinner" icon="/com/intellij/uiDesigner/icons/spinner.png" removable="false" auto-create-binding="true" can-attach-label="true">
-        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1" />
-      </item>
-      <item class="javax.swing.JSlider" icon="/com/intellij/uiDesigner/icons/slider.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1" />
-      </item>
-      <item class="javax.swing.JSeparator" icon="/com/intellij/uiDesigner/icons/separator.png" removable="false" auto-create-binding="false" can-attach-label="false">
-        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3" />
-      </item>
-      <item class="javax.swing.JProgressBar" icon="/com/intellij/uiDesigner/icons/progressbar.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="0" hsize-policy="6" anchor="0" fill="1" />
-      </item>
-      <item class="javax.swing.JToolBar" icon="/com/intellij/uiDesigner/icons/toolbar.png" removable="false" auto-create-binding="false" can-attach-label="false">
-        <default-constraints vsize-policy="0" hsize-policy="6" anchor="0" fill="1">
-          <preferred-size width="-1" height="20" />
-        </default-constraints>
-      </item>
-      <item class="javax.swing.JToolBar$Separator" icon="/com/intellij/uiDesigner/icons/toolbarSeparator.png" removable="false" auto-create-binding="false" can-attach-label="false">
-        <default-constraints vsize-policy="0" hsize-policy="0" anchor="0" fill="1" />
-      </item>
-      <item class="javax.swing.JScrollBar" icon="/com/intellij/uiDesigner/icons/scrollbar.png" removable="false" auto-create-binding="true" can-attach-label="false">
-        <default-constraints vsize-policy="6" hsize-policy="0" anchor="0" fill="2" />
-      </item>
-    </group>
-  </component>
-</project>
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
deleted file mode 100644
index 35eb1dd..0000000
--- a/.idea/vcs.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="" vcs="Git" />
-  </component>
-</project>
\ No newline at end of file
diff --git a/.idea/workspace.xml b/.idea/workspace.xml
deleted file mode 100644
index c4c433d..0000000
--- a/.idea/workspace.xml
+++ /dev/null
@@ -1,579 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ChangeListManager">
-    <list default="true" id="e9b58600-54b5-4d2f-b404-99f7f386f3f8" name="Default" comment="">
-      <change beforePath="$PROJECT_DIR$/.travis.yml" afterPath="$PROJECT_DIR$/.travis.yml" />
-    </list>
-    <ignored path="$PROJECT_DIR$/bin/" />
-    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
-    <option name="TRACKING_ENABLED" value="true" />
-    <option name="SHOW_DIALOG" value="false" />
-    <option name="HIGHLIGHT_CONFLICTS" value="true" />
-    <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
-    <option name="LAST_RESOLUTION" value="IGNORE" />
-  </component>
-  <component name="FileEditorManager">
-    <leaf SIDE_TABS_SIZE_LIMIT_KEY="300">
-      <file leaf-file-name="Robot.java" pinned="false" current-in-tab="false">
-        <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/robot/Robot.java">
-          <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="341">
-              <caret line="20" column="13" lean-forward="false" selection-start-line="20" selection-start-column="13" selection-end-line="20" selection-end-column="13" />
-              <folding />
-            </state>
-          </provider>
-        </entry>
-      </file>
-      <file leaf-file-name="OI.java" pinned="false" current-in-tab="false">
-        <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/robot/OI.java">
-          <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="434">
-              <caret line="23" column="44" lean-forward="false" selection-start-line="23" selection-start-column="44" selection-end-line="23" selection-end-column="44" />
-              <folding />
-            </state>
-          </provider>
-        </entry>
-      </file>
-      <file leaf-file-name="buildTravis.xml" pinned="false" current-in-tab="false">
-        <entry file="file://$PROJECT_DIR$/buildTravis.xml">
-          <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="31">
-              <caret line="1" column="0" lean-forward="false" selection-start-line="1" selection-start-column="0" selection-end-line="1" selection-end-column="0" />
-              <folding />
-            </state>
-          </provider>
-        </entry>
-      </file>
-      <file leaf-file-name="build.xml" pinned="false" current-in-tab="false">
-        <entry file="file://$PROJECT_DIR$/build.xml">
-          <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="899">
-              <caret line="29" column="10" lean-forward="false" selection-start-line="29" selection-start-column="10" selection-end-line="29" selection-end-column="10" />
-              <folding />
-            </state>
-          </provider>
-        </entry>
-      </file>
-      <file leaf-file-name="build.properties" pinned="false" current-in-tab="false">
-        <entry file="file://$PROJECT_DIR$/build.properties">
-          <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="62">
-              <caret line="2" column="28" lean-forward="false" selection-start-line="2" selection-start-column="28" selection-end-line="2" selection-end-column="28" />
-              <folding />
-            </state>
-          </provider>
-        </entry>
-      </file>
-      <file leaf-file-name=".travis.yml" pinned="false" current-in-tab="true">
-        <entry file="file://$PROJECT_DIR$/.travis.yml">
-          <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="279">
-              <caret line="9" column="39" lean-forward="false" selection-start-line="9" selection-start-column="39" selection-end-line="9" selection-end-column="39" />
-              <folding />
-            </state>
-          </provider>
-        </entry>
-      </file>
-      <file leaf-file-name="install.xml" pinned="false" current-in-tab="false">
-        <entry file="file://$PROJECT_DIR$/install.xml">
-          <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="186">
-              <caret line="6" column="42" lean-forward="false" selection-start-line="6" selection-start-column="42" selection-end-line="6" selection-end-column="42" />
-              <folding />
-            </state>
-          </provider>
-        </entry>
-      </file>
-    </leaf>
-  </component>
-  <component name="FileTemplateManagerImpl">
-    <option name="RECENT_TEMPLATES">
-      <list>
-        <option value="FRCSubsystem" />
-      </list>
-    </option>
-  </component>
-  <component name="FindInProjectRecents">
-    <findStrings>
-      <find>source</find>
-      <find>mkdir</find>
-      <find>wpilib.ant.</find>
-      <find>classpath</find>
-    </findStrings>
-  </component>
-  <component name="Git.Settings">
-    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
-  </component>
-  <component name="GradleLocalSettings">
-    <option name="externalProjectsViewState">
-      <projects_view />
-    </option>
-  </component>
-  <component name="IdeDocumentHistory">
-    <option name="CHANGED_PATHS">
-      <list>
-        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/OI.java" />
-        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/RobotMap.java" />
-        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/commands/ProtoRun.java" />
-        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/Robot.java" />
-        <option value="$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/subsystems/Proto.java" />
-        <option value="$PROJECT_DIR$/buildTravis.xml" />
-        <option value="$PROJECT_DIR$/build.xml" />
-        <option value="$PROJECT_DIR$/build.properties" />
-        <option value="$PROJECT_DIR$/install.xml" />
-        <option value="$PROJECT_DIR$/.travis.yml" />
-      </list>
-    </option>
-  </component>
-  <component name="JsBuildToolGruntFileManager" detection-done="true" sorting="DEFINITION_ORDER" />
-  <component name="JsBuildToolPackageJson" detection-done="true" sorting="DEFINITION_ORDER" />
-  <component name="JsGulpfileManager">
-    <detection-done>true</detection-done>
-    <sorting>DEFINITION_ORDER</sorting>
-  </component>
-  <component name="ProjectFrameBounds" fullScreen="true">
-    <option name="width" value="1440" />
-    <option name="height" value="900" />
-  </component>
-  <component name="ProjectView">
-    <navigator currentView="ProjectPane" proportions="" version="1">
-      <flattenPackages />
-      <showMembers />
-      <showModules />
-      <showLibraryContents />
-      <hideEmptyPackages />
-      <abbreviatePackageNames />
-      <autoscrollToSource />
-      <autoscrollFromSource />
-      <sortByType />
-      <manualOrder />
-      <foldersAlwaysOnTop value="true" />
-    </navigator>
-    <panes>
-      <pane id="ProjectPane">
-        <subPane>
-          <expand>
-            <path>
-              <item name="NU18" type="b2602c69:ProjectViewProjectNode" />
-              <item name="NU18" type="462c0819:PsiDirectoryNode" />
-            </path>
-            <path>
-              <item name="NU18" type="b2602c69:ProjectViewProjectNode" />
-              <item name="NU18" type="462c0819:PsiDirectoryNode" />
-              <item name="src" type="462c0819:PsiDirectoryNode" />
-            </path>
-            <path>
-              <item name="NU18" type="b2602c69:ProjectViewProjectNode" />
-              <item name="NU18" type="462c0819:PsiDirectoryNode" />
-              <item name="src" type="462c0819:PsiDirectoryNode" />
-              <item name="robot" type="462c0819:PsiDirectoryNode" />
-            </path>
-          </expand>
-          <select />
-        </subPane>
-      </pane>
-      <pane id="AndroidView" />
-      <pane id="Scratches" />
-      <pane id="Scope" />
-      <pane id="PackagesPane" />
-    </panes>
-  </component>
-  <component name="PropertiesComponent">
-    <property name="WebServerToolWindowFactoryState" value="false" />
-    <property name="aspect.path.notification.shown" value="true" />
-    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
-    <property name="project.structure.last.edited" value="Project" />
-    <property name="project.structure.proportion" value="0.15" />
-    <property name="project.structure.side.proportion" value="0.2" />
-  </component>
-  <component name="RecentsManager">
-    <key name="CopyClassDialog.RECENTS_KEY">
-      <recent name="org.usfirst.frc.team125.proto.commands" />
-    </key>
-  </component>
-  <component name="RunDashboard">
-    <option name="ruleStates">
-      <list>
-        <RuleState>
-          <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
-        </RuleState>
-        <RuleState>
-          <option name="name" value="StatusDashboardGroupingRule" />
-        </RuleState>
-      </list>
-    </option>
-  </component>
-  <component name="RunManager">
-    <configuration default="true" type="Applet" factoryName="Applet">
-      <option name="HTML_USED" value="false" />
-      <option name="WIDTH" value="400" />
-      <option name="HEIGHT" value="300" />
-      <option name="POLICY_FILE" value="$APPLICATION_HOME_DIR$/bin/appletviewer.policy" />
-      <module />
-    </configuration>
-    <configuration default="true" type="Application" factoryName="Application">
-      <extension name="coverage" enabled="false" merge="false" sample_coverage="true" runner="idea" />
-      <option name="MAIN_CLASS_NAME" />
-      <option name="VM_PARAMETERS" />
-      <option name="PROGRAM_PARAMETERS" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
-      <option name="ALTERNATIVE_JRE_PATH_ENABLED" value="false" />
-      <option name="ALTERNATIVE_JRE_PATH" />
-      <option name="ENABLE_SWING_INSPECTOR" value="false" />
-      <option name="ENV_VARIABLES" />
-      <option name="PASS_PARENT_ENVS" value="true" />
-      <module name="" />
-      <envs />
-    </configuration>
-    <configuration default="true" type="JUnit" factoryName="JUnit">
-      <extension name="coverage" enabled="false" merge="false" sample_coverage="true" runner="idea" />
-      <module name="" />
-      <option name="ALTERNATIVE_JRE_PATH_ENABLED" value="false" />
-      <option name="ALTERNATIVE_JRE_PATH" />
-      <option name="PACKAGE_NAME" />
-      <option name="MAIN_CLASS_NAME" />
-      <option name="METHOD_NAME" />
-      <option name="TEST_OBJECT" value="class" />
-      <option name="VM_PARAMETERS" value="-ea" />
-      <option name="PARAMETERS" />
-      <option name="WORKING_DIRECTORY" value="$MODULE_DIR$" />
-      <option name="ENV_VARIABLES" />
-      <option name="PASS_PARENT_ENVS" value="true" />
-      <option name="TEST_SEARCH_SCOPE">
-        <value defaultName="singleModule" />
-      </option>
-      <envs />
-      <patterns />
-    </configuration>
-    <configuration default="true" type="Remote" factoryName="Remote">
-      <option name="USE_SOCKET_TRANSPORT" value="true" />
-      <option name="SERVER_MODE" value="false" />
-      <option name="SHMEM_ADDRESS" value="javadebug" />
-      <option name="HOST" value="localhost" />
-      <option name="PORT" value="5005" />
-    </configuration>
-    <configuration default="true" type="TestNG" factoryName="TestNG">
-      <extension name="coverage" enabled="false" merge="false" sample_coverage="true" runner="idea" />
-      <module name="" />
-      <option name="ALTERNATIVE_JRE_PATH_ENABLED" value="false" />
-      <option name="ALTERNATIVE_JRE_PATH" />
-      <option name="SUITE_NAME" />
-      <option name="PACKAGE_NAME" />
-      <option name="MAIN_CLASS_NAME" />
-      <option name="METHOD_NAME" />
-      <option name="GROUP_NAME" />
-      <option name="TEST_OBJECT" value="CLASS" />
-      <option name="VM_PARAMETERS" value="-ea" />
-      <option name="PARAMETERS" />
-      <option name="WORKING_DIRECTORY" value="$MODULE_DIR$" />
-      <option name="OUTPUT_DIRECTORY" />
-      <option name="ANNOTATION_TYPE" />
-      <option name="ENV_VARIABLES" />
-      <option name="PASS_PARENT_ENVS" value="true" />
-      <option name="TEST_SEARCH_SCOPE">
-        <value defaultName="singleModule" />
-      </option>
-      <option name="USE_DEFAULT_REPORTERS" value="false" />
-      <option name="PROPERTIES_FILE" />
-      <envs />
-      <properties />
-      <listeners />
-    </configuration>
-    <configuration default="true" type="#org.jetbrains.idea.devkit.run.PluginConfigurationType" factoryName="Plugin">
-      <module name="" />
-      <option name="VM_PARAMETERS" value="-Xmx512m -Xms256m -XX:MaxPermSize=250m -ea" />
-      <option name="PROGRAM_PARAMETERS" />
-      <predefined_log_file id="idea.log" enabled="true" />
-    </configuration>
-  </component>
-  <component name="ShelveChangesManager" show_recycled="false">
-    <option name="remove_strategy" value="false" />
-  </component>
-  <component name="SvnConfiguration">
-    <configuration />
-  </component>
-  <component name="TaskManager">
-    <task active="true" id="Default" summary="Default task">
-      <changelist id="e9b58600-54b5-4d2f-b404-99f7f386f3f8" name="Default" comment="" />
-      <created>1513904922621</created>
-      <option name="number" value="Default" />
-      <option name="presentableId" value="Default" />
-      <updated>1513904922621</updated>
-      <workItem from="1513904930290" duration="1890000" />
-      <workItem from="1514943946483" duration="37000" />
-      <workItem from="1515254835888" duration="51000" />
-      <workItem from="1515358775592" duration="4357000" />
-      <workItem from="1515452804423" duration="3963000" />
-      <workItem from="1515542806966" duration="188000" />
-      <workItem from="1516053804749" duration="1332000" />
-      <workItem from="1516318467526" duration="2268000" />
-    </task>
-    <servers />
-  </component>
-  <component name="TimeTrackingManager">
-    <option name="totallyTimeSpent" value="22917000" />
-  </component>
-  <component name="ToolWindowManager">
-    <frame x="0" y="0" width="2560" height="1600" extended-state="0" />
-    <editor active="true" />
-    <layout>
-      <window_info id="Palette" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="TODO" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="6" side_tool="false" content_ui="tabs" />
-      <window_info id="Messages" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.32941177" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
-      <window_info id="Palette&#9;" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Image Layers" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Capture Analysis" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Event Log" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="true" content_ui="tabs" />
-      <window_info id="Maven Projects" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Run" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Version Control" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
-      <window_info id="Terminal" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
-      <window_info id="Capture Tool" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Designer" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Project" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="true" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="0" side_tool="false" content_ui="combo" />
-      <window_info id="FRC" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.29036546" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
-      <window_info id="Database" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Find" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.32956812" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
-      <window_info id="Structure" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
-      <window_info id="Ant Build" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
-      <window_info id="UI Designer" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Debug" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.4" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Theme Preview" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Favorites" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="true" content_ui="tabs" />
-      <window_info id="Cvs" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="4" side_tool="false" content_ui="tabs" />
-      <window_info id="Palette" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Message" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="0" side_tool="false" content_ui="tabs" />
-      <window_info id="Commander" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.4" sideWeight="0.5" order="0" side_tool="false" content_ui="tabs" />
-      <window_info id="Event Log" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="true" content_ui="tabs" />
-      <window_info id="Maven Projects" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Capture Tool" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Hierarchy" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="2" side_tool="false" content_ui="combo" />
-      <window_info id="Designer" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Database" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Structure" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
-      <window_info id="Ant Build" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
-      <window_info id="UI Designer" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Debug" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.4" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="TODO" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="6" side_tool="false" content_ui="tabs" />
-      <window_info id="Messages" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.32941177" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
-      <window_info id="Palette&#9;" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Image Layers" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Capture Analysis" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Inspection" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.4" sideWeight="0.5" order="5" side_tool="false" content_ui="tabs" />
-      <window_info id="Run" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
-      <window_info id="Version Control" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
-      <window_info id="Terminal" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
-      <window_info id="Project" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="true" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="0" side_tool="false" content_ui="combo" />
-      <window_info id="FRC" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="true" show_stripe_button="true" weight="0.29036546" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
-      <window_info id="Theme Preview" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
-      <window_info id="Favorites" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="true" content_ui="tabs" />
-    </layout>
-  </component>
-  <component name="TypeScriptGeneratedFilesManager">
-    <option name="version" value="1" />
-  </component>
-  <component name="VcsContentAnnotationSettings">
-    <option name="myLimit" value="2678400000" />
-  </component>
-  <component name="XDebuggerManager">
-    <breakpoint-manager />
-    <watches-manager />
-  </component>
-  <component name="editorHistoryManager">
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/Robot.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/can/TalonSRX.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/can/BaseMotorController.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/ControlMode.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/IMotorController.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/commands/ProtoRun.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/subsystems/Proto.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/OI.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/RobotMap.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/Robot.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/can/TalonSRX.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/can/BaseMotorController.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/ControlMode.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/IMotorController.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/commands/ProtoRun.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/subsystems/Proto.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/OI.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/Robot.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/RobotMap.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/commands/ProtoRun.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/subsystems/Proto.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/OI.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/RobotMap.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/subsystems/Proto.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/commands/ProtoRun.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/OI.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/RobotMap.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/Robot.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/commands/ProtoRun.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/subsystems/Proto.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/subsystems/ExampleSubsystem.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/Proto.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/IMotorController.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/can/BaseMotorController.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/can/TalonSRX.java" />
-    <entry file="jar://$USER_HOME$/wpilib/java/lib/CTRE_Phoenix-sources.jar!/com/ctre/phoenix/motorcontrol/ControlMode.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/RobotMap.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/commands/ProtoRun.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/subsystems/Proto.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/OI.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/proto/Robot.java" />
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/robot/Robot.java">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="341">
-          <caret line="20" column="13" lean-forward="false" selection-start-line="20" selection-start-column="13" selection-end-line="20" selection-end-column="13" />
-          <folding />
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/Pathfinder/Pathfinder-Java/src/main/java/jaci/pathfinder/followers/EncoderFollower.java">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="-2195">
-          <caret line="10" column="13" lean-forward="false" selection-start-line="10" selection-start-column="13" selection-end-line="10" selection-end-column="13" />
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/robot/subsystems/Drivetrain.java">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="434">
-          <caret line="27" column="17" lean-forward="false" selection-start-line="27" selection-start-column="17" selection-end-line="27" selection-end-column="17" />
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team125/robot/OI.java">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="434">
-          <caret line="23" column="44" lean-forward="false" selection-start-line="23" selection-start-column="44" selection-end-line="23" selection-end-column="44" />
-          <folding />
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/buildTravis.properties">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="124">
-          <caret line="4" column="25" lean-forward="false" selection-start-line="4" selection-start-column="25" selection-end-line="4" selection-end-column="25" />
-          <folding />
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/buildTravis.xml">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="31">
-          <caret line="1" column="0" lean-forward="false" selection-start-line="1" selection-start-column="0" selection-end-line="1" selection-end-column="0" />
-          <folding />
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/build.xml">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="899">
-          <caret line="29" column="10" lean-forward="false" selection-start-line="29" selection-start-column="10" selection-end-line="29" selection-end-column="10" />
-          <folding />
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/install.xml">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="186">
-          <caret line="6" column="42" lean-forward="false" selection-start-line="6" selection-start-column="42" selection-end-line="6" selection-end-column="42" />
-          <folding />
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/build.properties">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="62">
-          <caret line="2" column="28" lean-forward="false" selection-start-line="2" selection-start-column="28" selection-end-line="2" selection-end-column="28" />
-          <folding />
-        </state>
-      </provider>
-    </entry>
-    <entry file="file://$PROJECT_DIR$/.travis.yml">
-      <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="279">
-          <caret line="9" column="39" lean-forward="false" selection-start-line="9" selection-start-column="39" selection-end-line="9" selection-end-column="39" />
-          <folding />
-        </state>
-      </provider>
-    </entry>
-  </component>
-  <component name="masterDetails">
-    <states>
-      <state key="ArtifactsStructureConfigurable.UI">
-        <settings>
-          <artifact-editor />
-          <splitter-proportions>
-            <option name="proportions">
-              <list>
-                <option value="0.2" />
-              </list>
-            </option>
-          </splitter-proportions>
-        </settings>
-      </state>
-      <state key="FacetStructureConfigurable.UI">
-        <settings>
-          <last-edited>FRC (ProtoBoard)|FRC</last-edited>
-          <splitter-proportions>
-            <option name="proportions">
-              <list>
-                <option value="0.2" />
-              </list>
-            </option>
-          </splitter-proportions>
-        </settings>
-      </state>
-      <state key="GlobalLibrariesConfigurable.UI">
-        <settings>
-          <splitter-proportions>
-            <option name="proportions">
-              <list>
-                <option value="0.2" />
-              </list>
-            </option>
-          </splitter-proportions>
-        </settings>
-      </state>
-      <state key="JdkListConfigurable.UI">
-        <settings>
-          <last-edited>1.8</last-edited>
-          <splitter-proportions>
-            <option name="proportions">
-              <list>
-                <option value="0.2" />
-              </list>
-            </option>
-          </splitter-proportions>
-        </settings>
-      </state>
-      <state key="ModuleStructureConfigurable.UI">
-        <settings>
-          <last-edited>ProtoBoard</last-edited>
-          <splitter-proportions>
-            <option name="proportions">
-              <list>
-                <option value="0.2" />
-                <option value="0.6" />
-              </list>
-            </option>
-          </splitter-proportions>
-        </settings>
-      </state>
-      <state key="ProjectLibrariesConfigurable.UI">
-        <settings>
-          <last-edited>CTRE_Phoenix-sources</last-edited>
-          <splitter-proportions>
-            <option name="proportions">
-              <list>
-                <option value="0.2" />
-              </list>
-            </option>
-          </splitter-proportions>
-        </settings>
-      </state>
-    </states>
-  </component>
-</project>
\ No newline at end of file
diff --git a/src/NU18.iml b/src/NU18.iml
new file mode 100644
index 0000000..b107a2d
--- /dev/null
+++ b/src/NU18.iml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="JAVA_MODULE" version="4">
+  <component name="NewModuleRootManager" inherit-compiler-output="true">
+    <exclude-output />
+    <content url="file://$MODULE_DIR$">
+      <sourceFolder url="file://$MODULE_DIR$" isTestSource="false" />
+    </content>
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team125/robot/OI.java b/src/org/usfirst/frc/team125/robot/OI.java
index 3f5e207..c6a51f7 100644
--- a/src/org/usfirst/frc/team125/robot/OI.java
+++ b/src/org/usfirst/frc/team125/robot/OI.java
@@ -1,74 +1,153 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
 package org.usfirst.frc.team125.robot;
 
-import org.usfirst.frc.team125.robot.commands.*;
-
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team125.robot.commands.CubeLift.OpenGrabbersCmd;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.CubeLift.TogglePinCmd;
+import org.usfirst.frc.team125.robot.commands.CubeLift.UnpunchCmd;
+import org.usfirst.frc.team125.robot.commands.DoubleLift.DropLiftCmd;
+import org.usfirst.frc.team125.robot.commands.DoubleLift.LiftLiftCmd;
+import org.usfirst.frc.team125.robot.commands.DoubleLift.ReleaseCarrierCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.ChinUpCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.*;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.JoystickMap;
 
-/**
- * This class is the glue that binds the controls on the physical operator
- * interface to the commands and command groups that allow control of the robot.
- */
 public class OI {
-	
-	//Controllers
-	public Joystick opPad = new Joystick(1);
-	public Joystick driverPad = new Joystick(0);
-
-	//Intake Controls
-	private Button forwardIntakeButton = new JoystickButton(opPad, 1);
-	private Button reverseIntakeButton = new JoystickButton(opPad, 2);
-	private Button releaseCarrierButton = new JoystickButton(opPad, 3);
-	private Button retractCarrierButton = new JoystickButton(opPad, 4);
-	private Button openClampButton = new JoystickButton(opPad, 5);
-	private Button closeClampButton = new JoystickButton(opPad, 6);
-	private Button checkSmartIntake = new JoystickButton(driverPad, 1);
-
-	private static final double STICK_DEADBAND = 0.005;
-	
-
-	public OI() {
-		//Intake
-		this.forwardIntakeButton.whileHeld(new RunIntakeForwardCMD());
-    this.reverseIntakeButton.whileHeld(new RunIntakeReverseCMD());
-        
-		//Clamp
-		this.openClampButton.whenPressed(new OpenClampCMD());
-		this.closeClampButton.whenPressed(new CloseClampCMD());
-		this.checkSmartIntake.whileHeld(new UpdateCubeSwitchCMD());
-		
-		//Carrier
-		this.releaseCarrierButton.whenPressed(new ReleaseCarrierCMD());
-		this.retractCarrierButton.whenPressed(new RetractCarrierCMD());
-	}
-
-	private static double stickDeadband(double value, double deadband, double center) {
-		return (value < (center + deadband) && value > (center - deadband)) ? center : value;
-	}
-
-	public double getDriverLeftStickY() { return stickDeadband(this.driverPad.getRawAxis(1), STICK_DEADBAND, 0.0); }
-
-	public double getDriverLeftStickX() {
-		return stickDeadband(this.driverPad.getRawAxis(0), STICK_DEADBAND, 0.0);
-	}
-
-	public double getDriverRightStickY() { return stickDeadband(this.driverPad.getRawAxis(5), STICK_DEADBAND, 0.0); }
-
-	public double getDriverRightStickX() {
-		return stickDeadband(this.driverPad.getRawAxis(4), STICK_DEADBAND, 0.0);
-	}
-
-	public double getDriverTriggerSum() {
-		return this.driverPad.getRawAxis(3) - this.driverPad.getRawAxis(2); //TODO: Check Axis (Right - Left)
-	}
-	
+
+    //Controllers
+    public Joystick driverPad = new Joystick(0);
+    public Joystick opPad = new Joystick(1);
+
+    /* Operator Control */
+    public Button runEleScale = new JoystickButton(opPad, JoystickMap.Y);
+    public Button runEleSwitch = new JoystickButton(opPad, JoystickMap.X);
+    public Button runEleIntake = new JoystickButton(opPad, JoystickMap.A);
+    public Button runElePreClimb = new JoystickButton(opPad, JoystickMap.B);
+    public Button secureCube = new JoystickButton(opPad, JoystickMap.LB);
+    public Button toggleElevatorPin = new JoystickButton(opPad, JoystickMap.L3);
+    public Button toggleIntakePistonInOrOut = new JoystickButton(opPad, JoystickMap.R3);
+    public Button runEleClimb = new JoystickButton(opPad, JoystickMap.BACK);
+    public Button climb = new JoystickButton(opPad, JoystickMap.START);
+    public Button dropLift = new JoystickButton(opPad, JoystickMap.RB);
+
+    /* Driver Control */
+    public Button score = new JoystickButton(driverPad, JoystickMap.X);
+    private Button intake = new JoystickButton(driverPad, JoystickMap.A);
+    private Button outtake = new JoystickButton(driverPad, JoystickMap.B);
+    private Button emergencyIntake = new JoystickButton(driverPad, JoystickMap.Y);
+
+
+    private static final double STICK_DEADBAND = 0.05;
+
+    private class ConditionalOIControl implements Runnable {
+        int k = 0;
+
+        @Override
+        public void run() {
+            while (true) {
+                try {
+                    Thread.sleep(20L);
+                } catch (InterruptedException e) {
+                    e.printStackTrace();
+                }
+                checkTriggers();
+                checkPOV();
+                SmartDashboard.putNumber("k counter", k);
+                k++;
+            }
+        }
+    }
+
+    public void checkTriggers() {
+        if (getOpLeftTrigger() >= 0.5) {
+            new ReleaseCarrierCmd().start();
+            new IntakeDownCmd().start();
+        }
+        if (getOpRightTrigger() >= 0.5) {
+            new LiftLiftCmd().start();
+        }
+    }
+
+    public void checkPOV() {
+        switch (opPad.getPOV()) {
+            case 0:
+                new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreScaleHigh).start();
+                break;
+            case 180:
+                new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreScaleLow).start();
+                break;
+            default:
+                break;
+        }
+    }
+
+    public OI() {
+        Thread thisNewInfiniteManaSwainIsOPWhatTheFuckRiot = new Thread(new ConditionalOIControl());
+        thisNewInfiniteManaSwainIsOPWhatTheFuckRiot.start();
+        /* Operator Control */
+        runEleScale.whenPressed(new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreScale));
+        runEleSwitch.whenPressed(new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch));
+        runEleIntake.whenPressed(new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake));
+        runElePreClimb.whenPressed(new RunToPositionMotionMagicCmd(CubeLift.Positions.PreClimb));
+        secureCube.whenPressed(new SecureCubeCmdGrp());
+        toggleElevatorPin.whenPressed(new TogglePinCmd());
+        toggleIntakePistonInOrOut.whenPressed(new ToggleIntakeSolenoidCmd());
+        runEleClimb.whenPressed(new RunToPositionMotionMagicCmd(CubeLift.Positions.ClimbingBar));
+        climb.whenPressed(new ChinUpCmdGrp());
+        dropLift.whenPressed(new DropLiftCmd());
+
+        /* Driver Control */
+        //Intake and Scoring
+        intake.whileHeld(new IntakeCmd());
+        outtake.whileHeld(new OuttakeCmd());
+        outtake.whenPressed(new OpenGrabbersCmd());
+        score.whenPressed(new ScoreCmdGrp());
+        score.whenReleased(new UnpunchCmd());
+        emergencyIntake.whileHeld(new PulseIntakeCmd());
+        emergencyIntake.whenPressed(new OpenGrabbersCmd());
+    }
+
+
+    private static double stickDeadband(double value, double deadband, double center) {
+        return (value < (center + deadband) && value > (center - deadband)) ? center : value;
+    }
+
+    public double getDriverLeftStickY() {
+        return stickDeadband(this.driverPad.getRawAxis(JoystickMap.LEFT_Y), STICK_DEADBAND, 0.0);
+    }
+
+    public double getDriverLeftStickX() {
+        return stickDeadband(this.driverPad.getRawAxis(JoystickMap.LEFT_X), STICK_DEADBAND, 0.0);
+    }
+
+    public double getDriverRightStickY() {
+        return stickDeadband(this.driverPad.getRawAxis(JoystickMap.RIGHT_Y), STICK_DEADBAND, 0.0);
+    }
+
+    public double getDriverRightStickX() {
+        return stickDeadband(this.driverPad.getRawAxis(JoystickMap.RIGHT_X), STICK_DEADBAND, 0.0);
+    }
+
+    public double getDriverTriggerSum() {
+        return this.driverPad.getRawAxis(3) - this.driverPad.getRawAxis(2); //TODO: Check Axis (Right - Left)
+    }
+
+    public double getOpRightTrigger() {
+        return this.opPad.getRawAxis(JoystickMap.RIGHT_TRIGGER);
+    }
+
+    public double getOpLeftTrigger() {
+        return this.opPad.getRawAxis(JoystickMap.LEFT_TRIGGER);
+    }
+
+    public double getOpLeftStickY() {
+        return stickDeadband(this.opPad.getRawAxis(1), STICK_DEADBAND, 0.0);
+    }
 }
 
diff --git a/src/org/usfirst/frc/team125/robot/Robot.java b/src/org/usfirst/frc/team125/robot/Robot.java
index 82a97e4..0004e0f 100644
--- a/src/org/usfirst/frc/team125/robot/Robot.java
+++ b/src/org/usfirst/frc/team125/robot/Robot.java
@@ -2,35 +2,90 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.IterativeRobot;
-import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
-import java.lang.reflect.Array;
-import org.usfirst.frc.team125.robot.subsystems.DoubleLift;
-import org.usfirst.frc.team125.robot.subsystems.Intake;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import org.usfirst.frc.team125.robot.subsystems.Drivetrain;
-
-/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.properties file in the
- * project.
- */
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.GenericAuto;
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.CenterAutos.CenterLeftAuto;
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.CenterAutos.CenterRightAuto;
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.ScaleToSwitchAutos.*;
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.SwitchOnlyAutos.LeftSideCloseSwitchAuto;
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.SwitchOnlyAutos.LeftSideFarSwitchAuto;
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.SwitchOnlyAutos.RightSideCloseSwitchAuto;
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.SwitchOnlyAutos.RightSideFarSwitchAuto;
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.TwoScaleAutos.LeftSideCloseTwoScaleAuto;
+import org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.TwoScaleAutos.LeftSideFarTwoScaleAuto;
+import org.usfirst.frc.team125.robot.subsystems.*;
+
 public class Robot extends IterativeRobot {
 
-    public static Drivetrain dt = new Drivetrain();
+    public static Drivetrain drivetrain = new Drivetrain();
     public static Intake intake = new Intake();
     public static DoubleLift doubleLift = new DoubleLift();
+    public static CubeLift cubeLift = new CubeLift();
+    public static LEDController ledController = new LEDController();
+    public static OI oi;
 
-    public char[] autoData = new char[3];
+    Command autoCommand;
 
-    public static OI oi;
+    private enum Sides {
+        Left,
+        Right,
+        Center,
+    }
+
+    private enum Autos {
+        SwitchOnly,
+        ScaleToSwitch,
+        TwoScale,
+        DoNothing,
+    }
+
+    String gameData;
+
+    SendableChooser sideSelector;
+    SendableChooser autoSelector;
+
+    //Center Autos
+    Command centerLeftAuto = new CenterLeftAuto();
+    Command centerRightAuto = new CenterRightAuto();
+
+    //Scale To Switch Autos
+    Command leftSideCloseScaleCloseSwitchAuto = new LeftSideCloseScaleCloseSwitchAuto();
+    Command leftSideCloseScaleFarSwitchAuto = new LeftSideCloseScaleFarSwitchAuto();
+    Command leftSideFarScaleCloseSwitchAuto = new LeftSideFarScaleCloseSwitchAuto();
+    Command leftSideFarScaleFarSwitchAuto = new LeftSideFarScaleFarSwitchAuto();
+    Command rightSideCloseScaleCloseSwitchAuto = new RightSideCloseScaleCloseSwitchAuto();
+    Command rightSideCloseScaleFarSwitchAuto = new RightSideCloseScaleFarSwitchAuto();
+    Command rightSideFarScaleCloseSwitchAuto = new RightSideFarScaleCloseSwitchAuto();
+    Command rightSideFarScaleFarSwitchAuto = new RightSideFarScaleFarSwitchAuto();
+
+    //Switch Only Autos
+    Command leftSideCloseSwitchAuto = new LeftSideCloseSwitchAuto();
+    Command leftSideFarSwitchAuto = new LeftSideFarSwitchAuto();
+    Command rightSideCloseSwitchAuto = new RightSideCloseSwitchAuto();
+    Command rightSideFarSwitchAuto = new RightSideFarSwitchAuto();
+
+    //Two Scale
+    Command leftTwoScaleClose = new LeftSideCloseTwoScaleAuto();
+    Command leftTwoScaleFar = new LeftSideFarTwoScaleAuto();
 
     @Override
     public void robotInit() {
         oi = new OI();
+        drivetrain.timer.start();
+        sideSelector = new SendableChooser();
+        autoSelector = new SendableChooser();
+        sideSelector.addDefault("Left", Sides.Left); // Left Side
+        sideSelector.addObject("Right", Sides.Right); // Right Side
+        sideSelector.addObject("Center", Sides.Center); // Right Side
+        autoSelector.addDefault("Switch Only", Autos.SwitchOnly);
+        autoSelector.addObject("Scale To Switch", Autos.ScaleToSwitch);
+        autoSelector.addObject("Two Scale", Autos.TwoScale);
+        autoSelector.addObject("Do Nothing", Autos.DoNothing);
+        SmartDashboard.putData("Side Selector", sideSelector);
+        SmartDashboard.putData("Auto Selector", autoSelector);
     }
 
     @Override
@@ -40,43 +95,170 @@ public void disabledInit() {
     @Override
     public void disabledPeriodic() {
         Scheduler.getInstance().run();
+        updateSmartdashboard();
     }
 
+
     @Override
     public void autonomousInit() {
-        String gameData = DriverStation.getInstance().getGameSpecificMessage();
-        //1st switch position
-        if (gameData.charAt(0) == 'L') {
-            //Auto Code for left side
+        this.drivetrain.disableRamping();
+
+        String gameDataTemp = DriverStation.getInstance().getGameSpecificMessage();
+        if (gameDataTemp != null) {
+            gameData = DriverStation.getInstance().getGameSpecificMessage().substring(0, 2);
         } else {
-            //Auto Code for right side
+            System.out.println("NO GAME DATA!");
+            gameData = "";
         }
 
-        //scale position
-        if (gameData.charAt(1) == 'L') {
-            //Auto Code for left side
+        /*
+        if (autoSelector.getSelected().equals(Autos.DoNothing)) {
+            autoCommand = new WaitCommand(15);
+        } else if (sideSelector.getSelected().equals(Sides.Center)) {
+            switch (gameData.substring(0, 1)) {
+                case "L":
+                    autoCommand = centerLeftAuto;
+                    break;
+                case "R":
+                    autoCommand = centerRightAuto;
+                    break;
+                default:
+                    autoCommand = new WaitCommand(15);
+                    break;
+            }
+        } else if (autoSelector.getSelected().equals(Autos.TwoScale)) {
+            switch (gameData.substring(1, 2)) {
+                case "L":
+                    autoCommand = leftTwoScaleClose;
+                    break;
+                case "R":
+                    autoCommand = leftTwoScaleFar;
+                    break;
+                default:
+                    autoCommand = new WaitCommand(15);
+                    break;
+            }
         } else {
-            //Auto code for right side
+            switch (gameData) {
+                case "LR": // GOOD!
+                    if (sideSelector.getSelected().equals(Sides.Left)) {
+                        if (autoSelector.getSelected().equals(Autos.SwitchOnly)) {
+                            autoCommand = leftSideCloseSwitchAuto;
+                        }
+                        if (autoSelector.getSelected().equals(Autos.ScaleToSwitch)) {
+                            autoCommand = leftSideFarScaleCloseSwitchAuto;
+                        }
+                    } else {
+                        if (autoSelector.getSelected().equals(Autos.SwitchOnly)) {
+                            autoCommand = rightSideFarSwitchAuto;
+                        }
+                        if (autoSelector.getSelected().equals(Autos.ScaleToSwitch)) {
+                            autoCommand = rightSideCloseScaleFarSwitchAuto;
+                        }
+                    }
+                    break;
+                case "RL": // GOOD!
+                    if (sideSelector.getSelected().equals(Sides.Left)) {
+                        if (autoSelector.getSelected().equals(Autos.SwitchOnly)) {
+                            autoCommand = leftSideFarSwitchAuto;
+                        }
+                        if (autoSelector.getSelected().equals(Autos.ScaleToSwitch)) {
+                            autoCommand = leftSideCloseScaleFarSwitchAuto;
+                        }
+                    } else {
+                        if (autoSelector.getSelected().equals(Autos.SwitchOnly)) {
+                            autoCommand = rightSideCloseSwitchAuto;
+                        }
+                        if (autoSelector.getSelected().equals(Autos.ScaleToSwitch)) {
+                            autoCommand = rightSideFarScaleCloseSwitchAuto;
+                        }
+                    }
+                    break;
+
+                case "LL": //GOOD!
+                    if (sideSelector.getSelected().equals(Sides.Left)) {
+                        if (autoSelector.getSelected().equals(Autos.SwitchOnly)) {
+                            autoCommand = leftSideCloseSwitchAuto;
+                        }
+                        if (autoSelector.getSelected().equals(Autos.ScaleToSwitch)) {
+                            autoCommand = leftSideCloseScaleCloseSwitchAuto;
+                        }
+                    } else {
+                        if (autoSelector.getSelected().equals(Autos.SwitchOnly)) {
+                            autoCommand = rightSideFarSwitchAuto;
+                        }
+                        if (autoSelector.getSelected().equals(Autos.ScaleToSwitch)) {
+                            autoCommand = rightSideFarScaleFarSwitchAuto;
+                        }
+                    }
+                    break;
+
+                case "RR": //GOOD!
+                    if (sideSelector.getSelected().equals(Sides.Left)) {
+                        if (autoSelector.getSelected().equals(Autos.SwitchOnly)) {
+                            autoCommand = leftSideFarSwitchAuto;
+                        }
+                        if (autoSelector.getSelected().equals(Autos.ScaleToSwitch)) {
+                            autoCommand = leftSideFarScaleFarSwitchAuto;
+                        }
+                    } else {
+                        if (autoSelector.getSelected().equals(Autos.SwitchOnly)) {
+                            autoCommand = rightSideCloseSwitchAuto;
+                        }
+                        if (autoSelector.getSelected().equals(Autos.ScaleToSwitch)) {
+                            autoCommand = rightSideCloseScaleCloseSwitchAuto;
+                        }
+                    }
+                    break;
+
+                default:
+                    autoCommand = new WaitCommand(15);
+                    break;
+            }
         }
+        */
+        autoCommand = new GenericAuto();
+        autoCommand.start();
+        SmartDashboard.putString("Chosen Auto", autoCommand.toString());
     }
 
-        @Override
-        public void autonomousPeriodic() {
-            Scheduler.getInstance().run();
-        }
+    @Override
+    public void autonomousPeriodic() {
+        Scheduler.getInstance().run();
+        updateSmartdashboard();
+    }
 
-        @Override
-        public void teleopInit() {
-        }
+    @Override
+    public void teleopInit() {
+        this.drivetrain.enableRamping();
+    }
 
-        @Override
-        public void teleopPeriodic() {
-            Scheduler.getInstance().run();
-        }
+    public void teleopPeriodic() {
+        Scheduler.getInstance().run();
+        updateSmartdashboard();
+    }
 
-        @Override
-        public void testPeriodic() {
-        }
-        
-}
+    @Override
+    public void testPeriodic() {
+
+    }
+
+    public void updateSmartdashboard() {
+        SmartDashboard.putNumber("Left dt encoder", this.drivetrain.getEncoderRawLeft());
+        SmartDashboard.putNumber("Right dt encoder", this.drivetrain.getEncoderRawRight());
+        SmartDashboard.putNumber("Left dt meters", this.drivetrain.getEncoderDistanceMetersLeft());
+        SmartDashboard.putNumber("Right dt meters", this.drivetrain.getEncoderDistanceMetersRight());
+        SmartDashboard.putNumber("Left dt speed", this.drivetrain.getLeftVelocity());
+        SmartDashboard.putNumber("Right dt speed", this.drivetrain.getRightVelocity());
+        SmartDashboard.putNumber("Gyro angle", this.drivetrain.getAngle());
+        SmartDashboard.putNumber("Elevator encoder Quadrature Position", this.cubeLift.getQuadraturePosition());
+        this.drivetrain.updateAccelDashboard();
+        this.cubeLift.updatePIDFOnDashboard();
+        this.cubeLift.updatePIDFFromDashboard();
+        SmartDashboard.putString("Elevator state", this.cubeLift.getState().toString());
+        SmartDashboard.putString("Elevator position", this.cubeLift.getPosition().toString());
+        SmartDashboard.putNumber("Gyro rate", this.drivetrain.getGyroRate());
+        SmartDashboard.putNumber("opPad POV", this.oi.opPad.getPOV());
+    }
 
+}
diff --git a/src/org/usfirst/frc/team125/robot/RobotMap.java b/src/org/usfirst/frc/team125/robot/RobotMap.java
index 9a10dee..77b4a3a 100644
--- a/src/org/usfirst/frc/team125/robot/RobotMap.java
+++ b/src/org/usfirst/frc/team125/robot/RobotMap.java
@@ -1,36 +1,46 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
 package org.usfirst.frc.team125.robot;
 
 public class RobotMap {
-	
-	//Intake Motors
-	public static final int INTAKE_RIGHT = 2;
-	public static final int INTAKE_LEFT = 3;
-	
-	//Carrier Solenoids
-	public static final int DOUBLELIFT_LIFTER = 2;
-	public static final int DOUBLELIFT_RELEASE = 3;
 
-	//Intake Solenoids
-	public static final int INTAKE_CLAMP = 0;
-	public static final int INTAKE_RETRACT_FORWARD = 4;
-	public static final int INTAKE_RETRACT_REVERSE = 1;
+    /*
+    Can Devices
+     */
+    //Drivetrain
+    public static final int LEFT_DRIVE_MAIN = 1;
+    public static final int LEFT_DRIVE_SLAVE_A = 2;
+    public static final int LEFT_DRIVE_SLAVE_B = 3;
+    public static final int RIGHT_DRIVE_MAIN = 4;
+    public static final int RIGHT_DRIVE_SLAVE_A = 5;
+    public static final int RIGHT_DRIVE_SLAVE_B = 6;
+    //Elevator
+    public static final int RIGHT_ELEVATOR_MAIN = 7;
+    public static final int RIGHT_ELEVATOR_SLAVE = 9;
+    public static final int LEFT_ELEVATOR_SLAVE_A = 8;
+    public static final int LEFT_ELEVATOR_SLAVE_B = 10;
+    //Intake
+    public static final int INTAKE_RIGHT = 21;
+    public static final int INTAKE_LEFT = 20;
+
+    /*
+    PCM Device
+     */
+    //Single Acting Solenoids
+    public static final int GRABBERS = 3;
+    public static final int RELEASE_PIN = 0;
+    public static final int DOUBLELIFT_RELEASE = 2;
+    public static final int DOUBLELIFT_LIFTER = 1;
+    public static final int PUNCHER = 6;
+    //Double Acting Solenoids
+    public static final int INTAKE_RETRACT_FORWARD = 4;
+    public static final int INTAKE_RETRACT_REVERSE = 5;
+
 
-	//Digital Input
-	public static final int INTAKE_LIMIT_SWITCH = 5;
-  
-  //Drivetrain CAN Devices TODO: Make sure these are correspondant to the actual drivetrain motors on webdash
-  public static final int LEFT_DRIVE_MAIN = 0;
-  public static final int LEFT_DRIVE_SLAVE_A = 1;
-  public static final int LEFT_DRIVE_SLAVE_B = 2;
-  public static final int RIGHT_DRIVE_MAIN = 3;
-  public static final int RIGHT_DRIVE_SLAVE_A = 4;
-  public static final int RIGHT_DRIVE_SLAVE_B = 6;
-  
-}
+    /*
+    Other
+     */
+    //Digital Input
+    public static final int INTAKE_LIMIT_SWITCH_A = 0;
+    public static final int INTAKE_LIMIT_SWITCH_B = 1;
+    //PWM
+    public static final int BLINKIN = 0;
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/CloseGrabbersCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/CloseGrabbersCmd.java
new file mode 100644
index 0000000..79946aa
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/CloseGrabbersCmd.java
@@ -0,0 +1,32 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class CloseGrabbersCmd extends Command {
+
+    public CloseGrabbersCmd() {
+        requires(Robot.cubeLift);
+    }
+
+    protected void initialize() {
+        Robot.cubeLift.closeGrabbers();
+    }
+
+    protected void execute() {
+
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+
+    }
+
+    protected void interrupted() {
+        end();
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/ElevatorDriveCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/ElevatorDriveCmd.java
new file mode 100644
index 0000000..a16efa1
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/ElevatorDriveCmd.java
@@ -0,0 +1,31 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class ElevatorDriveCmd extends Command {
+
+    public ElevatorDriveCmd() {
+        requires(Robot.cubeLift);
+        setInterruptible(true);
+    }
+
+    protected void initialize() {
+    }
+
+    protected void execute() {
+        Robot.cubeLift.directElevate(-Robot.oi.getOpLeftStickY());
+    }
+
+    protected boolean isFinished() {
+        return false;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+        Robot.cubeLift.stopElevator();
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/DrivePathCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/OpenGrabbersCmd.java
similarity index 55%
rename from src/org/usfirst/frc/team125/robot/commands/DrivePathCmd.java
rename to src/org/usfirst/frc/team125/robot/commands/CubeLift/OpenGrabbersCmd.java
index 91b1701..524b264 100644
--- a/src/org/usfirst/frc/team125/robot/commands/DrivePathCmd.java
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/OpenGrabbersCmd.java
@@ -1,27 +1,24 @@
-package org.usfirst.frc.team125.robot.commands;
+package org.usfirst.frc.team125.robot.commands.CubeLift;
 
-import org.usfirst.frc.team125.robot.Robot;
 import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
 
-/**
- *
- */
-public class DrivePathCmd extends Command {
+public class OpenGrabbersCmd extends Command {
 
-    public DrivePathCmd() {
-        requires(Robot.dt);
+    public OpenGrabbersCmd() {
+        requires(Robot.cubeLift);
     }
 
     protected void initialize() {
-        Robot.dt.pathFollow();
+        Robot.cubeLift.openGrabbers();
     }
 
     protected void execute() {
-        Robot.dt.pathFollow();
+
     }
 
     protected boolean isFinished() {
-        return false;
+        return true;
     }
 
     protected void end() {
@@ -29,5 +26,4 @@ protected void end() {
 
     protected void interrupted() {
     }
-
 }
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/PinCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/PinCmd.java
new file mode 100644
index 0000000..03c6560
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/PinCmd.java
@@ -0,0 +1,33 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class PinCmd extends Command {
+
+    public PinCmd() {
+        requires(Robot.cubeLift);
+    }
+
+    protected void initialize() {
+        Robot.cubeLift.pin();
+    }
+
+    protected void execute() {
+
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+
+    }
+
+    protected void interrupted() {
+
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/PunchCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/PunchCmd.java
new file mode 100644
index 0000000..b98236e
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/PunchCmd.java
@@ -0,0 +1,29 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class PunchCmd extends Command {
+
+    public PunchCmd() {
+        requires(Robot.cubeLift);
+    }
+
+    protected void initialize() {
+        Robot.cubeLift.punch();
+    }
+
+    protected void execute() {
+
+    }
+
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/RunToPositionMotionMagicCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/RunToPositionMotionMagicCmd.java
new file mode 100644
index 0000000..8b8c47a
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/RunToPositionMotionMagicCmd.java
@@ -0,0 +1,65 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+
+public class RunToPositionMotionMagicCmd extends Command {
+
+    private CubeLift.Positions position;
+    private boolean started = false;
+
+    public RunToPositionMotionMagicCmd(CubeLift.Positions pos) {
+        requires(Robot.cubeLift);
+        this.position = pos;
+    }
+
+    protected void initialize() {
+        Robot.cubeLift.stopElevator();
+        Robot.cubeLift.closeGrabbers();
+        Robot.ledController.setRunningMotionMagic(true);
+        switch (position) {
+            case Intake:
+                Robot.intake.intakePistonDown();
+                break;
+            case ScoreSwitch:
+            case ScoreScale:
+                Robot.intake.intakePistonDown();
+                break;
+            case PreClimb:
+            case ClimbingBar:
+                Robot.intake.intakePistonUp();
+                break;
+            case ChinUp:
+                Robot.cubeLift.openGrabbers();
+                Robot.intake.intakePistonDown();
+                break;
+            default:
+                break;
+
+        }
+        Robot.cubeLift.startMotionMagic(this.position);
+    }
+
+    protected void execute() {
+        Robot.ledController.setRunningMotionMagic(true);
+        if (!started) {
+            started = true;
+        } else {
+            Robot.cubeLift.checkMotionMagicTermination(this.position);
+        }
+    }
+
+    protected boolean isFinished() {
+        return Robot.cubeLift.getState() == CubeLift.LiftState.Stationary && Robot.cubeLift.getPosition() == this.position;
+    }
+
+    protected void end() {
+        Robot.ledController.setRunningMotionMagic(false);
+        Robot.cubeLift.stopElevator();
+    }
+
+    protected void interrupted() {
+        end();
+    }
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/ToggleGrabbersCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/ToggleGrabbersCmd.java
new file mode 100644
index 0000000..7079fd0
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/ToggleGrabbersCmd.java
@@ -0,0 +1,32 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class ToggleGrabbersCmd extends Command {
+
+    public ToggleGrabbersCmd() {
+        requires(Robot.cubeLift);
+    }
+
+    protected void initialize() {
+        Robot.cubeLift.toggleGrabbers();
+    }
+
+    protected void execute() {
+
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+
+    }
+
+    protected void interrupted() {
+
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/TogglePinCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/TogglePinCmd.java
new file mode 100644
index 0000000..a2a1264
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/TogglePinCmd.java
@@ -0,0 +1,32 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class TogglePinCmd extends Command {
+
+    public TogglePinCmd() {
+        requires(Robot.cubeLift);
+    }
+
+    protected void initialize() {
+        Robot.cubeLift.togglePin();
+    }
+
+    protected void execute() {
+
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+
+    }
+
+    protected void interrupted() {
+
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/TogglePuncherCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/TogglePuncherCmd.java
new file mode 100644
index 0000000..f8d1f27
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/TogglePuncherCmd.java
@@ -0,0 +1,32 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class TogglePuncherCmd extends Command {
+
+    public TogglePuncherCmd() {
+        requires(Robot.cubeLift);
+    }
+
+    protected void initialize() {
+        Robot.cubeLift.togglePunch();
+    }
+
+    protected void execute() {
+
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+
+    }
+
+    protected void interrupted() {
+
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/UnpinCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/UnpinCmd.java
new file mode 100644
index 0000000..c72895b
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/UnpinCmd.java
@@ -0,0 +1,33 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class UnpinCmd extends Command {
+
+    public UnpinCmd() {
+        requires(Robot.cubeLift);
+    }
+
+    protected void initialize() {
+        Robot.cubeLift.unpin();
+    }
+
+    protected void execute() {
+
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+
+    }
+
+    protected void interrupted() {
+
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/CubeLift/UnpunchCmd.java b/src/org/usfirst/frc/team125/robot/commands/CubeLift/UnpunchCmd.java
new file mode 100644
index 0000000..d138dff
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/CubeLift/UnpunchCmd.java
@@ -0,0 +1,29 @@
+package org.usfirst.frc.team125.robot.commands.CubeLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class UnpunchCmd extends Command {
+
+    public UnpunchCmd() {
+        requires(Robot.cubeLift);
+    }
+
+    protected void initialize() {
+        Robot.cubeLift.unpunch();
+    }
+
+    protected void execute() {
+
+    }
+
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/DoubleLift/DropLiftCmd.java b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/DropLiftCmd.java
new file mode 100644
index 0000000..2160247
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/DropLiftCmd.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team125.robot.commands.DoubleLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class DropLiftCmd extends Command {
+
+    public DropLiftCmd() {
+        requires(Robot.doubleLift);
+    }
+
+    protected void initialize() {
+        Robot.doubleLift.dropLift();
+    }
+
+    protected void execute() {
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/DoubleLift/LiftLiftCmd.java b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/LiftLiftCmd.java
new file mode 100644
index 0000000..cacbe4a
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/LiftLiftCmd.java
@@ -0,0 +1,29 @@
+package org.usfirst.frc.team125.robot.commands.DoubleLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class LiftLiftCmd extends Command {
+
+    public LiftLiftCmd() {
+        requires(Robot.doubleLift);
+    }
+
+    protected void initialize() {
+        Robot.doubleLift.liftLift();
+    }
+
+    protected void execute() {
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/DoubleLift/ReleaseCarrierCmd.java b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/ReleaseCarrierCmd.java
new file mode 100644
index 0000000..eecf9b8
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/ReleaseCarrierCmd.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team125.robot.commands.DoubleLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class ReleaseCarrierCmd extends Command {
+
+    public ReleaseCarrierCmd() {
+        requires(Robot.doubleLift);
+    }
+
+    protected void initialize() {
+        Robot.doubleLift.releaseCarrier();
+    }
+
+    protected void execute() {
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}
+
diff --git a/src/org/usfirst/frc/team125/robot/commands/DoubleLift/ToggleLiftCmd.java b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/ToggleLiftCmd.java
new file mode 100644
index 0000000..ff0278b
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/ToggleLiftCmd.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team125.robot.commands.DoubleLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class ToggleLiftCmd extends Command {
+
+    public ToggleLiftCmd() {
+        requires(Robot.doubleLift);
+    }
+
+    protected void initialize() {
+        Robot.doubleLift.toggleLift();
+    }
+
+    protected void execute() {
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}
+
diff --git a/src/org/usfirst/frc/team125/robot/commands/DoubleLift/ToggleReleaserCmd.java b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/ToggleReleaserCmd.java
new file mode 100644
index 0000000..610fc14
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/ToggleReleaserCmd.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team125.robot.commands.DoubleLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class ToggleReleaserCmd extends Command {
+
+    public ToggleReleaserCmd() {
+        requires(Robot.doubleLift);
+    }
+
+    protected void initialize() {
+        Robot.doubleLift.toggleRelease();
+    }
+
+    protected void execute() {
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}
+
diff --git a/src/org/usfirst/frc/team125/robot/commands/DoubleLift/UnreleaseCarrierCmd.java b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/UnreleaseCarrierCmd.java
new file mode 100644
index 0000000..2b69d5f
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/DoubleLift/UnreleaseCarrierCmd.java
@@ -0,0 +1,29 @@
+package org.usfirst.frc.team125.robot.commands.DoubleLift;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class UnreleaseCarrierCmd extends Command {
+
+    public UnreleaseCarrierCmd() {
+        requires(Robot.doubleLift);
+    }
+
+    protected void initialize() {
+        Robot.doubleLift.unreleaseCarrier();
+    }
+
+    protected void execute() {
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/DriveArcadeCmd.java b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DriveArcadeCmd.java
similarity index 57%
rename from src/org/usfirst/frc/team125/robot/commands/DriveArcadeCmd.java
rename to src/org/usfirst/frc/team125/robot/commands/Drivetrain/DriveArcadeCmd.java
index b916642..8c42c3d 100644
--- a/src/org/usfirst/frc/team125/robot/commands/DriveArcadeCmd.java
+++ b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DriveArcadeCmd.java
@@ -1,4 +1,4 @@
-package org.usfirst.frc.team125.robot.commands;
+package org.usfirst.frc.team125.robot.commands.Drivetrain;
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team125.robot.Robot;
@@ -9,18 +9,20 @@
 public class DriveArcadeCmd extends Command {
 
     public DriveArcadeCmd() {
-        requires(Robot.dt);
+        requires(Robot.drivetrain);
     }
 
     protected void initialize() {
-        Robot.dt.disableBreakMode();
+        Robot.drivetrain.enableBreakMode();
     }
 
     protected void execute() {
-        Robot.dt.driveArcade(Robot.oi.getDriverTriggerSum(), Robot.oi.getDriverLeftStickX());
+        Robot.drivetrain.driveArcade(Robot.oi.getDriverTriggerSum(), Robot.oi.getDriverLeftStickX());
     }
 
-    protected boolean isFinished() { return false; }
+    protected boolean isFinished() {
+        return false;
+    }
 
     protected void end() {
         //Left empty intentionally
diff --git a/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DriveArcadeWithHoldHeadingCmd.java b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DriveArcadeWithHoldHeadingCmd.java
new file mode 100644
index 0000000..537f942
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DriveArcadeWithHoldHeadingCmd.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team125.robot.commands.Drivetrain;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+/**
+ *
+ */
+public class DriveArcadeWithHoldHeadingCmd extends Command {
+
+    public DriveArcadeWithHoldHeadingCmd() {
+        requires(Robot.drivetrain);
+    }
+
+    protected void initialize() {
+        Robot.drivetrain.enableBreakMode();
+        Robot.drivetrain.resetGyro();
+    }
+
+    protected void execute() {
+        Robot.drivetrain.driveHoldHeading(Robot.oi.getDriverTriggerSum());
+    }
+
+    protected boolean isFinished() {
+        return false;
+    }
+
+    protected void end() {
+        Robot.drivetrain.resetLastHeadingError();
+    }
+
+    protected void interrupted() {
+        end();
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DrivePathCmd.java b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DrivePathCmd.java
new file mode 100644
index 0000000..a2f2886
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DrivePathCmd.java
@@ -0,0 +1,44 @@
+package org.usfirst.frc.team125.robot.commands.Drivetrain;
+
+import edu.wpi.first.wpilibj.command.Command;
+import jaci.pathfinder.Waypoint;
+import jaci.pathfinder.followers.EncoderFollower;
+import org.usfirst.frc.team125.robot.Robot;
+
+/**
+ *
+ */
+public class DrivePathCmd extends Command {
+
+    Waypoint[] path;
+    EncoderFollower[] followers;
+
+    public DrivePathCmd(Waypoint[] path) {
+        requires(Robot.drivetrain);
+        this.path = path;
+        setInterruptible(false);
+        followers = Robot.drivetrain.pathSetup(path);
+    }
+
+    protected void initialize() {
+        Robot.drivetrain.resetForPath();
+        Robot.drivetrain.pathFollow(followers, false);
+    }
+
+    protected void execute() {
+        Robot.drivetrain.pathFollow(followers, false);
+    }
+
+    protected boolean isFinished() {
+        return Robot.drivetrain.getIsProfileFinished();
+    }
+
+    protected void end() {
+        Robot.drivetrain.drive(0., 0.);
+    }
+
+    protected void interrupted() {
+        end();
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DrivePathReverseCmd.java b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DrivePathReverseCmd.java
new file mode 100644
index 0000000..ced784e
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DrivePathReverseCmd.java
@@ -0,0 +1,44 @@
+package org.usfirst.frc.team125.robot.commands.Drivetrain;
+
+import edu.wpi.first.wpilibj.command.Command;
+import jaci.pathfinder.Waypoint;
+import jaci.pathfinder.followers.EncoderFollower;
+import org.usfirst.frc.team125.robot.Robot;
+
+/**
+ *
+ */
+public class DrivePathReverseCmd extends Command {
+
+    Waypoint[] path;
+    EncoderFollower[] followers;
+
+    public DrivePathReverseCmd(Waypoint[] path) {
+        requires(Robot.drivetrain);
+        this.path = path;
+        setInterruptible(false);
+        followers = Robot.drivetrain.pathSetup(path);
+    }
+
+    protected void initialize() {
+        Robot.drivetrain.resetForPath();
+        Robot.drivetrain.pathFollow(followers, true);
+    }
+
+    protected void execute() {
+        Robot.drivetrain.pathFollow(followers, true);
+    }
+
+    protected boolean isFinished() {
+        return Robot.drivetrain.getIsProfileFinished();
+    }
+
+    protected void end() {
+        Robot.drivetrain.drive(0., 0.);
+    }
+
+    protected void interrupted() {
+        end();
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/DriveTankCmd.java b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DriveTankCmd.java
similarity index 57%
rename from src/org/usfirst/frc/team125/robot/commands/DriveTankCmd.java
rename to src/org/usfirst/frc/team125/robot/commands/Drivetrain/DriveTankCmd.java
index ddf8952..b403f76 100644
--- a/src/org/usfirst/frc/team125/robot/commands/DriveTankCmd.java
+++ b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/DriveTankCmd.java
@@ -1,8 +1,7 @@
-package org.usfirst.frc.team125.robot.commands;
+package org.usfirst.frc.team125.robot.commands.Drivetrain;
 
-import org.usfirst.frc.team125.robot.Robot;
-import org.usfirst.frc.team125.robot.OI;
 import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
 
 /**
  *
@@ -10,18 +9,20 @@
 public class DriveTankCmd extends Command {
 
     public DriveTankCmd() {
-        requires(Robot.dt);
+        requires(Robot.drivetrain);
     }
 
     protected void initialize() {
-        Robot.dt.disableBreakMode();
+        Robot.drivetrain.enableBreakMode();
     }
 
     protected void execute() {
-        Robot.dt.drive(Robot.oi.getDriverLeftStickY(), Robot.oi.getDriverRightStickY());
+        Robot.drivetrain.drive(Robot.oi.getDriverLeftStickY(), Robot.oi.getDriverRightStickY());
     }
 
-    protected boolean isFinished() { return false; }
+    protected boolean isFinished() {
+        return false;
+    }
 
     protected void end() {
         //Left empty intentionally
diff --git a/src/org/usfirst/frc/team125/robot/commands/Drivetrain/TurnToAngleCmd.java b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/TurnToAngleCmd.java
new file mode 100644
index 0000000..1bd9e5a
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Drivetrain/TurnToAngleCmd.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team125.robot.commands.Drivetrain;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+/**
+ *
+ */
+public class TurnToAngleCmd extends Command {
+
+    double angle;
+    boolean isFinished = false;
+
+    public TurnToAngleCmd(double angle) {
+        requires(Robot.drivetrain);
+        this.angle = angle;
+
+    }
+
+    protected void initialize() {
+        Robot.drivetrain.enableBreakMode();
+        Robot.drivetrain.resetGyro();
+        Robot.drivetrain.resetLastHeadingError();
+    }
+
+    protected void execute() {
+        isFinished = Robot.drivetrain.turnToAngle(angle);
+    }
+
+    protected boolean isFinished() {
+        return isFinished;
+    }
+
+    protected void end() {
+        Robot.drivetrain.resetLastHeadingError();
+        Robot.drivetrain.drive(0.0, 0.0);
+    }
+
+    protected void interrupted() {
+        end();
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/AutoLiftCmdGrp.java b/src/org/usfirst/frc/team125/robot/commands/Groups/AutoLiftCmdGrp.java
new file mode 100644
index 0000000..1bd1a4f
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/AutoLiftCmdGrp.java
@@ -0,0 +1,15 @@
+package org.usfirst.frc.team125.robot.commands.Groups;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+
+public class AutoLiftCmdGrp extends CommandGroup {
+
+    public AutoLiftCmdGrp(double timeBuffer, CubeLift.Positions pos) {
+        addSequential(new RunToPositionMotionMagicCmd(CubeLift.Positions.Driving), 1.5);
+        addSequential(new WaitCommand(timeBuffer));
+        addSequential(new RunToPositionMotionMagicCmd(pos), 3);
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/GenericAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/GenericAuto.java
new file mode 100644
index 0000000..7682924
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/GenericAuto.java
@@ -0,0 +1,20 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeCmd;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths.LeftSideCloseScaleFarSwitchPaths;
+
+public class GenericAuto extends CommandGroup {
+    Command genericCmd = new DrivePathReverseCmd(LeftSideCloseScaleFarSwitchPaths.reverse_kTurnToSwitch1A);
+    Command genericCmd2 = new DrivePathCmd(LeftSideCloseScaleFarSwitchPaths.kTurnToSwitch1B);
+    Command genericCmd3 = new IntakeCmd();
+
+    public GenericAuto() {
+        //addSequential(genericCmd);
+        addSequential(genericCmd3);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/CenterAutos/CenterLeftAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/CenterAutos/CenterLeftAuto.java
new file mode 100644
index 0000000..f343197
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/CenterAutos/CenterLeftAuto.java
@@ -0,0 +1,53 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.CenterAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.CenterPaths.CenterLeftPath;
+
+public class CenterLeftAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevator = new AutoLiftCmdGrp(0.1, CubeLift.Positions.ScoreSwitch);
+    Command driveToSwitch = new DrivePathCmd(CenterLeftPath.toSwitch);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringEleToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveBackToLine = new DrivePathReverseCmd(CenterLeftPath.reverse_goBack);
+    Command driveToCube = new DrivePathCmd(CenterLeftPath.toCube);
+    Command intakeCube = new IntakeCmd();
+    Command backOffCube = new DrivePathReverseCmd(CenterLeftPath.reverse_backOffCube);
+    Command liftElevatorAgain = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command driveToSwitchAgain = new DrivePathCmd(CenterLeftPath.toSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+
+    public CenterLeftAuto() {
+        /*
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevator);
+        addSequential(driveToSwitch);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringEleToIntake);
+        addSequential(driveBackToLine);
+        addParallel(driveToCube);
+        addSequential(intakeCube);
+        addSequential(backOffCube);
+        addParallel(liftElevatorAgain);
+        addSequential(driveToSwitchAgain);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/CenterAutos/CenterRightAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/CenterAutos/CenterRightAuto.java
new file mode 100644
index 0000000..20478e1
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/CenterAutos/CenterRightAuto.java
@@ -0,0 +1,52 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.CenterAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.CenterPaths.CenterRightPath;
+
+public class CenterRightAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevator = new AutoLiftCmdGrp(0.1, CubeLift.Positions.ScoreSwitch);
+    Command driveToSwitch = new DrivePathCmd(CenterRightPath.toSwitch);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringEleToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveBackToLine = new DrivePathReverseCmd(CenterRightPath.reverse_goBack);
+    Command driveToCube = new DrivePathCmd(CenterRightPath.toCube);
+    Command intakeCube = new IntakeCmd();
+    Command backOffCube = new DrivePathReverseCmd(CenterRightPath.reverse_backOffCube);
+    Command liftElevatorAgain = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command driveToSwitchAgain = new DrivePathCmd(CenterRightPath.toSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public CenterRightAuto() {
+         /*
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevator);
+        addSequential(driveToSwitch);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringEleToIntake);
+        addSequential(driveBackToLine);
+        addParallel(driveToCube);
+        addSequential(intakeCube);
+        addSequential(backOffCube);
+        addParallel(liftElevatorAgain);
+        addSequential(driveToSwitchAgain);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideCloseScaleCloseSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideCloseScaleCloseSwitchAuto.java
new file mode 100644
index 0000000..8cd8c52
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideCloseScaleCloseSwitchAuto.java
@@ -0,0 +1,49 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.ScaleToSwitchAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.AutonomousIntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths.LeftSideCloseScaleCloseSwitchPaths;
+
+public class LeftSideCloseScaleCloseSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(LeftSideCloseScaleCloseSwitchPaths.toScale);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(LeftSideCloseScaleCloseSwitchPaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new AutonomousIntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(LeftSideCloseScaleCloseSwitchPaths.kTurnToSwitch1B);
+    Command secureCubeAgain = new SecureCubeCmdGrp();
+    Command liftElevatorToSwitch = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public LeftSideCloseScaleCloseSwitchAuto() {
+        /*addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addParallel(intakeCube, 3);
+        addSequential(driveToSwitchB);
+        addSequential(secureCubeAgain);
+        addSequential(liftElevatorToSwitch, 3);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideCloseScaleFarSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideCloseScaleFarSwitchAuto.java
new file mode 100644
index 0000000..d7143e5
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideCloseScaleFarSwitchAuto.java
@@ -0,0 +1,50 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.ScaleToSwitchAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.AutonomousIntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths.LeftSideCloseScaleFarSwitchPaths;
+
+public class LeftSideCloseScaleFarSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(LeftSideCloseScaleFarSwitchPaths.toScale);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(LeftSideCloseScaleFarSwitchPaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new AutonomousIntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(LeftSideCloseScaleFarSwitchPaths.kTurnToSwitch1B);
+    Command secureCubeAgain = new SecureCubeCmdGrp();
+    Command liftElevatorToSwitch = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public LeftSideCloseScaleFarSwitchAuto() {
+        /*
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addParallel(intakeCube, 5);
+        addSequential(driveToSwitchB);
+        addSequential(secureCubeAgain);
+        addSequential(liftElevatorToSwitch, 3);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideFarScaleCloseSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideFarScaleCloseSwitchAuto.java
new file mode 100644
index 0000000..c7f11b1
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideFarScaleCloseSwitchAuto.java
@@ -0,0 +1,51 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.ScaleToSwitchAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.AutonomousIntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths.LeftSideFarScaleCloseSwitchPaths;
+
+public class LeftSideFarScaleCloseSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(2.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(LeftSideFarScaleCloseSwitchPaths.toScale);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(LeftSideFarScaleCloseSwitchPaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new AutonomousIntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(LeftSideFarScaleCloseSwitchPaths.kTurnToSwitch1B);
+    Command secureCubeAgain = new SecureCubeCmdGrp();
+    Command liftElevatorToSwitch = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public LeftSideFarScaleCloseSwitchAuto() {
+        /*
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addSequential(new WaitCommand(1));
+        addParallel(intakeCube, 5);
+        addSequential(driveToSwitchB);
+        addSequential(secureCubeAgain);
+        addSequential(liftElevatorToSwitch, 3);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideFarScaleFarSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideFarScaleFarSwitchAuto.java
new file mode 100644
index 0000000..d8d0ef6
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/LeftSideFarScaleFarSwitchAuto.java
@@ -0,0 +1,51 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.ScaleToSwitchAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.AutonomousIntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths.LeftSideFarScaleFarSwitchPaths;
+
+public class LeftSideFarScaleFarSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(2.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(LeftSideFarScaleFarSwitchPaths.toScale);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(LeftSideFarScaleFarSwitchPaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new AutonomousIntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(LeftSideFarScaleFarSwitchPaths.kTurnToSwitch1B);
+    Command secureCubeAgain = new SecureCubeCmdGrp();
+    Command liftElevatorToSwitch = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public LeftSideFarScaleFarSwitchAuto() {
+        /*
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addSequential(new WaitCommand(0.1));
+        addParallel(intakeCube, 3);
+        addSequential(driveToSwitchB);
+        addSequential(secureCubeAgain);
+        addSequential(liftElevatorToSwitch, 3);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideCloseScaleCloseSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideCloseScaleCloseSwitchAuto.java
new file mode 100644
index 0000000..f6833a1
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideCloseScaleCloseSwitchAuto.java
@@ -0,0 +1,50 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.ScaleToSwitchAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.AutonomousIntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths.RightSideCloseScaleCloseSwitchPaths;
+
+public class RightSideCloseScaleCloseSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(RightSideCloseScaleCloseSwitchPaths.toScale);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(RightSideCloseScaleCloseSwitchPaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new AutonomousIntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(RightSideCloseScaleCloseSwitchPaths.kTurnToSwitch1B);
+    Command secureCubeAgain = new SecureCubeCmdGrp();
+    Command liftElevatorToSwitch = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public RightSideCloseScaleCloseSwitchAuto() {
+        /*
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addParallel(intakeCube, 3);
+        addSequential(driveToSwitchB);
+        addSequential(secureCubeAgain);
+        addSequential(liftElevatorToSwitch, 3);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideCloseScaleFarSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideCloseScaleFarSwitchAuto.java
new file mode 100644
index 0000000..7a8afec
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideCloseScaleFarSwitchAuto.java
@@ -0,0 +1,50 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.ScaleToSwitchAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.AutonomousIntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths.RightSideCloseScaleFarSwitchPaths;
+
+public class RightSideCloseScaleFarSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(RightSideCloseScaleFarSwitchPaths.toScale);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(RightSideCloseScaleFarSwitchPaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new AutonomousIntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(RightSideCloseScaleFarSwitchPaths.kTurnToSwitch1B);
+    Command secureCubeAgain = new SecureCubeCmdGrp();
+    Command liftElevatorToSwitch = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public RightSideCloseScaleFarSwitchAuto() {
+        /*
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addParallel(intakeCube, 5);
+        addSequential(driveToSwitchB);
+        addSequential(secureCubeAgain);
+        addSequential(liftElevatorToSwitch, 3);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideFarScaleCloseSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideFarScaleCloseSwitchAuto.java
new file mode 100644
index 0000000..9e05102
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideFarScaleCloseSwitchAuto.java
@@ -0,0 +1,51 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.ScaleToSwitchAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.AutonomousIntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths.RightSideFarScaleCloseSwitchPaths;
+
+public class RightSideFarScaleCloseSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(RightSideFarScaleCloseSwitchPaths.toScale);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(RightSideFarScaleCloseSwitchPaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new AutonomousIntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(RightSideFarScaleCloseSwitchPaths.kTurnToSwitch1B);
+    Command secureCubeAgain = new SecureCubeCmdGrp();
+    Command liftElevatorToSwitch = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public RightSideFarScaleCloseSwitchAuto() {
+        /*
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addSequential(new WaitCommand(1));
+        addParallel(intakeCube, 5);
+        addSequential(driveToSwitchB);
+        addSequential(secureCubeAgain);
+        addSequential(liftElevatorToSwitch, 3);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideFarScaleFarSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideFarScaleFarSwitchAuto.java
new file mode 100644
index 0000000..1996ced
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/ScaleToSwitchAutos/RightSideFarScaleFarSwitchAuto.java
@@ -0,0 +1,51 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.ScaleToSwitchAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.AutonomousIntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths.RightSideFarScaleFarSwitchPaths;
+
+public class RightSideFarScaleFarSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(RightSideFarScaleFarSwitchPaths.toScale);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(RightSideFarScaleFarSwitchPaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new AutonomousIntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(RightSideFarScaleFarSwitchPaths.kTurnToSwitch1B);
+    Command secureCubeAgain = new SecureCubeCmdGrp();
+    Command liftElevatorToSwitch = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreSwitch);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public RightSideFarScaleFarSwitchAuto() {
+        /*
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addSequential(new WaitCommand(0.1));
+        addParallel(intakeCube, 3);
+        addSequential(driveToSwitchB);
+        addSequential(secureCubeAgain);
+        addSequential(liftElevatorToSwitch, 3);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/LeftSideCloseSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/LeftSideCloseSwitchAuto.java
new file mode 100644
index 0000000..a64417e
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/LeftSideCloseSwitchAuto.java
@@ -0,0 +1,33 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.SwitchOnlyAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.SwitchOnlyPaths.LeftSideCloseSwitchPaths;
+
+public class LeftSideCloseSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevator = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreSwitch);
+    Command driveToBeforeSwitch = new DrivePathCmd(LeftSideCloseSwitchPaths.toBeforeSwitchTurn);
+    Command driveToSwitch = new DrivePathCmd(LeftSideCloseSwitchPaths.turnToSwitch);
+    Command scoreCube = new ScoreCmdGrp();
+
+
+    public LeftSideCloseSwitchAuto() {
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        addParallel(liftElevator);
+        addSequential(driveToBeforeSwitch);
+        addSequential(driveToSwitch);
+        addSequential(scoreCube);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/LeftSideFarSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/LeftSideFarSwitchAuto.java
new file mode 100644
index 0000000..d3b26d8
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/LeftSideFarSwitchAuto.java
@@ -0,0 +1,33 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.SwitchOnlyAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.SwitchOnlyPaths.LeftSideFarSwitchPaths;
+
+public class LeftSideFarSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevator = new AutoLiftCmdGrp(0.75, CubeLift.Positions.ScoreSwitch);
+    Command driveToBeforeSwitch = new DrivePathCmd(LeftSideFarSwitchPaths.toBeforeSwitchTurn);
+    Command driveToSwitch = new DrivePathCmd(LeftSideFarSwitchPaths.turnToSwitch);
+    Command scoreCube = new ScoreCmdGrp();
+
+
+    public LeftSideFarSwitchAuto() {
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        addParallel(liftElevator);
+        addSequential(driveToBeforeSwitch);
+        addSequential(driveToSwitch);
+        addSequential(scoreCube);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/RightSideCloseSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/RightSideCloseSwitchAuto.java
new file mode 100644
index 0000000..398cb47
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/RightSideCloseSwitchAuto.java
@@ -0,0 +1,33 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.SwitchOnlyAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.SwitchOnlyPaths.RightSideCloseSwitchPaths;
+
+public class RightSideCloseSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevator = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreSwitch);
+    Command driveToBeforeSwitch = new DrivePathCmd(RightSideCloseSwitchPaths.toBeforeSwitchTurn);
+    Command driveToSwitch = new DrivePathCmd(RightSideCloseSwitchPaths.turnToSwitch);
+    Command scoreCube = new ScoreCmdGrp();
+
+
+    public RightSideCloseSwitchAuto() {
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        addParallel(liftElevator);
+        addSequential(driveToBeforeSwitch);
+        addSequential(driveToSwitch);
+        addSequential(scoreCube);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/RightSideFarSwitchAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/RightSideFarSwitchAuto.java
new file mode 100644
index 0000000..53b5efe
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/SwitchOnlyAutos/RightSideFarSwitchAuto.java
@@ -0,0 +1,32 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.SwitchOnlyAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.SwitchOnlyPaths.RightSideFarSwitchPaths;
+
+public class RightSideFarSwitchAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevator = new AutoLiftCmdGrp(0.75, CubeLift.Positions.ScoreSwitch);
+    Command driveToBeforeSwitch = new DrivePathCmd(RightSideFarSwitchPaths.toBeforeSwitchTurn);
+    Command driveToSwitch = new DrivePathCmd(RightSideFarSwitchPaths.turnToSwitch);
+    Command scoreCube = new ScoreCmdGrp();
+
+
+    public RightSideFarSwitchAuto() {
+        addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        addParallel(liftElevator);
+        addSequential(driveToBeforeSwitch);
+        addSequential(driveToSwitch);
+        addSequential(scoreCube);
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/TwoScaleAutos/LeftSideCloseTwoScaleAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/TwoScaleAutos/LeftSideCloseTwoScaleAuto.java
new file mode 100644
index 0000000..fa89c10
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/TwoScaleAutos/LeftSideCloseTwoScaleAuto.java
@@ -0,0 +1,51 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.TwoScaleAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.TwoScale.LeftSideCloseTwoScalePaths;
+
+public class LeftSideCloseTwoScaleAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(LeftSideCloseTwoScalePaths.toScale);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(LeftSideCloseTwoScalePaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new IntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(LeftSideCloseTwoScalePaths.kTurnToSwitch1B);
+    Command liftElevatorToScaleAgain = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreScale);
+    Command driveToScaleA = new DrivePathReverseCmd(LeftSideCloseTwoScalePaths.reverse_kTurnToScaleA);
+    Command driveToScaleB = new DrivePathCmd(LeftSideCloseTwoScalePaths.kTurnToScaleB);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public LeftSideCloseTwoScaleAuto() {
+        /*addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addParallel(driveToSwitchB);
+        addSequential(intakeCube);
+        addParallel(liftElevatorToScaleAgain, 3);
+        addSequential(driveToScaleA);
+        addSequential(driveToScaleB);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/TwoScaleAutos/LeftSideFarTwoScaleAuto.java b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/TwoScaleAutos/LeftSideFarTwoScaleAuto.java
new file mode 100644
index 0000000..510faab
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/Autos/PalmettoAutos/TwoScaleAutos/LeftSideFarTwoScaleAuto.java
@@ -0,0 +1,51 @@
+package org.usfirst.frc.team125.robot.commands.Groups.Autos.PalmettoAutos.TwoScaleAutos;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathCmd;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DrivePathReverseCmd;
+import org.usfirst.frc.team125.robot.commands.Groups.AutoLiftCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.ScoreCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Groups.SecureCubeCmdGrp;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeDownCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+import org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.TwoScale.LeftSideFarTwoScalePaths;
+
+public class LeftSideFarTwoScaleAuto extends CommandGroup {
+    Command intakeDown = new IntakeDownCmd();
+    Command secureCube = new SecureCubeCmdGrp();
+    Command liftElevatorToScale = new AutoLiftCmdGrp(0.5, CubeLift.Positions.ScoreScale);
+    Command driveToScale = new DrivePathCmd(LeftSideFarTwoScalePaths.toBeforeScaleTurn);
+    Command scoreCube = new ScoreCmdGrp();
+    Command bringElevatorToIntake = new RunToPositionMotionMagicCmd(CubeLift.Positions.Intake);
+    Command driveToSwitchA = new DrivePathReverseCmd(LeftSideFarTwoScalePaths.reverse_kTurnToSwitch1A);
+    Command intakeCube = new IntakeCmd();
+    Command driveToSwitchB = new DrivePathCmd(LeftSideFarTwoScalePaths.kTurnToSwitch1B);
+    Command liftElevatorToScaleAgain = new RunToPositionMotionMagicCmd(CubeLift.Positions.ScoreScale);
+    Command driveToScaleA = new DrivePathReverseCmd(LeftSideFarTwoScalePaths.reverse_kTurnToScaleA);
+    Command driveToScaleB = new DrivePathCmd(LeftSideFarTwoScalePaths.kTurnToScaleB);
+    Command scoreCubeAgain = new ScoreCmdGrp();
+
+    public LeftSideFarTwoScaleAuto() {
+        /*addSequential(intakeDown);
+        addSequential(new WaitCommand(0.25));
+        addSequential(secureCube);
+        */
+        addParallel(liftElevatorToScale);
+        addSequential(driveToScale);
+        addSequential(scoreCube);
+        addSequential(new WaitCommand(0.4));
+        addParallel(bringElevatorToIntake);
+        addSequential(driveToSwitchA);
+        addParallel(driveToSwitchB);
+        addSequential(intakeCube);
+        addParallel(liftElevatorToScaleAgain, 3);
+        addSequential(driveToScaleA);
+        addSequential(driveToScaleB);
+        addSequential(scoreCubeAgain);
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/ChinUpCmdGrp.java b/src/org/usfirst/frc/team125/robot/commands/Groups/ChinUpCmdGrp.java
new file mode 100644
index 0000000..9187caf
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/ChinUpCmdGrp.java
@@ -0,0 +1,16 @@
+package org.usfirst.frc.team125.robot.commands.Groups;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.commands.CubeLift.UnpinCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+
+public class ChinUpCmdGrp extends CommandGroup {
+
+    public ChinUpCmdGrp() {
+        addParallel(new UnpinCmd());
+        addSequential(new WaitCommand(0.5));
+        addSequential(new RunToPositionMotionMagicCmd(CubeLift.Positions.ChinUp), 4);
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/ClampAndLiftCmdGrp.java b/src/org/usfirst/frc/team125/robot/commands/Groups/ClampAndLiftCmdGrp.java
new file mode 100644
index 0000000..1039599
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/ClampAndLiftCmdGrp.java
@@ -0,0 +1,18 @@
+package org.usfirst.frc.team125.robot.commands.Groups;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.CloseGrabbersCmd;
+import org.usfirst.frc.team125.robot.commands.CubeLift.RunToPositionMotionMagicCmd;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+
+public class ClampAndLiftCmdGrp extends CommandGroup {
+    public ClampAndLiftCmdGrp() {
+        /*addParallel(new CloseGrabbersCmd());
+        addSequential(new PulseIntakeCmd(), 0.5);
+        */
+        addSequential(new CloseGrabbersCmd());
+        addSequential(new WaitCommand(0.34));
+        addSequential(new RunToPositionMotionMagicCmd(CubeLift.Positions.Driving));
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/ScoreCmdGrp.java b/src/org/usfirst/frc/team125/robot/commands/Groups/ScoreCmdGrp.java
new file mode 100644
index 0000000..7153bd6
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/ScoreCmdGrp.java
@@ -0,0 +1,14 @@
+package org.usfirst.frc.team125.robot.commands.Groups;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import org.usfirst.frc.team125.robot.commands.CubeLift.OpenGrabbersCmd;
+import org.usfirst.frc.team125.robot.commands.CubeLift.PunchCmd;
+
+public class ScoreCmdGrp extends CommandGroup {
+    public ScoreCmdGrp() {
+        addSequential(new OpenGrabbersCmd());
+        addSequential(new WaitCommand(0.1));
+        addSequential(new PunchCmd());
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Groups/SecureCubeCmdGrp.java b/src/org/usfirst/frc/team125/robot/commands/Groups/SecureCubeCmdGrp.java
new file mode 100644
index 0000000..297a207
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Groups/SecureCubeCmdGrp.java
@@ -0,0 +1,12 @@
+package org.usfirst.frc.team125.robot.commands.Groups;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team125.robot.commands.CubeLift.CloseGrabbersCmd;
+import org.usfirst.frc.team125.robot.commands.Intake.IntakeCmd;
+
+public class SecureCubeCmdGrp extends CommandGroup {
+    public SecureCubeCmdGrp() {
+        addSequential(new IntakeCmd(), 0.25);
+        addSequential(new CloseGrabbersCmd());
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Intake/AutonomousIntakeCmd.java b/src/org/usfirst/frc/team125/robot/commands/Intake/AutonomousIntakeCmd.java
new file mode 100644
index 0000000..0dcf692
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Intake/AutonomousIntakeCmd.java
@@ -0,0 +1,38 @@
+package org.usfirst.frc.team125.robot.commands.Intake;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+
+public class AutonomousIntakeCmd extends Command {
+
+    public AutonomousIntakeCmd() {
+        requires(Robot.intake);
+    }
+
+    @Override
+    protected void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        Robot.cubeLift.openGrabbers();
+        Robot.cubeLift.unpunch();
+        Robot.intake.intake();
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    protected void end() {
+        Robot.intake.stopIntake();
+    }
+
+    protected void interrupted() {
+        end();
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Intake/IntakeCmd.java b/src/org/usfirst/frc/team125/robot/commands/Intake/IntakeCmd.java
new file mode 100644
index 0000000..b17a948
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Intake/IntakeCmd.java
@@ -0,0 +1,49 @@
+package org.usfirst.frc.team125.robot.commands.Intake;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+import org.usfirst.frc.team125.robot.commands.Groups.ClampAndLiftCmdGrp;
+import org.usfirst.frc.team125.robot.subsystems.CubeLift;
+
+
+public class IntakeCmd extends Command {
+
+    public IntakeCmd() {
+        requires(Robot.intake);
+    }
+
+    @Override
+    protected void initialize() {
+        Robot.cubeLift.unpunch();
+    }
+
+    @Override
+    public void execute() {
+        Robot.cubeLift.unpunch();
+        if (Robot.cubeLift.getPosition() != CubeLift.Positions.Intake || Robot.cubeLift.getState() != CubeLift.LiftState.Stationary) {
+            Robot.intake.stopIntake();
+        } else if (Robot.intake.checkSmartIntakeTriggered()) {
+            Command smartIntakeTriggered = new ClampAndLiftCmdGrp();
+            smartIntakeTriggered.start();
+            Robot.intake.intake(); // TODO: TEST TEST TEST!!!
+        } else {
+            Robot.cubeLift.openGrabbers();
+            Robot.intake.intake();
+        }
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return Robot.intake.checkSmartIntakeTriggered();
+    }
+
+    @Override
+    protected void end() {
+        Robot.intake.stopIntake();
+    }
+
+    protected void interrupted() {
+        end();
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/OpenClampCMD.java b/src/org/usfirst/frc/team125/robot/commands/Intake/IntakeDownCmd.java
similarity index 65%
rename from src/org/usfirst/frc/team125/robot/commands/OpenClampCMD.java
rename to src/org/usfirst/frc/team125/robot/commands/Intake/IntakeDownCmd.java
index df57f7b..1838fa9 100644
--- a/src/org/usfirst/frc/team125/robot/commands/OpenClampCMD.java
+++ b/src/org/usfirst/frc/team125/robot/commands/Intake/IntakeDownCmd.java
@@ -1,25 +1,22 @@
-package org.usfirst.frc.team125.robot.commands;
-
-import org.usfirst.frc.team125.robot.Robot;
+package org.usfirst.frc.team125.robot.commands.Intake;
 
 import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
 
-/**
- *
- */
-public class OpenClampCMD extends Command {
+public class IntakeDownCmd extends Command {
 
-    public OpenClampCMD() {
+    public IntakeDownCmd() {
         requires(Robot.intake);
     }
 
     protected void initialize() {
+        Robot.intake.intakePistonDown();
     }
 
     protected void execute() {
-    	Robot.intake.openClamp();
     }
 
+    @Override
     protected boolean isFinished() {
         return true;
     }
diff --git a/src/org/usfirst/frc/team125/robot/commands/UpdateCubeSwitchCMD.java b/src/org/usfirst/frc/team125/robot/commands/Intake/IntakeStopCmd.java
similarity index 55%
rename from src/org/usfirst/frc/team125/robot/commands/UpdateCubeSwitchCMD.java
rename to src/org/usfirst/frc/team125/robot/commands/Intake/IntakeStopCmd.java
index 6a68b4c..8ba843b 100644
--- a/src/org/usfirst/frc/team125/robot/commands/UpdateCubeSwitchCMD.java
+++ b/src/org/usfirst/frc/team125/robot/commands/Intake/IntakeStopCmd.java
@@ -1,31 +1,30 @@
-package org.usfirst.frc.team125.robot.commands;
+package org.usfirst.frc.team125.robot.commands.Intake;
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team125.robot.Robot;
 
-/**
- *
- */
-public class UpdateCubeSwitchCMD extends Command {
+public class IntakeStopCmd extends Command {
 
-    public UpdateCubeSwitchCMD() {
+    public IntakeStopCmd() {
         requires(Robot.intake);
     }
 
+    @Override
     protected void initialize() {
-
     }
 
-    protected void execute() {
-        Robot.intake.updateCubeSwitch(true);
+    @Override
+    public void execute() {
+        Robot.intake.currentCounterReset();
     }
 
+    @Override
     protected boolean isFinished() {
         return false;
     }
 
+    @Override
     protected void end() {
-        Robot.intake.updateCubeSwitch(false);
     }
 
     protected void interrupted() {
diff --git a/src/org/usfirst/frc/team125/robot/commands/CloseClampCMD.java b/src/org/usfirst/frc/team125/robot/commands/Intake/IntakeUpCmd.java
similarity index 66%
rename from src/org/usfirst/frc/team125/robot/commands/CloseClampCMD.java
rename to src/org/usfirst/frc/team125/robot/commands/Intake/IntakeUpCmd.java
index f03ae21..ef943be 100644
--- a/src/org/usfirst/frc/team125/robot/commands/CloseClampCMD.java
+++ b/src/org/usfirst/frc/team125/robot/commands/Intake/IntakeUpCmd.java
@@ -1,32 +1,29 @@
-package org.usfirst.frc.team125.robot.commands;
-
-import org.usfirst.frc.team125.robot.Robot;
+package org.usfirst.frc.team125.robot.commands.Intake;
 
 import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
 
-/**
- *
- */
-public class CloseClampCMD extends Command {
+public class IntakeUpCmd extends Command {
 
-    public CloseClampCMD() {
+    public IntakeUpCmd() {
         requires(Robot.intake);
     }
 
     protected void initialize() {
+        Robot.intake.intakePistonUp();
     }
 
     protected void execute() {
-    	Robot.intake.closeClamp();
     }
 
+    @Override
     protected boolean isFinished() {
         return true;
     }
 
     protected void end() {
     }
-    
+
     protected void interrupted() {
     }
 }
diff --git a/src/org/usfirst/frc/team125/robot/commands/RunIntakeForwardCMD.java b/src/org/usfirst/frc/team125/robot/commands/Intake/OuttakeCmd.java
similarity index 55%
rename from src/org/usfirst/frc/team125/robot/commands/RunIntakeForwardCMD.java
rename to src/org/usfirst/frc/team125/robot/commands/Intake/OuttakeCmd.java
index 7a56fb0..45550b1 100644
--- a/src/org/usfirst/frc/team125/robot/commands/RunIntakeForwardCMD.java
+++ b/src/org/usfirst/frc/team125/robot/commands/Intake/OuttakeCmd.java
@@ -5,42 +5,41 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package org.usfirst.frc.team125.robot.commands;
+package org.usfirst.frc.team125.robot.commands.Intake;
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team125.robot.Robot;
-import org.usfirst.frc.team125.robot.subsystems.Intake;
 
 /**
  * An example command.  You can replace me with your own command.
  */
-public class RunIntakeForwardCMD extends Command {
-
-	public RunIntakeForwardCMD() {
-		requires(Robot.intake);
-	}
-
-	@Override
-	protected void initialize() {
-	}
-
-	@Override
-	protected void execute() {
-		Robot.intake.runIntake(1.0);
-	}
-
-	@Override
-	protected boolean isFinished() {
-		return false;
-	}
-
-	@Override
-	protected void end() {
-		Robot.intake.stopIntake();
-	}
-
-	@Override
-	protected void interrupted() {
-		end();
-	}
+public class OuttakeCmd extends Command {
+
+    public OuttakeCmd() {
+        requires(Robot.intake);
+    }
+
+    @Override
+    protected void initialize() {
+    }
+
+    @Override
+    protected void execute() {
+        Robot.intake.outtake();
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    protected void end() {
+        Robot.intake.stopIntake();
+    }
+
+    @Override
+    protected void interrupted() {
+        end();
+    }
 }
diff --git a/src/org/usfirst/frc/team125/robot/commands/Intake/PulseIntakeCmd.java b/src/org/usfirst/frc/team125/robot/commands/Intake/PulseIntakeCmd.java
new file mode 100644
index 0000000..a921fd9
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Intake/PulseIntakeCmd.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team125.robot.commands.Intake;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+
+public class PulseIntakeCmd extends Command {
+
+    public PulseIntakeCmd() {
+        requires(Robot.intake);
+    }
+
+    @Override
+    protected void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        Robot.intake.intake();
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    protected void end() {
+        Robot.intake.stopIntake();
+    }
+
+    protected void interrupted() {
+        end();
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/commands/Intake/ToggleIntakeSolenoidCmd.java b/src/org/usfirst/frc/team125/robot/commands/Intake/ToggleIntakeSolenoidCmd.java
new file mode 100644
index 0000000..1d31514
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/commands/Intake/ToggleIntakeSolenoidCmd.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team125.robot.commands.Intake;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team125.robot.Robot;
+
+public class ToggleIntakeSolenoidCmd extends Command {
+
+    public ToggleIntakeSolenoidCmd() {
+        requires(Robot.intake);
+    }
+
+    protected void initialize() {
+        Robot.intake.toggleIntakePiston();
+    }
+
+    protected void execute() {
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}
+
diff --git a/src/org/usfirst/frc/team125/robot/commands/ReleaseCarrierCMD.java b/src/org/usfirst/frc/team125/robot/commands/ReleaseCarrierCMD.java
deleted file mode 100644
index 70b8eed..0000000
--- a/src/org/usfirst/frc/team125/robot/commands/ReleaseCarrierCMD.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.usfirst.frc.team125.robot.commands;
-
-import org.usfirst.frc.team125.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class ReleaseCarrierCMD extends Command{
-
-	public ReleaseCarrierCMD() {
-		requires(Robot.doubleLift);
-	}
-	
-	protected void initialize() {
-		Robot.doubleLift.releaseCarrier();
-	}
-	
-	protected void execute() {
-	}
-
-	@Override
-	protected boolean isFinished() {
-		return true;
-	}
-	
-	protected void end() {
-	}
-	
-	protected void inturrupted() {
-	}
-}
diff --git a/src/org/usfirst/frc/team125/robot/commands/RetractCarrierCMD.java b/src/org/usfirst/frc/team125/robot/commands/RetractCarrierCMD.java
deleted file mode 100644
index 93e0315..0000000
--- a/src/org/usfirst/frc/team125/robot/commands/RetractCarrierCMD.java
+++ /dev/null
@@ -1,29 +0,0 @@
-package org.usfirst.frc.team125.robot.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team125.robot.Robot;
-
-public class RetractCarrierCMD extends Command {
-
-	public RetractCarrierCMD() {
-			requires(Robot.doubleLift);
-		}
-		
-	protected void initialize() {
-		Robot.doubleLift.lift();
-	}
-		
-	protected void execute() {
-	}
-		
-    @Override
-    protected boolean isFinished() {
-			return true;
-		}
-
-    protected void end() {
-    }
-		
-    protected void inturrupted() {
-    }
-}
diff --git a/src/org/usfirst/frc/team125/robot/commands/RunIntakeReverseCMD.java b/src/org/usfirst/frc/team125/robot/commands/RunIntakeReverseCMD.java
deleted file mode 100644
index b62038b..0000000
--- a/src/org/usfirst/frc/team125/robot/commands/RunIntakeReverseCMD.java
+++ /dev/null
@@ -1,37 +0,0 @@
-package org.usfirst.frc.team125.robot.commands;
-
-import org.usfirst.frc.team125.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team125.robot.subsystems.Intake;
-
-public class RunIntakeReverseCMD extends Command{
-
-	public RunIntakeReverseCMD() {
-		requires(Robot.intake);
-	}
-	
-	@Override
-	protected void initialize() {
-	}
-	
-	@Override
-	public void execute() {
-		Robot.intake.runIntake(-1.0);
-	}
-
-	@Override
-	protected boolean isFinished() {
-		return false;
-	}
-	
-	@Override
-	protected void end() {
-		Robot.intake.stopIntake();
-	}
-	
-	protected void interrupted() {
-		end();
-	}
-
-}
diff --git a/src/org/usfirst/frc/team125/robot/commands/SetupPathCmd.java b/src/org/usfirst/frc/team125/robot/commands/SetupPathCmd.java
deleted file mode 100644
index c24ac5c..0000000
--- a/src/org/usfirst/frc/team125/robot/commands/SetupPathCmd.java
+++ /dev/null
@@ -1,37 +0,0 @@
-package org.usfirst.frc.team125.robot.commands;
-
-import org.usfirst.frc.team125.robot.Robot;
-import edu.wpi.first.wpilibj.command.Command;
-import jaci.pathfinder.modifiers.TankModifier;
-
-/**
- *
- */
-public class SetupPathCmd extends Command {
-
-    TankModifier modifier;
-
-    public SetupPathCmd(TankModifier modifier) {
-        requires(Robot.dt);
-        this.modifier = modifier;
-    }
-
-    protected void initialize() {
-        Robot.dt.pathSetup(modifier, true);
-    }
-
-    protected void execute() {
-        Robot.dt.pathSetup(modifier, true);
-    }
-
-    protected boolean isFinished() {
-        return false;
-    }
-
-    protected void end() {
-    }
-
-    protected void interrupted() {
-    }
-
-}
diff --git a/src/org/usfirst/frc/team125/robot/subsystems/CubeLift.java b/src/org/usfirst/frc/team125/robot/subsystems/CubeLift.java
new file mode 100644
index 0000000..19a5174
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/subsystems/CubeLift.java
@@ -0,0 +1,343 @@
+package org.usfirst.frc.team125.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.motorcontrol.can.VictorSPX;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team125.robot.RobotMap;
+import org.usfirst.frc.team125.robot.commands.CubeLift.ElevatorDriveCmd;
+import org.usfirst.frc.team125.robot.util.CurrentReader;
+import org.usfirst.frc.team125.robot.util.DebouncedBoolean;
+
+public class CubeLift extends Subsystem {
+
+    public static enum LiftState {
+        GoingUp,
+        GoingDown,
+        Stationary,
+        BottomedOut,
+        ToppedOut,
+    }
+
+    public LiftState getState() {
+        return state;
+    }
+
+    private void setState(LiftState newState) {
+        this.state = newState;
+    }
+
+    private volatile LiftState state = LiftState.Stationary;
+
+    private volatile TalonSRX rightElevatorLeader = new TalonSRX(RobotMap.RIGHT_ELEVATOR_MAIN);
+    private VictorSPX rightElevatorSlave = new VictorSPX(RobotMap.RIGHT_ELEVATOR_SLAVE);
+    private VictorSPX leftElevatorSlaveA = new VictorSPX(RobotMap.LEFT_ELEVATOR_SLAVE_A);
+    private VictorSPX leftElevatorSlaveB = new VictorSPX(RobotMap.LEFT_ELEVATOR_SLAVE_B);
+
+    private Solenoid grabbers = new Solenoid(RobotMap.GRABBERS);
+    private Solenoid releasePin = new Solenoid(RobotMap.RELEASE_PIN);
+    private Solenoid puncher = new Solenoid(RobotMap.PUNCHER);
+
+    private CurrentReader elevatorCurrentReader = new CurrentReader();
+    private DebouncedBoolean currentSpikeDebouncer = new DebouncedBoolean(1.0);
+
+    private boolean isCurrentReaderMaxed() {
+        return elevatorCurrentReader.getTotalCurrent(CurrentReader.CurrentPorts.Cubelift) >= CurrentReader.CUBELIFT_MAX_CURRENT;
+    }
+
+    public void updateCurrentSpikeDebouncer() {
+        currentSpikeDebouncer.update(isCurrentReaderMaxed());
+    }
+
+    public boolean getCurrentSpikeDebouncer() {
+        return currentSpikeDebouncer.get();
+    }
+
+    private double kP = 0.5; // .3
+    private double kI = 0.0;
+    private double kD = 4.0; // 4.0
+    private double kF = 0.1165 * 2; // 0.1165 * 2
+
+    //81.2 inches per/s
+    //Gear is 42:26 geared up
+    //4096 ticks per rev
+    //233 ticks per inch
+    private static final int CRUISE_VELOCITY = 17600; // 1024
+    private static final int CRUISE_ACCELERATION = 11000; // 1024
+    private static final int CRUISE_VELOCITY_DOWN = (int) (CRUISE_VELOCITY * 0.7); // 1024
+    private static final int CRUISE_ACCELERATION_DOWN = (int) (CRUISE_ACCELERATION * 0.6); // 1024
+
+    public enum Positions {
+        Intake(300),
+        Driving(25000),
+        ScoreSwitch(40000),
+        ScoreScale(70000),
+        ScoreScaleLow(60000),
+        ScoreScaleHigh(75000),
+        PreClimb(74000),
+        Top(75001),
+        ChinUp(40500),
+        ClimbingBar(67500);
+        private int position;
+
+        Positions(int encPos) {
+            this.position = encPos;
+        }
+
+        public int getPosition() {
+            return this.position;
+        }
+    }
+
+    private volatile Positions position = Positions.Intake;
+
+    public Positions getPosition() {
+        return position;
+    }
+
+    private void setPosition(Positions newPos) {
+        this.position = newPos;
+    }
+
+    private class ElevatorSafety implements Runnable {
+        int i = 0;
+
+        @Override
+        public void run() {
+            while (true) {
+                try {
+                    Thread.sleep(20L);
+                } catch (InterruptedException e) {
+                    e.printStackTrace();
+                }
+                checkIfToppedOut();
+                checkIfZeroedOut();
+                SmartDashboard.putNumber("i counter", i);
+                i++;
+            }
+        }
+    }
+
+    private static final boolean GRAB_SET = false;
+    private static final boolean UNGRAB_SET = true;
+    private boolean grabberToggle = false;
+    private static final boolean UNPIN_SET = true;
+    private static final boolean PIN_SET = false;
+    private boolean pinToggle = false;
+    private static final boolean PUNCH_SET = true;
+    private static final boolean UNPUNCH_SET = false;
+    private boolean puncherToggle = false;
+
+    private final int MOTION_MAGIC_TOLERANCE = 300;
+    private static final double ELEVATOR_HI_POW = 1.0;
+    private static final double ELEVATOR_LOW_POW = -ELEVATOR_HI_POW;
+
+    public CubeLift() {
+        Thread thread = new Thread(new ElevatorSafety());
+        thread.start();
+        this.rightElevatorSlave.follow(rightElevatorLeader);
+        this.leftElevatorSlaveA.follow(rightElevatorLeader);
+        this.leftElevatorSlaveB.follow(rightElevatorLeader);
+        this.rightElevatorLeader.configPeakOutputForward(ELEVATOR_HI_POW, 0);
+        this.rightElevatorLeader.configPeakOutputReverse(ELEVATOR_LOW_POW, 0);
+        this.rightElevatorLeader.configNominalOutputForward(0.0, 0);
+        this.rightElevatorLeader.configNominalOutputReverse(0.0, 0);
+        this.rightElevatorSlave.configPeakOutputForward(ELEVATOR_HI_POW, 0);
+        this.rightElevatorSlave.configPeakOutputReverse(ELEVATOR_LOW_POW, 0);
+        this.rightElevatorSlave.configNominalOutputForward(0.0, 0);
+        this.rightElevatorSlave.configNominalOutputReverse(0.0, 0);
+        this.leftElevatorSlaveA.configPeakOutputForward(ELEVATOR_HI_POW, 0);
+        this.leftElevatorSlaveA.configPeakOutputReverse(ELEVATOR_LOW_POW, 0);
+        this.leftElevatorSlaveA.configNominalOutputForward(0.0, 0);
+        this.leftElevatorSlaveA.configNominalOutputReverse(0.0, 0);
+        this.leftElevatorSlaveB.configPeakOutputForward(ELEVATOR_HI_POW, 0);
+        this.leftElevatorSlaveB.configPeakOutputReverse(ELEVATOR_LOW_POW, 0);
+        this.leftElevatorSlaveB.configNominalOutputForward(0.0, 0);
+        this.leftElevatorSlaveB.configNominalOutputReverse(0.0, 0);
+
+        //Encoder
+        this.rightElevatorLeader.setSensorPhase(true);
+        this.rightElevatorLeader.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
+        resetEncoders();
+        //this.rightElevatorLeader.setSelectedSensorPosition(rightElevatorLeader.getSensorCollection().getQuadraturePosition(), 0, 0);
+
+        //Neutral mode
+        this.rightElevatorLeader.setNeutralMode(NeutralMode.Brake);
+        this.rightElevatorSlave.setNeutralMode(NeutralMode.Brake);
+        this.leftElevatorSlaveA.setNeutralMode(NeutralMode.Brake);
+        this.leftElevatorSlaveB.setNeutralMode(NeutralMode.Brake);
+
+        this.leftElevatorSlaveA.setInverted(true);
+        this.leftElevatorSlaveB.setInverted(true);
+        this.rightElevatorLeader.setInverted(false);
+        this.rightElevatorSlave.setInverted(false);
+
+        configPIDF(kP, kI, kD, kF);
+        configMotionMagic(CRUISE_VELOCITY, CRUISE_ACCELERATION);
+
+        //Hard coded to false to reduce chance of breaking...
+        grabbers.set(false);
+        releasePin.set(false);
+    }
+
+
+    public void resetEncoders() {
+        this.rightElevatorLeader.setSelectedSensorPosition(0, 0, 0);
+    }
+
+    public int getQuadraturePosition() {
+        return rightElevatorLeader.getSelectedSensorPosition(0);
+        //return rightElevatorLeader.getSensorCollection().getQuadraturePosition();
+    }
+
+    public void startMotionMagic(Positions pos) { // Up is now negative
+        if (getQuadraturePosition() > pos.getPosition()) {
+            setState(LiftState.GoingDown);
+            configMotionMagic(CRUISE_VELOCITY_DOWN, CRUISE_ACCELERATION_DOWN);
+        } else if (getQuadraturePosition() < pos.getPosition()) {
+            setState(LiftState.GoingUp);
+            configMotionMagic(CRUISE_VELOCITY, CRUISE_ACCELERATION);
+        }
+
+        rightElevatorLeader.set(ControlMode.MotionMagic, pos.getPosition());
+    }
+
+    public void checkMotionMagicTermination(Positions pos) {
+        if (pos == Positions.Intake) {
+            if (getQuadraturePosition() <= (MOTION_MAGIC_TOLERANCE * 2)) {
+                state = LiftState.Stationary;
+                stopElevator();
+                position = pos;
+            }
+        } else if (Math.abs(pos.getPosition() - getQuadraturePosition()) <= MOTION_MAGIC_TOLERANCE) {
+            state = LiftState.Stationary;
+            stopElevator();
+            position = pos;
+        }
+        SmartDashboard.putString("Desired elevator position enum", pos.toString());
+        SmartDashboard.putNumber("Motion Magic set position", rightElevatorLeader.getClosedLoopTarget(0));
+        SmartDashboard.putNumber("CTRError", rightElevatorLeader.getClosedLoopError(0));
+        SmartDashboard.putNumber("Desired elevator position", pos.getPosition());
+        SmartDashboard.putNumber("Closed loop error", Math.abs(pos.getPosition() - getQuadraturePosition()));
+    }
+
+    public void openGrabbers() {
+        grabbers.set(UNGRAB_SET);
+    }
+
+    public void closeGrabbers() {
+        grabbers.set(GRAB_SET);
+    }
+
+    public void toggleGrabbers() {
+        grabberToggle = !grabberToggle;
+        grabbers.set(grabberToggle);
+    }
+
+    public void unpin() {
+        releasePin.set(UNPIN_SET);
+    }
+
+    public void pin() {
+        releasePin.set(PIN_SET);
+    }
+
+    public void togglePin() {
+        pinToggle = !pinToggle;
+        releasePin.set(pinToggle);
+    }
+
+    public void punch() {
+        puncher.set(PUNCH_SET);
+    }
+
+    public void unpunch() {
+        puncher.set(UNPUNCH_SET);
+    }
+
+    public void togglePunch() {
+        puncherToggle = !puncherToggle;
+        puncher.set(puncherToggle);
+    }
+
+    public void stopElevator() {
+        rightElevatorLeader.set(ControlMode.PercentOutput, 0.0);
+    }
+
+    public void directElevate(double pow) {
+        if (getState() == LiftState.BottomedOut && pow < 0.0) {
+            return;
+        }
+        if (getState() == LiftState.ToppedOut && pow > 0.0) {
+            return;
+        }
+        if (pow > 0.0) {
+            setState(LiftState.GoingUp);
+        }
+        if (pow < 0.0) {
+            setState(LiftState.GoingDown);
+        }
+        if (pow == 0.0) {
+            setState(LiftState.Stationary);
+        }
+        rightElevatorLeader.set(ControlMode.PercentOutput, pow);
+    }
+
+    private void checkIfToppedOut() {
+        if (getQuadraturePosition() >= Positions.Top.getPosition() && getState() != LiftState.GoingDown) {
+            setState(LiftState.ToppedOut);
+            setPosition(Positions.Top);
+            stopElevator();
+        }
+    }
+
+    private void checkIfZeroedOut() {
+        if (getQuadraturePosition() <= Positions.Intake.getPosition() && getState() != LiftState.GoingUp) {
+            setState(LiftState.BottomedOut);
+            setPosition(Positions.Intake);
+            stopElevator();
+        }
+    }
+
+    public void configPIDF(double kP, double kI, double kD, double kF) {
+        rightElevatorLeader.config_kP(0, kP, 0);
+        rightElevatorLeader.config_kI(0, kI, 0);
+        rightElevatorLeader.config_kD(0, kD, 0);
+        rightElevatorLeader.config_kF(0, kF, 0);
+    }
+
+    /**
+     * Set parameters for motion magic control
+     *
+     * @param cruiseVelocity cruise velocity in sensorUnits per 100ms
+     * @param acceleration   cruise acceleration in sensorUnits per 100ms
+     */
+    public void configMotionMagic(int cruiseVelocity, int acceleration) {
+        rightElevatorLeader.configMotionCruiseVelocity(cruiseVelocity, 0);
+        rightElevatorLeader.configMotionAcceleration(acceleration, 0);
+    }
+
+    public void updatePIDFOnDashboard() {
+        SmartDashboard.putNumber("kP", kP);
+        SmartDashboard.putNumber("kI", kI);
+        SmartDashboard.putNumber("kD", kD);
+        SmartDashboard.putNumber("kF", kF);
+    }
+
+    public void updatePIDFFromDashboard() {
+        kP = SmartDashboard.getNumber("kP", kP);
+        kI = SmartDashboard.getNumber("kI", kI);
+        kD = SmartDashboard.getNumber("kD", kD);
+        kF = SmartDashboard.getNumber("kF", kF);
+        configPIDF(kP, kI, kD, kF);
+    }
+
+    public void initDefaultCommand() {
+        setDefaultCommand(new ElevatorDriveCmd());
+    }
+}
+
diff --git a/src/org/usfirst/frc/team125/robot/subsystems/DoubleLift.java b/src/org/usfirst/frc/team125/robot/subsystems/DoubleLift.java
index 4a0bf54..f2da0a2 100644
--- a/src/org/usfirst/frc/team125/robot/subsystems/DoubleLift.java
+++ b/src/org/usfirst/frc/team125/robot/subsystems/DoubleLift.java
@@ -1,35 +1,53 @@
 package org.usfirst.frc.team125.robot.subsystems;
 
-import org.usfirst.frc.team125.robot.RobotMap;
-
-import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import org.usfirst.frc.team125.robot.RobotMap;
+
+public class DoubleLift extends Subsystem {
+
+    private Solenoid doubleLifter = new Solenoid(RobotMap.DOUBLELIFT_LIFTER);
+    private Solenoid release = new Solenoid(RobotMap.DOUBLELIFT_RELEASE);
+    private static final boolean RELEASE_CARRIER_SET = true;
+    private static final boolean UNRELEASE_CARRIER_SET = false;
+    private static final boolean DROP_LIFT_SET = false;
+    private static final boolean LIFT_LIFT_SET = true;
+
+    private boolean releaseToggle = false;
+    private boolean liftToggle = true;
+
+    public DoubleLift() {
+        this.doubleLifter.set(false); // We want it up, hard coded for safety
+        this.release.set(false); // We want it not out, hard coded for safety
+    }
+
+    public void releaseCarrier() {
+        this.release.set(RELEASE_CARRIER_SET);
+    }
+
+    public void unreleaseCarrier() {
+        this.release.set(UNRELEASE_CARRIER_SET);
+    }
+
+    public void toggleRelease() {
+        this.releaseToggle = !releaseToggle;
+        this.release.set(releaseToggle);
+    }
+
+    public void dropLift() {
+        this.doubleLifter.set(DROP_LIFT_SET);
+    }
+
+    public void liftLift() {
+        this.doubleLifter.set(LIFT_LIFT_SET);
+    }
 
-/**
- * 
- * The mechansim to lift another robot (including a seperate release)
- */
-public class DoubleLift extends Subsystem{
-
-	private Solenoid lift = new Solenoid(RobotMap.DOUBLELIFT_LIFTER);
-	private Solenoid release = new Solenoid(RobotMap.DOUBLELIFT_RELEASE);
-	
-	public DoubleLift() {
-		this.lift.set(false);
-		this.release.set(false);
-	}
-	
-	public void releaseCarrier() {
-		this.release.set(true);
-	} // TODO: Check the .set()
-	
-	public void lift() {
-		this.lift.set(true);
-	} // TODO: Check the .set()
-	
-	@Override
-	protected void initDefaultCommand() {
-	}
+    public void toggleLift() {
+        this.liftToggle = !liftToggle;
+        this.doubleLifter.set(liftToggle);
+    }
 
+    @Override
+    protected void initDefaultCommand() {
+    }
 }
diff --git a/src/org/usfirst/frc/team125/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team125/robot/subsystems/Drivetrain.java
index 958d5ae..1556715 100644
--- a/src/org/usfirst/frc/team125/robot/subsystems/Drivetrain.java
+++ b/src/org/usfirst/frc/team125/robot/subsystems/Drivetrain.java
@@ -5,19 +5,21 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.VictorSPX;
-import org.usfirst.frc.team125.robot.RobotMap;
-import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Trajectory;
+import jaci.pathfinder.Waypoint;
 import jaci.pathfinder.followers.EncoderFollower;
 import jaci.pathfinder.modifiers.TankModifier;
-import org.usfirst.frc.team125.robot.commands.DriveArcadeCmd;
+import org.usfirst.frc.team125.robot.RobotMap;
+import org.usfirst.frc.team125.robot.commands.Drivetrain.DriveArcadeCmd;
+
+import java.io.File;
 
-/**
- * 4 Motor Drivetrain Subclass
- */
 public class Drivetrain extends Subsystem {
 
     //Controllers
@@ -28,15 +30,19 @@ public class Drivetrain extends Subsystem {
     private VictorSPX rightDriveSlaveA = new VictorSPX(RobotMap.RIGHT_DRIVE_SLAVE_A);
     private VictorSPX rightDriveSlaveB = new VictorSPX(RobotMap.RIGHT_DRIVE_SLAVE_B);
 
-    ADXRS450_Gyro gyro = new ADXRS450_Gyro();
+    private static final double HIGH_POW = 1.0;
+    private static final double LOW_POW = -HIGH_POW;
+    private static final double RAMP_RATE = 0.25;
 
-    //Encoder Stuff
-    EncoderFollower left;
-    EncoderFollower right;
+    AHRS gyro = new AHRS(I2C.Port.kMXP);
 
     //Timing
     public Timer timer = new Timer();
 
+    //Gyro logging for driving
+    double lastHeadingError = 0.0;
+    final double TURN_TO_ANGLE_TOLERANCE = 10.0;
+
     public Drivetrain() {
         //Slave Control
         this.leftDriveSlaveA.follow(leftDriveMain);
@@ -44,46 +50,70 @@ public Drivetrain() {
         this.leftDriveSlaveB.follow(leftDriveMain);
         this.rightDriveSlaveB.follow(rightDriveMain);
 
-        this.leftDriveMain.configPeakOutputForward(1.0, 0);
-        this.leftDriveMain.configPeakOutputReverse(-1.0, 0);
+        this.leftDriveMain.configPeakOutputForward(HIGH_POW, 0);
+        this.leftDriveMain.configPeakOutputReverse(LOW_POW, 0);
         this.leftDriveMain.configNominalOutputForward(0.0, 0);
         this.leftDriveMain.configNominalOutputReverse(0.0, 0);
-        this.leftDriveSlaveA.configPeakOutputForward(1.0, 0);
-        this.leftDriveSlaveA.configPeakOutputReverse(-1.0, 0);
+        this.leftDriveSlaveA.configPeakOutputForward(HIGH_POW, 0);
+        this.leftDriveSlaveA.configPeakOutputReverse(LOW_POW, 0);
         this.leftDriveSlaveA.configNominalOutputForward(0.0, 0);
         this.leftDriveSlaveA.configNominalOutputReverse(0.0, 0);
-        this.leftDriveSlaveB.configPeakOutputForward(1.0, 0);
-        this.leftDriveSlaveB.configPeakOutputReverse(-1.0, 0);
+        this.leftDriveSlaveB.configPeakOutputForward(HIGH_POW, 0);
+        this.leftDriveSlaveB.configPeakOutputReverse(LOW_POW, 0);
         this.leftDriveSlaveB.configNominalOutputForward(0.0, 0);
         this.leftDriveSlaveB.configNominalOutputReverse(0.0, 0);
 
-        this.rightDriveMain.configPeakOutputForward(1.0, 0);
-        this.rightDriveMain.configPeakOutputReverse(-1.0, 0);
+        this.rightDriveMain.configPeakOutputForward(HIGH_POW, 0);
+        this.rightDriveMain.configPeakOutputReverse(LOW_POW, 0);
         this.rightDriveMain.configNominalOutputForward(0.0, 0);
         this.rightDriveMain.configNominalOutputReverse(0.0, 0);
-        this.rightDriveSlaveA.configPeakOutputForward(1.0, 0);
-        this.rightDriveSlaveA.configPeakOutputReverse(-1.0, 0);
+        this.rightDriveSlaveA.configPeakOutputForward(HIGH_POW, 0);
+        this.rightDriveSlaveA.configPeakOutputReverse(LOW_POW, 0);
         this.rightDriveSlaveA.configNominalOutputForward(0.0, 0);
         this.rightDriveSlaveA.configNominalOutputReverse(0.0, 0);
-        this.rightDriveSlaveB.configPeakOutputForward(1.0, 0);
-        this.rightDriveSlaveB.configPeakOutputReverse(-1.0, 0);
+        this.rightDriveSlaveB.configPeakOutputForward(HIGH_POW, 0);
+        this.rightDriveSlaveB.configPeakOutputReverse(LOW_POW, 0);
         this.rightDriveSlaveB.configNominalOutputForward(0.0, 0);
         this.rightDriveSlaveB.configNominalOutputReverse(0.0, 0);
 
+
+        this.leftDriveMain.setInverted(false);
+        this.leftDriveSlaveA.setInverted(false);
+        this.leftDriveSlaveB.setInverted(false);
+        this.rightDriveMain.setInverted(true);
+        this.rightDriveSlaveA.setInverted(true);
+        this.rightDriveSlaveB.setInverted(true);
+
         //Encoder
         this.leftDriveMain.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
         this.rightDriveMain.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
 
-        disableBreakMode();
+        enableRamping();
+
+        resetEncoders();
+        enableBreakMode();
 
         //Gyro
-        gyro.reset();
-        gyro.calibrate();
+        resetGyro();
+    }
+
+    public void enableRamping() {
+        this.leftDriveMain.configOpenloopRamp(RAMP_RATE, 0);
+        this.leftDriveMain.configClosedloopRamp(RAMP_RATE, 0);
+        this.rightDriveMain.configOpenloopRamp(RAMP_RATE, 0);
+        this.rightDriveMain.configClosedloopRamp(RAMP_RATE, 0);
+    }
+
+    public void disableRamping() {
+        this.leftDriveMain.configOpenloopRamp(0.0, 0);
+        this.leftDriveMain.configClosedloopRamp(0.0, 0);
+        this.rightDriveMain.configOpenloopRamp(0.0, 0);
+        this.rightDriveMain.configClosedloopRamp(0.0, 0);
     }
 
     public void drive(double powLeft, double powRight) {
         this.leftDriveMain.set(ControlMode.PercentOutput, powLeft);
-        this.rightDriveMain.set(ControlMode.PercentOutput, -powRight);
+        this.rightDriveMain.set(ControlMode.PercentOutput, powRight);
     }
 
     public void driveArcade(double throttle, double turn) {
@@ -91,8 +121,34 @@ public void driveArcade(double throttle, double turn) {
         this.rightDriveMain.set(ControlMode.PercentOutput, throttle - turn);
     }
 
+    public void driveHoldHeading(double throttle) {
+        double turn = (DrivetrainProfiling.gp * gyro.getAngle()) + (DrivetrainProfiling.gd * (gyro.getAngle() - lastHeadingError));
+        this.lastHeadingError = gyro.getAngle();
+        this.leftDriveMain.set(ControlMode.PercentOutput, throttle - turn);
+        this.rightDriveMain.set(ControlMode.PercentOutput, throttle + turn);
+    }
+
+    public boolean turnToAngle(double angle) {
+        double error = angle - gyro.getAngle();
+        double turn = DrivetrainProfiling.gp * error + (DrivetrainProfiling.gd *
+                ((error - lastHeadingError) / DrivetrainProfiling.dt));
+        this.leftDriveMain.set(ControlMode.PercentOutput, turn);
+        this.rightDriveMain.set(ControlMode.PercentOutput, -turn);
+        lastHeadingError = error;
+        SmartDashboard.putNumber("turn to angle error", error);
+        return Math.abs(error) <= TURN_TO_ANGLE_TOLERANCE;
+    }
+
+    public void resetLastHeadingError() {
+        this.lastHeadingError = 0.0;
+    }
+
+    public double getGyroRate() {
+        return gyro.getRate();
+    }
+
     public double getLeftVelocity() {
-        return (leftDriveMain.getSelectedSensorVelocity(0) * Math.PI * DrivetrainProfiling.wheel_diameter) / (DrivetrainProfiling.ticks_per_rev)  * 10;
+        return (leftDriveMain.getSelectedSensorVelocity(0) * Math.PI * DrivetrainProfiling.wheel_diameter) / (DrivetrainProfiling.ticks_per_rev) * 10;
     }
 
     public double getRightVelocity() {
@@ -129,54 +185,145 @@ public void resetEncoders() {
     }
 
     public double getEncoderDistanceMetersRight() {
-        return (rightDriveMain.getSelectedSensorPosition(0) * Math.PI * DrivetrainProfiling.wheel_diameter) / DrivetrainProfiling.ticks_per_rev;
+        return (getEncoderRawRight() * Math.PI * DrivetrainProfiling.wheel_diameter) / DrivetrainProfiling.ticks_per_rev;
     }
 
     public double getEncoderDistanceMetersLeft() {
-        return (leftDriveMain.getSelectedSensorPosition(0) * Math.PI * DrivetrainProfiling.wheel_diameter) / DrivetrainProfiling.ticks_per_rev;
+        return (getEncoderRawLeft() * Math.PI * DrivetrainProfiling.wheel_diameter) / DrivetrainProfiling.ticks_per_rev;
     }
-    
-    public double getEncoderRawLeft() {
+
+    public int getEncoderRawLeft() {
         return leftDriveMain.getSelectedSensorPosition(0);
     }
 
-    public double getEncoderRawRight() {
+    public int getEncoderRawRight() {
         return rightDriveMain.getSelectedSensorPosition(0);
     }
 
     public double getAngle() {
-        return gyro.getAngle();
+        return -gyro.getAngle();
+    }
+
+    public void resetGyro() {
+        this.gyro.reset();
+    }
+
+    public double generateHashCode(Waypoint[] path) {
+        double hash = 1.0;
+        for (int i = 0; i < path.length; i++) {
+            hash = ((path[i].x * 3) + (path[i].y * 7) + (path[i].angle * 11));
+        }
+        return (int) Math.abs(hash * 1000) * path.length;
     }
 
-    public void pathSetup(TankModifier modifier, boolean relative) {
-        if(relative) {
-            resetEncoders();
-            gyro.reset();
+    public EncoderFollower[] pathSetup(Waypoint[] path) {
+
+        EncoderFollower left = new EncoderFollower();
+        EncoderFollower right = new EncoderFollower();
+        Trajectory.Config cfg = new Trajectory.Config(Trajectory.FitMethod.HERMITE_QUINTIC, Trajectory.Config.SAMPLES_HIGH,
+                Drivetrain.DrivetrainProfiling.dt, Drivetrain.DrivetrainProfiling.max_velocity, Drivetrain.DrivetrainProfiling.max_acceleration, Drivetrain.DrivetrainProfiling.max_jerk);
+        String pathHash = String.valueOf(generateHashCode(path));
+        SmartDashboard.putString("Path Hash", pathHash);
+        Trajectory toFollow;
+        File trajectory = new File("/home/lvuser/paths/" + pathHash + ".csv");
+        if (!trajectory.exists()) {
+            toFollow = Pathfinder.generate(path, cfg);
+            Pathfinder.writeToCSV(trajectory, toFollow);
+            System.out.println(pathHash + ".csv not found, wrote to file");
+        } else {
+            System.out.println(pathHash + ".csv read from file");
+            toFollow = Pathfinder.readFromCSV(trajectory);
         }
 
+        TankModifier modifier = new TankModifier(toFollow).modify((Drivetrain.DrivetrainProfiling.wheel_base_width));
         DrivetrainProfiling.last_gyro_error = 0.0;
         left = new EncoderFollower(modifier.getLeftTrajectory());
         right = new EncoderFollower(modifier.getRightTrajectory());
-        left.configureEncoder(leftDriveMain.getSelectedSensorPosition(0), DrivetrainProfiling.ticks_per_rev, DrivetrainProfiling.wheel_diameter);
-        right.configureEncoder(rightDriveMain.getSelectedSensorPosition(0), DrivetrainProfiling.ticks_per_rev, DrivetrainProfiling.wheel_diameter);
+        left.configureEncoder(getEncoderRawLeft(), DrivetrainProfiling.ticks_per_rev, DrivetrainProfiling.wheel_diameter);
+        right.configureEncoder(getEncoderRawRight(), DrivetrainProfiling.ticks_per_rev, DrivetrainProfiling.wheel_diameter);
         left.configurePIDVA(DrivetrainProfiling.kp, DrivetrainProfiling.ki, DrivetrainProfiling.kd, DrivetrainProfiling.kv, DrivetrainProfiling.ka);
         right.configurePIDVA(DrivetrainProfiling.kp, DrivetrainProfiling.ki, DrivetrainProfiling.kd, DrivetrainProfiling.kv, DrivetrainProfiling.ka);
+        return new EncoderFollower[]{
+                left, // 0
+                right, // 1
+        };
+    }
+
+    public void resetForPath() {
+        isProfileFinished = false;
+        resetEncoders();
+        resetGyro();
     }
 
-    public void pathFollow() {
-        double l = left.calculate(leftDriveMain.getSelectedSensorPosition(0));
-        double r = right.calculate(rightDriveMain.getSelectedSensorPosition(0));
 
-        double gyro_heading = gyro.getAngle();
+    public void resetPathAngleOffset() {
+        DrivetrainProfiling.path_angle_offset = 0.0;
+    }
+
+    private boolean isProfileFinished = false;
+
+    public boolean getIsProfileFinished() {
+        return isProfileFinished;
+    }
+
+    /*
+    If going !reverse going forward x is positive, going left y is positive, turning left is positive
+    If going reverse going backwards x is positive, going right y is negative, turning left is negative
+     */
+    public void pathFollow(EncoderFollower[] followers, boolean reverse) {
+        EncoderFollower left = followers[0];
+        EncoderFollower right = followers[1];
+        double l;
+        double r;
+        double localGp = DrivetrainProfiling.gp;
+        if (!reverse) {
+            localGp *= -1;
+            l = left.calculate(-getEncoderRawLeft());
+            r = right.calculate(-getEncoderRawRight());
+        } else {
+            l = left.calculate(getEncoderRawLeft());
+            r = right.calculate(getEncoderRawRight());
+        }
+
+        double gyro_heading = reverse ? -getAngle() - DrivetrainProfiling.path_angle_offset : getAngle() + DrivetrainProfiling.path_angle_offset;
         double angle_setpoint = Pathfinder.r2d(left.getHeading());
+        SmartDashboard.putNumber("Angle setpoint", angle_setpoint);
         double angleDifference = Pathfinder.boundHalfDegrees(angle_setpoint - gyro_heading);
+        SmartDashboard.putNumber("Angle difference", angleDifference);
 
-        double turn = DrivetrainProfiling.gp * angleDifference + (DrivetrainProfiling.gd *
+        double turn = localGp * angleDifference + (DrivetrainProfiling.gd *
                 ((angleDifference - DrivetrainProfiling.last_gyro_error) / DrivetrainProfiling.dt));
 
         DrivetrainProfiling.last_gyro_error = angleDifference;
 
-        drive(l + turn, r - turn);
+        if (left != null && !left.isFinished()) {
+            SmartDashboard.putNumber("Left diff", left.getSegment().x + this.getEncoderDistanceMetersLeft());
+            SmartDashboard.putNumber("Left set vel", left.getSegment().velocity);
+            SmartDashboard.putNumber("Left set pos", left.getSegment().x);
+            SmartDashboard.putNumber("Left calc voltage", l);
+            SmartDashboard.putNumber("Commanded seg heading", left.getHeading());
+            SmartDashboard.putNumber("Left + turn", l + turn);
+            SmartDashboard.putNumber("Left seg acceleration", left.getSegment().acceleration);
+            SmartDashboard.putNumber("Path angle offset", DrivetrainProfiling.path_angle_offset);
+            SmartDashboard.putNumber("Angle offset w/ new path angle offset", angleDifference + DrivetrainProfiling.path_angle_offset);
+        }
+        if (!reverse) {
+            drive(l + turn, r - turn);
+        } else {
+            drive(-l + turn, -r - turn);
+        }
+
+        if (left.isFinished() && right.isFinished()) {
+            isProfileFinished = true;
+            DrivetrainProfiling.path_angle_offset = 0.0;
+        }
+    }
+
+
+    public void updateAccelDashboard() {
+        SmartDashboard.putNumber("Accel X", gyro.getWorldLinearAccelX());
+        SmartDashboard.putNumber("Accel Y", gyro.getWorldLinearAccelY());
+        SmartDashboard.putNumber("Accel Z", gyro.getWorldLinearAccelZ());
     }
 
     public void initDefaultCommand() {
@@ -185,23 +332,23 @@ public void initDefaultCommand() {
 
     public static class DrivetrainProfiling {
         //TODO: TUNE CONSTANTS
-        public static double kp = 0.0; // 1.5
-        public static double kd = 0.0; // 0.775
-        public static double gp = 0.0; // 0.06
-        public static double gd = 0.0; // 0.03
-        public static double ki = 0.0; // Not Used
+        public static double kp = 0.8; //1.2;
+        public static double kd = 0.0;
+        public static double gp = 0.0375; // 0.0375
+        public static double gd = 0.0; //0.0025
+        public static double ki = 0.0;
 
-        //gyro logging
+        //Gyro logging for motion profiling
         public static double last_gyro_error = 0.0;
 
-        //hard constants TODO: UPDATE FOR 2018 CONSTANTS ARE OLD FOR 2017
-        public static final double max_velocity = 0.0; // Max is 3.2
-        public static final double kv = 1 / max_velocity;
-        public static final double max_acceleration = 0.0;
-        public static final double ka = 0.0;
-        public static final double max_jerk = 0.0;
-        public static final double wheel_diameter = 0.0;
-        public static final double wheel_base_width = 0.0;
+        public static double path_angle_offset = 0.0;
+        public static final double max_velocity = 2.2; //4 is real
+        public static final double kv = 1.0 / max_velocity; // Calculated for test Drivetrain
+        public static final double max_acceleration = 1.9; // Estimated # 3.8
+        public static final double ka = 0.05; //0.015
+        public static final double max_jerk = 8.0; // 16.0
+        public static final double wheel_diameter = 0.125;
+        public static final double wheel_base_width = 0.72;
         public static final int ticks_per_rev = 4096; // CTRE Mag Encoder
         public static final double dt = 0.02; // Calculated - Confirmed
 
@@ -221,5 +368,4 @@ public static void updatePIDG() {
             gd = SmartDashboard.getNumber("gD", 0.0);
         }
     }
-
 }
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team125/robot/subsystems/Intake.java b/src/org/usfirst/frc/team125/robot/subsystems/Intake.java
index 73c7e92..bf96381 100644
--- a/src/org/usfirst/frc/team125/robot/subsystems/Intake.java
+++ b/src/org/usfirst/frc/team125/robot/subsystems/Intake.java
@@ -1,98 +1,140 @@
 package org.usfirst.frc.team125.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import edu.wpi.first.wpilibj.*;
-import org.usfirst.frc.team125.robot.RobotMap;
-
 import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-
+import com.ctre.phoenix.motorcontrol.IMotorController;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.VictorSPX;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team125.robot.Robot;
+import org.usfirst.frc.team125.robot.RobotMap;
+import org.usfirst.frc.team125.robot.util.CurrentReader;
 import org.usfirst.frc.team125.robot.util.DebouncedBoolean;
 
 public class Intake extends Subsystem {
-	
-	//intake motors
-	private TalonSRX intakeL = new TalonSRX(RobotMap.INTAKE_LEFT);
-	private TalonSRX intakeR = new TalonSRX(RobotMap.INTAKE_RIGHT);
-	//switch to TalonSRX  when using protoBoard
-	
-	private DoubleSolenoid intakePiston = new DoubleSolenoid(RobotMap.INTAKE_RETRACT_FORWARD, RobotMap.INTAKE_RETRACT_REVERSE);
-	private Solenoid clamp = new Solenoid(RobotMap.INTAKE_CLAMP);
-
-	private DigitalInput smartIntake = new DigitalInput(RobotMap.INTAKE_LIMIT_SWITCH);
-	private static final double minimumSmartIntakeTime = 2.0; // Is 2 seconds too long???
-	private DebouncedBoolean smartIntakeDebouncer = new DebouncedBoolean(minimumSmartIntakeTime);
-
-	private static final double RIGHT_INTAKE_SPEED = 1.0;
-
-
-
-	public static final double INTAKE_POWER = 1.0;
-
-	public Intake() {
-		
-		//Left side
-		this.intakeL.configPeakOutputForward(1.0, 0);
-		this.intakeL.configPeakOutputReverse(-1.0, 0);
-		this.intakeL.configNominalOutputForward(0.0, 0);
-		this.intakeL.configNominalOutputReverse(0.0, 0);
-		
-		//Right side
-		this.intakeR.configPeakOutputForward(RIGHT_INTAKE_SPEED, 0);
-		this.intakeR.configPeakOutputReverse(-RIGHT_INTAKE_SPEED, 0);
-		this.intakeR.configNominalOutputForward(0.0, 0);
-		this.intakeR.configNominalOutputReverse(0.0, 0);
-
-		this.intakeL.setNeutralMode(NeutralMode.Coast);
-		this.intakeR.setNeutralMode(NeutralMode.Coast);
-
-		this.intakePiston.set(DoubleSolenoid.Value.kReverse); // TODO: Check if this is right...
-		this.clamp.set(false); // TODO: Check .set()
-	}
-
-	public void runIntake(double power) {
-		this.intakeL.set(ControlMode.PercentOutput, power);
-		this.intakeR.set(ControlMode.PercentOutput, power);
-	}
-	
-	public void runIntakeReversed(double power) {
-		this.intakeL.set(ControlMode.PercentOutput, power);
-		this.intakeR.set(ControlMode.PercentOutput, power);
-	}
-	
-	public void stopIntake() {
-		this.intakeL.set(ControlMode.PercentOutput, 0);
-		this.intakeR.set(ControlMode.PercentOutput, 0);
-	}
-	
-	public void openClamp(){
-		this.clamp.set(false);
-	}
-	
-	public void closeClamp(){
-		this.clamp.set(true);
-	}
-
-	public void updateCubeSwitch(boolean val) { // Its going to have to be called during all robot periodic...
-		smartIntakeDebouncer.update(val);
-		if(smartIntakeDebouncer.get()){
-			this.pistonIn();
-		} else {
-			this.pistonOut();
-		}
-	}
-	
-	public void pistonIn() {
-		this.intakePiston.set(DoubleSolenoid.Value.kForward);
-	}
-	
-	public void pistonOut() {
-		this.intakePiston.set(DoubleSolenoid.Value.kReverse);
-	}
-
-	@Override
-	protected void initDefaultCommand() {
-	}
 
+    //Intake motors
+    private IMotorController intakeL = new VictorSPX(RobotMap.INTAKE_LEFT);
+    private IMotorController intakeR = new VictorSPX(RobotMap.INTAKE_RIGHT);
+
+    private DoubleSolenoid intakeSolenoid = new DoubleSolenoid(RobotMap.INTAKE_RETRACT_FORWARD, RobotMap.INTAKE_RETRACT_REVERSE);
+
+    private DigitalInput smartIntakeA = new DigitalInput(RobotMap.INTAKE_LIMIT_SWITCH_A);
+    private DigitalInput smartIntakeB = new DigitalInput(RobotMap.INTAKE_LIMIT_SWITCH_B);
+    private static final double minimumSmartIntakeTime = 0.2; // Is 2 seconds too long???
+    private DebouncedBoolean smartIntakeDebouncerA = new DebouncedBoolean(minimumSmartIntakeTime);
+    private DebouncedBoolean smartIntakeDebouncerB = new DebouncedBoolean(minimumSmartIntakeTime);
+
+    public static final double INTAKE_POWER_LEFT = 1.0;
+    public static final double INTAKE_POWER_RIGHT = 1.0;
+    public static final double CURRENT_MAX = 110;
+
+    public CurrentReader intakeCurrentReader = new CurrentReader();
+
+    private static final DoubleSolenoid.Value INTAKE_FORWARD_VALUE = DoubleSolenoid.Value.kForward;
+    private static final DoubleSolenoid.Value INTAKE_REVERSE_VALUE = DoubleSolenoid.Value.kReverse;
+
+    private class SmartIntakeUpdater implements Runnable {
+        int j = 0;
+
+        @Override
+        public void run() {
+            while (true) {
+                try {
+                    Thread.sleep(20L);
+                } catch (InterruptedException e) {
+                    e.printStackTrace();
+                }
+                checkSmartIntakeTriggered();
+                SmartDashboard.putNumber("j counter", j);
+                j++;
+            }
+        }
+    }
+
+    public Intake() {
+        Thread thread = new Thread(new Intake.SmartIntakeUpdater());
+        thread.start();
+        //Left side
+        this.intakeL.configPeakOutputForward(INTAKE_POWER_LEFT, 0);
+        this.intakeL.configPeakOutputReverse(-INTAKE_POWER_LEFT, 0);
+        this.intakeL.configNominalOutputForward(0.0, 0);
+        this.intakeL.configNominalOutputReverse(0.0, 0);
+
+        //Right side
+        this.intakeR.configPeakOutputForward(INTAKE_POWER_RIGHT, 0);
+        this.intakeR.configPeakOutputReverse(-INTAKE_POWER_RIGHT, 0);
+        this.intakeR.configNominalOutputForward(0.0, 0);
+        this.intakeR.configNominalOutputReverse(0.0, 0);
+
+        this.intakeL.setNeutralMode(NeutralMode.Coast);
+        this.intakeR.setNeutralMode(NeutralMode.Coast);
+
+        intakePistonUp(); // TODO: Check if this is right...
+    }
+
+    public void intake() {
+        this.intakeL.set(ControlMode.PercentOutput, -INTAKE_POWER_LEFT);
+        this.intakeR.set(ControlMode.PercentOutput, INTAKE_POWER_RIGHT);
+    }
+
+    public void outtake() {
+        this.intakeL.set(ControlMode.PercentOutput, INTAKE_POWER_LEFT);
+        this.intakeR.set(ControlMode.PercentOutput, -INTAKE_POWER_RIGHT);
+    }
+
+    public void stopIntake() {
+        this.intakeL.set(ControlMode.PercentOutput, 0);
+        this.intakeR.set(ControlMode.PercentOutput, 0);
+    }
+
+    public boolean passedCurrentLimit() {
+        double intakeCurrent = 0;
+        intakeCurrent = this.intakeCurrentReader.getTotalCurrent(CurrentReader.CurrentPorts.Intake);
+        SmartDashboard.putNumber("intakeCurrent", intakeCurrent);
+
+        if (intakeCurrent > this.intakeCurrentReader.INTAKE_MAX_CURRENT) {
+            this.intakeCurrentReader.currentCounter += 1;
+        }
+        return this.intakeCurrentReader.currentCounter > this.intakeCurrentReader.COUNTER_MAX ? true : false;
+    }
+
+    public int currentCounterReset() {
+        return this.intakeCurrentReader.currentCounter = 0;
+    }
+
+    public boolean checkSmartIntakeTriggered() {
+        smartIntakeDebouncerA.update(!smartIntakeA.get());
+        smartIntakeDebouncerB.update(!smartIntakeB.get());
+        Robot.ledController.setSmartIntakeTriggered(smartIntakeDebouncerA.get() || smartIntakeDebouncerB.get());
+        SmartDashboard.putBoolean("Smart intake a", smartIntakeA.get());
+        SmartDashboard.putBoolean("Smart intake de-bouncer a", smartIntakeDebouncerA.get());
+        SmartDashboard.putBoolean("Smart intake b", smartIntakeB.get());
+        SmartDashboard.putBoolean("Smart intake de-bouncer b", smartIntakeDebouncerB.get());
+        return (smartIntakeDebouncerA.get() || smartIntakeDebouncerB.get())
+                || (!smartIntakeA.get() && !smartIntakeB.get());
+    }
+
+    public void intakePistonUp() {
+        this.intakeSolenoid.set(INTAKE_REVERSE_VALUE);
+    }
+
+    public void intakePistonDown() {
+        this.intakeSolenoid.set(INTAKE_FORWARD_VALUE);
+    }
+
+    public void toggleIntakePiston() {
+        if (intakeSolenoid.get() == INTAKE_FORWARD_VALUE) {
+            intakeSolenoid.set(INTAKE_REVERSE_VALUE);
+        }
+        if (intakeSolenoid.get() == INTAKE_REVERSE_VALUE) {
+            intakeSolenoid.set(INTAKE_FORWARD_VALUE);
+        }
+    }
+
+    @Override
+    protected void initDefaultCommand() {
+    }
 }
diff --git a/src/org/usfirst/frc/team125/robot/subsystems/LEDController.java b/src/org/usfirst/frc/team125/robot/subsystems/LEDController.java
new file mode 100644
index 0000000..a39c9d5
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/subsystems/LEDController.java
@@ -0,0 +1,76 @@
+package org.usfirst.frc.team125.robot.subsystems;
+
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team125.robot.RobotMap;
+
+public class LEDController extends Subsystem {
+
+    private Spark blinkinLED = new Spark(RobotMap.BLINKIN);
+
+    private boolean runningMotionMagic = false;
+
+    private boolean smartIntakeTriggered = false;
+
+    public void setRunningMotionMagic(boolean val) {
+        runningMotionMagic = val;
+    }
+
+    public void setSmartIntakeTriggered(boolean val) {
+        smartIntakeTriggered = val;
+    }
+
+    private class LEDAutoUpdater implements Runnable {
+        int p = 0;
+
+        @Override
+        public void run() {
+            while (true) {
+                try {
+                    Thread.sleep(20L);
+                } catch (InterruptedException e) {
+                    e.printStackTrace();
+                }
+                updateLEDStatus();
+                SmartDashboard.putNumber("p counter", p);
+                p++;
+            }
+        }
+    }
+
+    public LEDController() {
+        Thread thread = new Thread(new LEDAutoUpdater());
+        thread.start();
+    }
+
+    public void setLEDRed() {
+        blinkinLED.set(0.61);
+    }
+
+    public void setLEDStrobeRed() {
+        blinkinLED.set(-0.11);
+    }
+
+    public void setLEDGreen() {
+        blinkinLED.set(0.77);
+    }
+
+    public void setLEDYellow() {
+        blinkinLED.set(0.67);
+    }
+
+    public void updateLEDStatus() {
+        if (runningMotionMagic) {
+            setLEDStrobeRed();
+        } else if (smartIntakeTriggered) {
+            setLEDYellow();
+        } else {
+            setLEDRed();
+        }
+    }
+
+    @Override
+    protected void initDefaultCommand() {
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/CurrentReader.java b/src/org/usfirst/frc/team125/robot/util/CurrentReader.java
new file mode 100644
index 0000000..5c9e502
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/CurrentReader.java
@@ -0,0 +1,75 @@
+package org.usfirst.frc.team125.robot.util;
+
+import edu.wpi.first.wpilibj.PowerDistributionPanel;
+
+public class CurrentReader {
+
+    public static enum CurrentPorts {
+        Drivetrain,
+        Intake,
+        Cubelift
+    }
+
+    public static final double INTAKE_MAX_CURRENT = 45;
+    public static final double DRIVETRAIN_MAX_CURRENT = 5;
+    public static final double CUBELIFT_MAX_CURRENT = 4;
+
+    public static final int COUNTER_MAX = 100;
+
+    public int currentCounter = 0;
+
+    public static PowerDistributionPanel pdp = new PowerDistributionPanel();
+
+    public CurrentReader() {
+
+    }
+
+    public double getTotalCurrent(CurrentPorts newPorts) {
+        double currentVal = 0;
+        switch (newPorts) {
+            case Drivetrain:
+                currentVal = pdp.getCurrent(0) + pdp.getCurrent(1) + pdp.getCurrent(2) + pdp.getCurrent(3)
+                        + pdp.getCurrent(4) + pdp.getCurrent(6);
+                break;
+
+            case Intake:
+                currentVal = pdp.getCurrent(5) + pdp.getCurrent(10);
+                break;
+
+            case Cubelift:
+                currentVal = pdp.getCurrent(8) + pdp.getCurrent(9) + pdp.getCurrent(7) + pdp.getCurrent(11);
+                break;
+
+            default:
+                break;
+
+        }
+
+        return currentVal;
+    }
+
+    public double getAverageCurrent(CurrentPorts newPorts) {
+        double currentVal = 0;
+        switch (newPorts) {
+            case Drivetrain:
+                currentVal = (pdp.getCurrent(0) + pdp.getCurrent(1) + pdp.getCurrent(2) + pdp.getCurrent(3)
+                        + pdp.getCurrent(4) + pdp.getCurrent(5)) / 6;
+                break;
+
+            case Intake:
+                currentVal = (pdp.getCurrent(6) + pdp.getCurrent(7)) / 2;
+                break;
+
+            case Cubelift:
+                currentVal = (pdp.getCurrent(8) + pdp.getCurrent(9) + pdp.getCurrent(10) + pdp.getCurrent(11)) / 4;
+                break;
+
+            default:
+                break;
+
+        }
+
+        return currentVal;
+    }
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/DebouncedBoolean.java b/src/org/usfirst/frc/team125/robot/util/DebouncedBoolean.java
index c32f85f..8c57dfb 100644
--- a/src/org/usfirst/frc/team125/robot/util/DebouncedBoolean.java
+++ b/src/org/usfirst/frc/team125/robot/util/DebouncedBoolean.java
@@ -8,20 +8,19 @@ public class DebouncedBoolean {
     private int minimumLoops;
 
     public DebouncedBoolean(double minimumSeconds) {
-        minimumLoops = (int)Math.ceil(minimumSeconds / 0.02);
+        minimumLoops = (int) Math.ceil(minimumSeconds / 0.02);
     }
 
     public boolean get() {
-        return loopCounter >= minimumLoops;
+        return loopCounter > minimumLoops;
     }
 
     public void update(boolean value) {
-        if(value) {
+        if (value) {
             loopCounter++;
         } else {
             loopCounter = 0;
         }
         SmartDashboard.putNumber("Loop Counter", loopCounter);
     }
-    
 }
diff --git a/src/org/usfirst/frc/team125/robot/util/JoystickMap.java b/src/org/usfirst/frc/team125/robot/util/JoystickMap.java
new file mode 100644
index 0000000..92040f5
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/JoystickMap.java
@@ -0,0 +1,21 @@
+package org.usfirst.frc.team125.robot.util;
+
+public class JoystickMap {
+
+    public static final int A = 1;
+    public static final int B = 2;
+    public static final int X = 3;
+    public static final int Y = 4;
+    public static final int LB = 5;
+    public static final int RB = 6;
+    public static final int BACK = 7;
+    public static final int START = 8;
+    public static final int L3 = 9;
+    public static final int R3 = 10;
+    public static final int LEFT_TRIGGER = 2;
+    public static final int RIGHT_TRIGGER = 3;
+    public static final int LEFT_X = 0;
+    public static final int LEFT_Y = 1;
+    public static final int RIGHT_X = 4;
+    public static final int RIGHT_Y = 5;
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/PIDController.java b/src/org/usfirst/frc/team125/robot/util/PIDController.java
new file mode 100644
index 0000000..7776c1f
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/PIDController.java
@@ -0,0 +1,52 @@
+package org.usfirst.frc.team125.robot.util;
+
+public class PIDController {
+
+    private double error;
+    private double lastError;
+
+    private double integral;
+    private double kP;
+    private double kI;
+    private double kD;
+    private double kF;
+
+    private final double DT = 0.02; // In seconds so 20ms
+
+    public PIDController(double kP, double kI, double kD, double kF) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+
+        this.lastError = 0.0;
+        this.error = 0.0;
+        this.integral = 0.0;
+    }
+
+    public double updatePID(double position, double goal) {
+        error = goal - position;
+        integral += error * DT;
+        double output = (kP * error) + (kI * integral) + (kD * (error - lastError) / DT) + kF;
+
+        lastError = error;
+        return output;
+    }
+
+    public void resetController() {
+        this.lastError = 0.0;
+        this.error = 0.0;
+        this.integral = 0.0;
+    }
+
+    public void updateConstants(double kP, double kI, double kD, double kF) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+    }
+
+    public double getError() {
+        return lastError;
+    }
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/AutoPathsConstants.java b/src/org/usfirst/frc/team125/robot/util/Paths/AutoPathsConstants.java
new file mode 100644
index 0000000..e4c8bea
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/AutoPathsConstants.java
@@ -0,0 +1,5 @@
+package org.usfirst.frc.team125.robot.util.Paths;
+
+public class AutoPathsConstants {
+    public static final double DRIVETRAIN_LENGTH = 0.79375;
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/GenericPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/GenericPaths.java
new file mode 100644
index 0000000..c38c015
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/GenericPaths.java
@@ -0,0 +1,14 @@
+package org.usfirst.frc.team125.robot.util.Paths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class GenericPaths {
+
+    public static Waypoint[] path = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.0, 1.0, Pathfinder.d2r(90.0)),
+    };
+
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/CenterPaths/CenterLeftPath.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/CenterPaths/CenterLeftPath.java
new file mode 100644
index 0000000..a3aac4f
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/CenterPaths/CenterLeftPath.java
@@ -0,0 +1,29 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.CenterPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class CenterLeftPath {
+
+    public static Waypoint[] toSwitch = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(2.5, 1.2, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_goBack = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(2.5, -1.2, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] toCube = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.75, 0.0, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_backOffCube = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.75, 0.0, Pathfinder.d2r(0.0)),
+    };
+
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/CenterPaths/CenterRightPath.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/CenterPaths/CenterRightPath.java
new file mode 100644
index 0000000..907149c
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/CenterPaths/CenterRightPath.java
@@ -0,0 +1,28 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.CenterPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class CenterRightPath {
+
+    public static Waypoint[] toSwitch = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(2.5, -1.2, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_goBack = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(2.5, 1.2, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] toCube = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.75, 0.0, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_backOffCube = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.75, 0.0, Pathfinder.d2r(0.0)),
+    };
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideCloseScaleCloseSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideCloseScaleCloseSwitchPaths.java
new file mode 100644
index 0000000..41dcc61
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideCloseScaleCloseSwitchPaths.java
@@ -0,0 +1,25 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+import static org.usfirst.frc.team125.robot.util.Paths.AutoPathsConstants.DRIVETRAIN_LENGTH;
+
+public class LeftSideCloseScaleCloseSwitchPaths {
+
+    public static Waypoint[] toScale = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint((3.9784 - DRIVETRAIN_LENGTH), 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(6.5, -0.7, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.65, 0.65, Pathfinder.d2r(-90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.65, -0.75, Pathfinder.d2r(-90.0)),
+    };
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideCloseScaleFarSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideCloseScaleFarSwitchPaths.java
new file mode 100644
index 0000000..9c8d1df
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideCloseScaleFarSwitchPaths.java
@@ -0,0 +1,26 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+import static org.usfirst.frc.team125.robot.util.Paths.AutoPathsConstants.DRIVETRAIN_LENGTH;
+
+public class LeftSideCloseScaleFarSwitchPaths {
+
+    public static Waypoint[] toScale = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint((3.9784 - DRIVETRAIN_LENGTH), 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(6.5, -0.7, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.8, 0.65, Pathfinder.d2r(-90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.9, -0.75, Pathfinder.d2r(-90.0)),
+    };
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideFarScaleCloseSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideFarScaleCloseSwitchPaths.java
new file mode 100644
index 0000000..e586882
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideFarScaleCloseSwitchPaths.java
@@ -0,0 +1,27 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class LeftSideFarScaleCloseSwitchPaths {
+
+    public static Waypoint[] toScale = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.7, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(4.9, -1.2, Pathfinder.d2r(-90.0)),
+            new Waypoint(4.9, -3.2, Pathfinder.d2r(-90.0)),
+            new Waypoint(6.5, -4.2, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.8, -1.0, Pathfinder.d2r(90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.5, 0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.4, 1.9, Pathfinder.d2r(90.0)),
+    };
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideFarScaleFarSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideFarScaleFarSwitchPaths.java
new file mode 100644
index 0000000..b228af9
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/LeftSideFarScaleFarSwitchPaths.java
@@ -0,0 +1,26 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class LeftSideFarScaleFarSwitchPaths {
+
+    public static Waypoint[] toScale = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.7, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(4.9, -1.2, Pathfinder.d2r(-90.0)),
+            new Waypoint(4.9, -3.2, Pathfinder.d2r(-90.0)),
+            new Waypoint(6.5, -4.2, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.75, -1.0, Pathfinder.d2r(90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.85, 1.15, Pathfinder.d2r(90.0)),
+    };
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideCloseScaleCloseSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideCloseScaleCloseSwitchPaths.java
new file mode 100644
index 0000000..99dbc77
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideCloseScaleCloseSwitchPaths.java
@@ -0,0 +1,25 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+import static org.usfirst.frc.team125.robot.util.Paths.AutoPathsConstants.DRIVETRAIN_LENGTH;
+
+public class RightSideCloseScaleCloseSwitchPaths {
+
+    public static Waypoint[] toScale = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint((3.9784 - DRIVETRAIN_LENGTH), 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(6.5, 0.2, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.65, -0.65, Pathfinder.d2r(90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.95, 0.75, Pathfinder.d2r(90.0)),
+    };
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideCloseScaleFarSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideCloseScaleFarSwitchPaths.java
new file mode 100644
index 0000000..2342172
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideCloseScaleFarSwitchPaths.java
@@ -0,0 +1,27 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+import static org.usfirst.frc.team125.robot.util.Paths.AutoPathsConstants.DRIVETRAIN_LENGTH;
+
+public class RightSideCloseScaleFarSwitchPaths {
+
+    public static Waypoint[] toScale = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint((3.9784 - DRIVETRAIN_LENGTH), 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(6.5, 0.2, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.75, -1.0, Pathfinder.d2r(90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.5, 0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.4, 1.9, Pathfinder.d2r(90.0)),
+    };
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideFarScaleCloseSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideFarScaleCloseSwitchPaths.java
new file mode 100644
index 0000000..d3e1ef5
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideFarScaleCloseSwitchPaths.java
@@ -0,0 +1,27 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class RightSideFarScaleCloseSwitchPaths {
+
+    public static Waypoint[] toScale = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.7, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(5.2, 2.0, Pathfinder.d2r(90.0)),
+            new Waypoint(5.2, 4.0, Pathfinder.d2r(90.0)),
+            new Waypoint(6.5, 5.0, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.75, 1.0, Pathfinder.d2r(-90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(4.0, -1.0, Pathfinder.d2r(-90.0)),
+    };
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideFarScaleFarSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideFarScaleFarSwitchPaths.java
new file mode 100644
index 0000000..031b28e
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/ScaleToSwitchPaths/RightSideFarScaleFarSwitchPaths.java
@@ -0,0 +1,25 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.ScaleToSwitchPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class RightSideFarScaleFarSwitchPaths {
+
+    public static Waypoint[] toScale = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.7, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(5.2, 2.0, Pathfinder.d2r(90.0)),
+            new Waypoint(5.2, 4.0, Pathfinder.d2r(90.0)),
+            new Waypoint(6.5, 5.0, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.75, 1.0, Pathfinder.d2r(-90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.85, -1.15, Pathfinder.d2r(-90.0)),
+    };
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/LeftSideCloseSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/LeftSideCloseSwitchPaths.java
new file mode 100644
index 0000000..58eecad
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/LeftSideCloseSwitchPaths.java
@@ -0,0 +1,20 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.SwitchOnlyPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class LeftSideCloseSwitchPaths {
+
+    public static Waypoint[] toBeforeSwitchTurn = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.7, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(5.2, -1.5, Pathfinder.d2r(-90.0)),
+    };
+
+    public static Waypoint[] turnToSwitch = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.0, -1.0, Pathfinder.d2r(-90.0)),
+    };
+
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/LeftSideFarSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/LeftSideFarSwitchPaths.java
new file mode 100644
index 0000000..cd97e35
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/LeftSideFarSwitchPaths.java
@@ -0,0 +1,21 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.SwitchOnlyPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class LeftSideFarSwitchPaths {
+
+    public static Waypoint[] toBeforeSwitchTurn = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.7, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(5.2, -1.5, Pathfinder.d2r(-90.0)),
+            new Waypoint(5.2, -2.5, Pathfinder.d2r(-90.0)),
+    };
+
+    public static Waypoint[] turnToSwitch = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.0, -1.0, Pathfinder.d2r(-90.0)),
+    };
+
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/RightSideCloseSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/RightSideCloseSwitchPaths.java
new file mode 100644
index 0000000..4951157
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/RightSideCloseSwitchPaths.java
@@ -0,0 +1,20 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.SwitchOnlyPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class RightSideCloseSwitchPaths {
+
+    public static Waypoint[] toBeforeSwitchTurn = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.7, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(5.2, 1.5, Pathfinder.d2r(90.0)),
+    };
+
+    public static Waypoint[] turnToSwitch = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.0, 1.0, Pathfinder.d2r(90.0)),
+    };
+
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/RightSideFarSwitchPaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/RightSideFarSwitchPaths.java
new file mode 100644
index 0000000..e1e60ab
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/SwitchOnlyPaths/RightSideFarSwitchPaths.java
@@ -0,0 +1,21 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.SwitchOnlyPaths;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class RightSideFarSwitchPaths {
+
+    public static Waypoint[] toBeforeSwitchTurn = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.7, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(5.2, 1.5, Pathfinder.d2r(90.0)),
+            new Waypoint(5.2, 2.5, Pathfinder.d2r(90.0)),
+    };
+
+    public static Waypoint[] turnToSwitch = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(1.0, 1.0, Pathfinder.d2r(90.0)),
+    };
+
+
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/TwoScale/LeftSideCloseTwoScalePaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/TwoScale/LeftSideCloseTwoScalePaths.java
new file mode 100644
index 0000000..99e7eb7
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/TwoScale/LeftSideCloseTwoScalePaths.java
@@ -0,0 +1,35 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.TwoScale;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+import static org.usfirst.frc.team125.robot.util.Paths.AutoPathsConstants.DRIVETRAIN_LENGTH;
+
+public class LeftSideCloseTwoScalePaths {
+
+    public static Waypoint[] toScale = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint((3.9784 - DRIVETRAIN_LENGTH), 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(6.5, -0.7, Pathfinder.d2r(0.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.65, 0.65, Pathfinder.d2r(-90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.8, -0.75, Pathfinder.d2r(-90.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToScaleA = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.65, -0.75, Pathfinder.d2r(90.0)),
+    };
+
+    public static Waypoint[] kTurnToScaleB = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.65, 0.65, Pathfinder.d2r(90.0)),
+    };
+}
diff --git a/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/TwoScale/LeftSideFarTwoScalePaths.java b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/TwoScale/LeftSideFarTwoScalePaths.java
new file mode 100644
index 0000000..7a1ea47
--- /dev/null
+++ b/src/org/usfirst/frc/team125/robot/util/Paths/PalmettoPaths/TwoScale/LeftSideFarTwoScalePaths.java
@@ -0,0 +1,37 @@
+package org.usfirst.frc.team125.robot.util.Paths.PalmettoPaths.TwoScale;
+
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Waypoint;
+
+public class LeftSideFarTwoScalePaths {
+
+    public static Waypoint[] toBeforeScaleTurn = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(3.7, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(4.9, -1.75, Pathfinder.d2r(-90.0)),
+            new Waypoint(4.9, -3.75, Pathfinder.d2r(-90.0)),
+            new Waypoint(6.5, -4.75, Pathfinder.d2r(0.0)),
+    };
+    // 0.55m change from palmetto matches
+
+    public static Waypoint[] reverse_kTurnToSwitch1A = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.75, -1.0, Pathfinder.d2r(90.0)),
+    };
+
+    public static Waypoint[] kTurnToSwitch1B = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.85, 1.15, Pathfinder.d2r(90.0)),
+    };
+
+    public static Waypoint[] reverse_kTurnToScaleA = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.85, 1.15, Pathfinder.d2r(-90.0)),
+    };
+
+    public static Waypoint[] kTurnToScaleB = new Waypoint[]{
+            new Waypoint(0.0, 0.0, Pathfinder.d2r(0.0)),
+            new Waypoint(0.75, -1.0, Pathfinder.d2r(-90.0)),
+    };
+
+}