Initial Commit; Stubbed directories for FreightFrenzy, included source of UltimateGoal for quick reference, including TimeCraftersConfigurationTool library until I get it published to maven, disabled UltimateGoal engines.

TeamCode/src/main/java/org/cyberarm/container/InputChecker.java

@@ -0,0 +1,58 @@
+package org.cyberarm.container;
+
+import android.util.Log;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import java.lang.reflect.Field;
+import java.util.ArrayList;
+import java.util.HashMap;
+
+public class InputChecker {
+  private Gamepad gamepad;
+  private HashMap<String, Byte> buttons;
+  private ArrayList<String> buttonList;
+  private byte NULL    = 0,
+               PRESSED = 1,
+               RELEASED= 2;
+  public InputChecker(Gamepad gamepad) {
+    this.gamepad = gamepad;
+    buttons = new HashMap<>();
+    buttonList = new ArrayList<>();
+
+    buttonList.add("a"); buttonList.add("b"); buttonList.add("x"); buttonList.add("y");
+    buttonList.add("start"); buttonList.add("guide"); buttonList.add("back");
+    buttonList.add("left_bumper"); buttonList.add("right_bumper");
+    buttonList.add("left_stick_button"); buttonList.add("right_stick_button");
+    buttonList.add("dpad_left"); buttonList.add("dpad_right");
+    buttonList.add("dpad_up"); buttonList.add("dpad_down");
+  }
+
+  public void update() {
+    for (int i = 0; i < buttonList.size(); i++) {
+      try {
+        Field field = gamepad.getClass().getDeclaredField(buttonList.get(i));
+
+        if (field.getBoolean(gamepad)) {
+          buttons.put(buttonList.get(i), PRESSED);
+        } else {
+          if (buttons.get(buttonList.get(i)) != null && buttons.get(buttonList.get(i)) == PRESSED) {
+            buttons.put(buttonList.get(i), RELEASED);
+          }
+        }
+      } catch (NoSuchFieldException|IllegalAccessException e) {
+        e.printStackTrace();
+      }
+    }
+  }
+
+  public boolean check(String button) {
+    boolean state = false;
+    if (buttons.containsKey(button) && buttons.get(button) == RELEASED) {
+      Log.d("InputChecker","button \""+button+"\" has been released on \"gamepad"+gamepad.getGamepadId()+"\"");
+      state = true;
+      buttons.put(button, NULL);
+    }
+    return state;
+  }
+}

TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java

@@ -0,0 +1,231 @@
+package org.cyberarm.engine.V2;
+
+import android.util.Log;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import java.util.concurrent.CopyOnWriteArrayList;
+
+/**
+ * CyberarmEngine Version 2.0 | October 26th 2018
+ * AN Experimental reimplementation of GoldfishPi's original Engine system.
+ * Designed to be easily maintainable, extensible, and easy to understand.
+ */
+public abstract class CyberarmEngine extends OpMode {
+
+  public static CyberarmEngine instance;
+  //Array To Hold States
+  private CopyOnWriteArrayList<CyberarmState> cyberarmStates = new CopyOnWriteArrayList<>();
+  private int activeStateIndex = 0;
+  private boolean isRunning;
+
+  private static String TAG = "PROGRAM.ENGINE";
+  public boolean showStateChildrenListInTelemetry = false;
+
+  /**
+   * Called when INIT button on Driver Station is pushed
+   * ENSURE to call super.init() if you override this method
+   */
+  public void init() {
+    CyberarmEngine.instance = this;
+    isRunning = false;
+
+    setup();
+
+    isRunning = true;
+
+    for (CyberarmState state: cyberarmStates) {
+      initState(state);
+    }
+  }
+
+  /**
+   * Setup states for engine to use
+   * For example:
+   * <pre>
+   * @<code>
+   *   public void setup() {
+   *     addState(new TestState());
+   *     addState(new AnotherState(100, 500));
+   *   }
+   * </code>
+   * </pre>
+   */
+  public abstract void setup();
+
+  /**
+   * Called when START button on Driver Station is pushed
+   * ENSURE to call super.start() if you override this method
+   */
+  public void start() {
+    if (cyberarmStates.size() > 0) {
+      runState(cyberarmStates.get(0));
+    }
+  }
+
+  /**
+   * Engine main loop
+   * ENSURE to call super.loop() if you override this method
+   */
+  public void loop() {
+    CyberarmState state;
+
+    // Try to set state to the current state, if it fails assume that there are no states to run
+    try {
+       state = cyberarmStates.get(activeStateIndex);
+    } catch(IndexOutOfBoundsException e) {
+      // The engine is now out of states.
+      stop();
+
+      telemetry.addLine("" + this.getClass().getSimpleName() + " is out of states to run!");
+      telemetry.addLine();
+      return;
+    }
+
+      // Add telemetry to show currently running state
+    telemetry.addLine("Running state: " +state.getClass().getSimpleName() + ". State: " + activeStateIndex + " of " + (cyberarmStates.size()-1));
+    if (showStateChildrenListInTelemetry && state.hasChildren()) {
+      for(CyberarmState child: state.children) {
+        telemetry.addLine("    Child: " + child.getClass().getSimpleName() + " [" + child.children.size() + "] grandchildren");
+      }
+    }
+    telemetry.addLine();
+
+    if (state.getHasFinished() && state.childrenHaveFinished()) {
+      activeStateIndex++;
+
+      try {
+        state = cyberarmStates.get(activeStateIndex);
+        runState(state);
+      } catch(IndexOutOfBoundsException e) { /* loop will handle this in a few milliseconds */ }
+
+    } else {
+      stateTelemetry(state);
+    }
+  }
+
+  /**
+   * Stops every known state
+   */
+  @Override
+  public void stop() {
+    for (CyberarmState state: cyberarmStates) {
+      stopState(state);
+    }
+
+
+  }
+
+  /**
+   * Recursively calls telemetry() on states
+   * @param state State to get telemetry
+   */
+  private void stateTelemetry(CyberarmState state) {
+    if (!state.getHasFinished()) {
+      state.telemetry();
+    }
+
+    for(CyberarmState childState : state.children) {
+      if (!childState.getHasFinished()) {
+        stateTelemetry(childState);
+      }
+    }
+  }
+
+  /**
+   * Called when INIT button on Driver Station is pressed
+   * Recursively initiates states
+   * @param state State to initiate
+   */
+  private void initState(CyberarmState state) {
+    state.init();
+
+    for(CyberarmState childState : state.children) {
+      initState(childState);
+    }
+  }
+
+  /**
+   * Called when programs ends or STOP button on Driver Station is pressed
+   * Recursively stop states
+   * @param state State to stop
+   */
+  public void stopState(CyberarmState state) {
+    state.setHasFinished(true);
+    state.stop();
+
+    for(CyberarmState childState : state.children) {
+      stopState(childState);
+    }
+  }
+
+  /**
+   * Recursively start up states
+   * @param state State to run
+   */
+  protected void runState(CyberarmState state) {
+    final CyberarmState finalState = state;
+//    if (state.isRunning()) { return; } // Assume that we have already started running this state
+
+    new Thread(new Runnable() {
+      @Override
+      public void run() {
+        finalState.prestart();
+        finalState.start();
+        finalState.startTime = System.currentTimeMillis();
+        finalState.run();
+      }
+    }).start();
+
+    for (CyberarmState kid : state.children) {
+      runState(kid);
+    }
+  }
+
+  /**
+   * Add state to queue, will call init() on state if engine is running
+   * @param state State to add to queue
+   */
+  public CyberarmState addState(CyberarmState state) {
+    Log.i(TAG, "Adding cyberarmState "+ state.getClass());
+    cyberarmStates.add(state);
+
+    if (isRunning()) { initState(state); }
+
+    return state;
+  }
+
+  /**
+   * Inserts state after the query state plus an offset to ensure logical insertion
+   * @param query State to add state after
+   * @param state State to be inserted
+   * @return
+   */
+  public CyberarmState insertState(CyberarmState query, CyberarmState state) {
+    int index = cyberarmStates.indexOf(query) + query.insertOffset;
+    Log.i(TAG, "Adding cyberarmState "+ state.getClass());
+
+    cyberarmStates.add(index, state);
+    query.insertOffset++;
+
+    if (isRunning()) { initState(state); }
+
+    return state;
+  }
+
+  /**
+   * This will return false while Engine.setup() is executing, and be true after.
+   * @return Whether the engine main loop is running
+   */
+  public boolean isRunning() {
+    return isRunning;
+  }
+
+  /**
+   *
+   * @return The index used to lookup the current state from cyberarmStates
+   */
+  public int getActiveStateIndex() {
+    return activeStateIndex;
+  }
+}

TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java

@@ -0,0 +1,216 @@
+package org.cyberarm.engine.V2;
+
+import android.util.Log;
+
+import java.util.concurrent.CopyOnWriteArrayList;
+
+/**
+ * A State for use with CyberarmEngineV2
+ */
+public abstract class CyberarmState implements Runnable {
+
+  private volatile boolean isRunning, hasFinished;
+  public static String TAG = "PROGRAM.STATE";
+  public CyberarmEngine engine = CyberarmEngine.instance;
+  public CopyOnWriteArrayList<CyberarmState> children = new CopyOnWriteArrayList<>();
+  public long startTime = 0;
+  public int insertOffset = 1;
+
+  /**
+   * Called when INIT button on Driver Station is pushed
+   */
+  public void init() {
+  }
+
+  /**
+   * Called just before start to ensure state is in correct state
+   */
+  protected void prestart() {
+    isRunning = true;
+  }
+
+  /**
+   * Called when state has begin to run
+   */
+  public void start() {
+  }
+
+  /**
+   * Called while State is running
+   */
+  public abstract void exec();
+
+  /**
+   * State's main loop, calls exec() until hasFinished is true
+   * DO NO OVERRIDE
+   */
+  @Override
+  public void run() {
+    while (!hasFinished) {
+      exec();
+    }
+    isRunning = false;
+  }
+
+  /**
+   * Place telemetry calls in here instead of inside exec() to have them displayed correctly on the Driver Station
+   * (States update thousands of times per second, resulting in missing or weirdly formatted telemetry if telemetry is added in exec())
+   */
+  public void telemetry() {
+  }
+
+  /**
+   * Called when Engine is finished
+   */
+  public void stop() {
+  }
+
+  /**
+   * Add a state which runs in parallel with this one
+   */
+  public CyberarmState addParallelState(CyberarmState state) {
+    Log.i(TAG, "Adding " + state.getClass() + " to " + this.getClass());
+    children.add(state);
+
+    if (isRunning()) {
+      state.init();
+      engine.runState(state);
+      Log.i(TAG, "Started " + state.getClass() + " in " + this.getClass());
+    }
+
+    return state;
+  }
+
+  /**
+   * Add a state to engine which will run after this one finishes
+   */
+  public CyberarmState addState(CyberarmState state) {
+    engine.insertState(this, state);
+
+    return state;
+  }
+
+  /**
+   * Returns whether or not state has children
+   * @return True if state has children, false otherwise
+   */
+  public boolean hasChildren() {
+    return (children.size() > 0);
+  }
+
+  /**
+   * Have all of the states children finished running themselves?
+   * @return Whether or not all children have finished running
+   */
+  public boolean childrenHaveFinished() {
+    return childrenHaveFinished(children);
+  }
+
+  /**
+   * Have all of the states children finished running themselves?
+   * @param kids CopyOnWriteArrayList of children to check for hasFinished()
+   * @return Whether or not all children have finished running
+   */
+  public boolean childrenHaveFinished(CopyOnWriteArrayList<CyberarmState> kids) {
+    boolean allDone = true;
+
+      for (CyberarmState state : kids) {
+        if (!state.hasFinished) {
+          allDone = false;
+          break;
+        } else {
+          if (!state.childrenHaveFinished()) {
+            allDone = false;
+            break;
+          }
+        }
+      }
+
+    return allDone;
+  }
+
+  /**
+   *
+   * @return The number of milliseconds this state has been running for
+   */
+  public double runTime() {
+    return (System.currentTimeMillis() - startTime);
+  }
+
+  /**
+   * Set whether state has finished or not
+   * @param value
+   */
+  public void setHasFinished(boolean value) {
+    hasFinished = value;
+  }
+
+  /**
+   *
+   * @return Get value of hasFinished
+   */
+  public boolean getHasFinished() {
+    return hasFinished;
+  }
+
+  /**
+   *
+   * @return Get value of isRunning
+   */
+  public boolean isRunning() {
+    return isRunning;
+  }
+
+  /**
+   *
+   * @param timems How long to sleep in milliseconds
+   */
+  public void sleep(long timems) {
+    try {
+      Thread.sleep(timems);
+    } catch (InterruptedException e) {
+      e.printStackTrace();
+    }
+  }
+
+  /**
+   *
+   * @param width How many characters wide to be
+   * @param percentCompleted Number between 0.0 and 100.0
+   * @param bar What character to draw the completion bar with
+   * @param padding What character to draw non-completed bar with
+   * @return A string
+   */
+  public String progressBar(int width, double percentCompleted, String bar, String padding) {
+    String percentCompletedString = "" + Math.round(percentCompleted) + "%";
+    double activeWidth = (width - 2) - percentCompletedString.length();
+
+    String string = "[";
+    double completed = (percentCompleted / 100.0) * activeWidth;
+
+    for (int i = 0; i <= ((int) activeWidth); i++) {
+      if (i == ((int) activeWidth) / 2) {
+        string += percentCompletedString;
+      } else {
+        if (i <= (int) completed && (int) completed > 0) {
+          string += bar;
+        } else {
+          string += padding;
+        }
+      }
+    }
+
+    string += "]";
+    return string;
+  }
+
+  /**
+   *
+   * @param width How many characters wide to be
+   * @param percentCompleted Number between 0.0 and 100.0
+   * @return A string
+   */
+  public String progressBar(int width, double percentCompleted) {
+    return progressBar(width, percentCompleted, "=", "  ");
+  }
+}

TeamCode/src/main/java/org/cyberarm/engine/V2/README.txt

@@ -0,0 +1,28 @@
+CyberarmEngine V2 Architecture
+
+Engine
+  -> [States]
+    -> [ParallelStates]
+
+Start with an Engine and override setup():
+
+public class Engine extends CyberarmEngineV2 {
+    public void setup() {
+        addState(new State(arguments));
+    }
+}
+
+NOTE: states do not need to be passed the instance of Engine as they have a field 'cyberarmEngine'
+      which is set to CyberarmEngineV2.instance when they are created.
+
+States can have 'children' which are also States, which run in parallel with their parent. There is
+no fixed limit to how many grandchildren can exist (Children themselves can have children.):
+
+public class State extends CyberarmEngineStateV2 {
+    public init() {
+      addParallelState(new ParallelState(arguments));
+    }
+
+    public exec() {
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md

@@ -1,121 +0,0 @@
-## TeamCode Module
-
-Welcome!
-
-This module, TeamCode, is the place where you will write/paste the code for your team's
-robot controller App. This module is currently empty (a clean slate) but the
-process for adding OpModes is straightforward.
-
-## Creating your own OpModes
-
-The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.
-
-Sample opmodes exist in the FtcRobotController module.
-To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
-
-Expand the following tree elements:
- FtcRobotController / java / org.firstinspires.ftc.robotcontroller / external / samples
-
-A range of different samples classes can be seen in this folder.
-The class names follow a naming convention which indicates the purpose of each class.
-The full description of this convention is found in the samples/sample_convention.md file.
-
-A brief synopsis of the naming convention is given here:
-The prefix of the name will be one of the following:
-
-* Basic:    This is a minimally functional OpMode used to illustrate the skeleton/structure
-            of a particular style of OpMode.  These are bare bones examples.
-* Sensor:   This is a Sample OpMode that shows how to use a specific sensor.
-            It is not intended as a functioning robot, it is simply showing the minimal code
-            required to read and display the sensor values.
-* Hardware: This is not an actual OpMode, but a helper class that is used to describe
-            one particular robot's hardware devices: eg: for a Pushbot.  Look at any
-            Pushbot sample to see how this can be used in an OpMode.
-            Teams can copy one of these to create their own robot definition.
-* Pushbot:  This is a Sample OpMode that uses the Pushbot robot structure as a base.
-* Concept:	This is a sample OpMode that illustrates performing a specific function or concept.
-            These may be complex, but their operation should be explained clearly in the comments,
-            or the header should reference an external doc, guide or tutorial.
-* Library:  This is a class, or set of classes used to implement some strategy.
-            These will typically NOT implement a full OpMode.  Instead they will be included
-            by an OpMode to provide some stand-alone capability.
-
-Once you are familiar with the range of samples available, you can choose one to be the
-basis for your own robot.  In all cases, the desired sample(s) needs to be copied into
-your TeamCode module to be used.
-
-This is done inside Android Studio directly, using the following steps:
-
- 1) Locate the desired sample class in the Project/Android tree.
-
- 2) Right click on the sample class and select "Copy"
-
- 3) Expand the  TeamCode / java folder
-
- 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
-
- 5) You will be prompted for a class name for the copy.
-    Choose something meaningful based on the purpose of this class.
-    Start with a capital letter, and remember that there may be more similar classes later.
-
-Once your copy has been created, you should prepare it for use on your robot.
-This is done by adjusting the OpMode's name, and enabling it to be displayed on the
-Driver Station's OpMode list.
-
-Each OpMode sample class begins with several lines of code like the ones shown below:
-
-```
- @TeleOp(name="Template: Linear OpMode", group="Linear Opmode")
- @Disabled
-```
-
-The name that will appear on the driver station's "opmode list" is defined by the code:
- ``name="Template: Linear OpMode"``
-You can change what appears between the quotes to better describe your opmode.
-The "group=" portion of the code can be used to help organize your list of OpModes.
-
-As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
-  ``@Disabled`` annotation which has been included.
-This line can simply be deleted , or commented out, to make the OpMode visible.
-
-
-
-## ADVANCED Multi-Team App management:  Cloning the TeamCode Module
-
-In some situations, you have multiple teams in your club and you want them to all share
-a common code organization, with each being able to *see* the others code but each having
-their own team module with their own code that they maintain themselves.
-
-In this situation, you might wish to clone the TeamCode module, once for each of these teams.
-Each of the clones would then appear along side each other in the Android Studio module list,
-together with the FtcRobotController module (and the original TeamCode module).
-
-Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
-prior to clicking to the green Run arrow.
-
-Warning:  This is not for the inexperienced Software developer.
-You will need to be comfortable with File manipulations and managing Android Studio Modules.
-These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.
- 
-Also.. Make a full project backup before you start this :)
-
-To clone TeamCode, do the following:
-
-Note: Some names start with "Team" and others start with "team".  This is intentional.
-
-1)  Using your operating system file management tools, copy the whole "TeamCode"
-    folder to a sibling folder with a corresponding new name, eg: "Team0417".
-
-2)  In the new Team0417 folder, delete the TeamCode.iml file.
-
-3)  the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
-    to a matching name with a lowercase 'team' eg:  "team0417".
-
-4)  In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains
-         package="org.firstinspires.ftc.teamcode"
-    to be
-         package="org.firstinspires.ftc.team0417"
-
-5)  Add:    include ':Team0417' to the "/settings.gradle" file.
-    
-6)  Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/AutonomousEngine.java

@@ -0,0 +1,11 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.StubState;
+
+public class AutonomousEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new StubState());
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/StubState.java

@@ -0,0 +1,9 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class StubState extends CyberarmState {
+    @Override
+    public void exec() {
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java

@@ -0,0 +1,4 @@
+package org.timecrafters.FreightFrenzy.Competition.Common;
+
+public class Robot {
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/ISO8061Engine.java

@@ -0,0 +1,14 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.HardwareTesting.States.ISO8061State;
+
+@TeleOp(name = "ISO8061-Is-Best", group = "testing")
+public class ISO8061Engine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new ISO8061State());
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/ISO8061State.java

@@ -0,0 +1,15 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class ISO8061State extends CyberarmState {
+    @Override
+    public void exec() {
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("The ISO8061 Date Format is Best");
+        engine.telemetry.addLine("YYYY-MM-DD");
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/TimeCraftersConfiguration.java

@@ -0,0 +1,150 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library;
+
+import com.google.gson.Gson;
+import com.google.gson.GsonBuilder;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Settings;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.TAC;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ActionDeserializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ActionSerializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigDeserializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigSerializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigurationDeserializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigurationSerializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.GroupDeserializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.GroupSerializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.PresetsDeserializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.PresetsSerializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.SettingsDeserializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.SettingsSerializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.VariableDeserializer;
+import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.VariableSerializer;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.FileReader;
+
+public class TimeCraftersConfiguration {
+    private static final String TAG = "TCT|TCConfig";
+    private Config config;
+
+    public TimeCraftersConfiguration() {
+        Settings settings = loadSettings();
+        this.config = loadConfig(settings.config);
+    }
+
+    public TimeCraftersConfiguration(String configName) {
+        this.config = loadConfig(configName);
+    }
+
+    public Config getConfig() {
+        return config;
+    }
+
+    public Group group(String groupName) {
+        for (final Group group : config.getGroups()) {
+            if (group.name.trim().equals(groupName.trim())) {
+                return group;
+            }
+        }
+
+        throw(new RuntimeException("Failed to find a group named:\"" + groupName.trim() + "\" in config \"" + config.getName() + "\""));
+    }
+
+    public Action action(String groupName, String actionName) {
+        final Group group = group(groupName);
+
+        for (Action action : group.getActions()) {
+            if (action.name.trim().equals(actionName.trim())) {
+                return action;
+            }
+        }
+
+        throw(new RuntimeException("Failed to find an action named:\"" + actionName.trim() + "\" in group \"" + groupName.trim() + "\" in config \"" + config.getName() + "\""));
+    }
+
+    public Variable variable(String groupName, String actionName, String variableName) {
+        final Action action = action(groupName, actionName);
+
+        for (Variable variable : action.getVariables()) {
+            if (variable.name.trim().equals(variableName.trim())) {
+                return variable;
+            }
+        }
+
+        throw(new RuntimeException("Failed to find a variable named \"" + variableName.trim() + "\" in action:\"" + actionName.trim() +
+                "\" in group \"" + groupName.trim() + "\" in config \"" + config.getName() + "\""));
+    }
+
+    private Settings loadSettings() {
+        File settingsFile = new File(TAC.SETTINGS_PATH);
+
+        if (!settingsFile.exists()) {
+            throw( new RuntimeException("Unable to load settings.json, file does not exist!") );
+        }
+
+        try {
+            return gsonForSettings().fromJson(new FileReader(settingsFile), Settings.class);
+        } catch (FileNotFoundException e) {
+            throw( new RuntimeException("Unable to load settings.json") );
+        }
+    }
+
+    private Config loadConfig(String name) {
+        if (name.equals("")) {
+            throw(new RuntimeException("Cannot load a config with an empty name!"));
+        }
+
+        String path = TAC.CONFIGS_PATH + File.separator + name + ".json";
+        File configFile = new File(path);
+
+        if (configFile.exists() && configFile.isFile()) {
+            try {
+                Config config = gsonForConfig().fromJson(new FileReader(configFile), Config.class);
+                config.setName(name);
+
+                return config;
+            } catch (FileNotFoundException e) {
+                e.printStackTrace();
+                throw(new RuntimeException("Unable to find a config file named \"" + name + "\""));
+            }
+        } else {
+            throw(new RuntimeException("Unable to find a config file named \"" + name + "\""));
+        }
+    }
+
+    private Gson gsonForSettings() {
+        return new GsonBuilder()
+                .registerTypeAdapter(Settings.class, new SettingsSerializer())
+                .registerTypeAdapter(Settings.class, new SettingsDeserializer())
+                .create();
+    }
+
+    public Gson gsonForConfig() {
+        return new GsonBuilder()
+                .registerTypeAdapter(Config.class, new ConfigSerializer())
+                .registerTypeAdapter(Config.class, new ConfigDeserializer())
+
+                .registerTypeAdapter(Configuration.class, new ConfigurationSerializer())
+                .registerTypeAdapter(Configuration.class, new ConfigurationDeserializer())
+
+                .registerTypeAdapter(Group.class, new GroupSerializer())
+                .registerTypeAdapter(Group.class, new GroupDeserializer())
+
+                .registerTypeAdapter(Action.class, new ActionSerializer())
+                .registerTypeAdapter(Action.class, new ActionDeserializer())
+
+                .registerTypeAdapter(Variable.class, new VariableSerializer())
+                .registerTypeAdapter(Variable.class, new VariableDeserializer())
+
+                .registerTypeAdapter(Presets.class, new PresetsSerializer())
+                .registerTypeAdapter(Presets.class, new PresetsDeserializer())
+                .create();
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Config.java

@@ -0,0 +1,44 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.backend;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
+
+import java.util.ArrayList;
+import java.util.Date;
+
+public class Config {
+    private String name;
+    private Configuration configuration;
+    private ArrayList<Group> groups;
+    private Presets presets;
+
+    public Config(String name) {
+        this.name = name;
+        this.configuration = new Configuration(new Date(), new Date(), TAC.CONFIG_SPEC_VERSION, 0);
+        groups = new ArrayList<>();
+        presets = new Presets(new ArrayList<Group>(), new ArrayList<Action>());
+    }
+
+    public Config(Configuration configuration, ArrayList<Group> groups, Presets presets) {
+        this.configuration = configuration;
+        this.groups = groups;
+        this.presets = presets;
+    }
+
+    public String getName() { return name; }
+    public void setName(String name) { this.name = name; }
+
+    public Configuration getConfiguration() {
+        return configuration;
+    }
+
+    public Presets getPresets() {
+        return presets;
+    }
+
+    public ArrayList<Group> getGroups() {
+        return groups;
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Settings.java

@@ -0,0 +1,12 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.backend;
+
+public class Settings {
+    public String hostname, config;
+    public int port;
+
+    public Settings(String hostname, int port, String config) {
+        this.hostname = hostname;
+        this.port = port;
+        this.config = config;
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/TAC.java

@@ -0,0 +1,14 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.backend;
+
+import android.os.Environment;
+
+import java.io.File;
+
+public class TAC {
+    // TODO: Update filesystem handling
+    public static final String ROOT_PATH = Environment.getExternalStorageDirectory().getAbsolutePath() + File.separator + "TimeCrafters_Configuration_Tool",
+                               CONFIGS_PATH = ROOT_PATH + File.separator + "/configs",
+                               SETTINGS_PATH = ROOT_PATH + File.separator + "settings.json";
+
+    public static final int CONFIG_SPEC_VERSION = 2;
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Action.java

@@ -0,0 +1,18 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
+
+import java.util.ArrayList;
+
+public class Action {
+    public String name, comment;
+    public boolean enabled;
+    private ArrayList<Variable> variables;
+
+    public Action(String name, String comment, boolean enabled, ArrayList<Variable> variables) {
+        this.name = name;
+        this.comment = comment;
+        this.enabled = enabled;
+        this.variables = variables;
+    }
+
+    public ArrayList<Variable> getVariables() { return variables; }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Configuration.java

@@ -0,0 +1,20 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
+
+import java.util.Date;
+
+public class Configuration {
+    public static final String DATE_FORMAT = "yyyy-MM-dd HH:mm:ss Z";
+
+    public Date createdAt, updatedAt;
+    private int specVersion;
+    public int revision;
+
+    public Configuration(Date createdAt, Date updatedAt, int specVersion, int revision) {
+        this.createdAt = createdAt;
+        this.updatedAt = updatedAt;
+        this.specVersion = specVersion;
+        this.revision = revision;
+    }
+
+    public int getSpecVersion() { return specVersion; }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Group.java

@@ -0,0 +1,27 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
+
+import java.util.ArrayList;
+
+public class Group {
+    public String name;
+    private ArrayList<Action> actions;
+
+    public Group(String name, ArrayList<Action> actions) {
+        this.name = name;
+        this.actions = actions;
+    }
+
+    public static boolean nameIsUnique(ArrayList<Group> groups, String name) {
+        for (Group group: groups) {
+            if (group.name.equals(name)) {
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    public ArrayList<Action> getActions() {
+        return actions;
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Presets.java

@@ -0,0 +1,22 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
+
+
+import java.util.ArrayList;
+
+public class Presets {
+    private ArrayList<Group> groups;
+    private ArrayList<Action> actions;
+
+    public Presets(ArrayList<Group> groups, ArrayList<Action> actions) {
+        this.groups = groups;
+        this.actions = actions;
+    }
+
+    public ArrayList<Group> getGroups() {
+        return groups;
+    }
+
+    public ArrayList<Action> getActions() {
+        return actions;
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Variable.java

@@ -0,0 +1,92 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
+
+import android.util.Log;
+
+import java.util.Arrays;
+
+public class Variable {
+    public String name;
+    private String value;
+
+    public Variable(String name, String value) {
+        this.name = name;
+        this.value = value;
+    }
+
+    public String rawValue() {
+        return value;
+    }
+
+    public <T> T value() {
+        return valueOf(value);
+    }
+
+    public void setValue(String value) {
+        this.value = value;
+    }
+
+    @SuppressWarnings("unchecked")
+    static public <T> T valueOf(String value) {
+        String[] split = value.split("x", 2);
+//        Log.d("Variable", "valueOf split: " + Arrays.toString(split));
+
+        switch (split[0]) {
+            case "B": {
+                return (T) Boolean.valueOf(split[(split.length-1)]);
+            }
+            case "D": {
+                return (T) Double.valueOf(split[(split.length-1)]);
+            }
+            case "F": {
+                return (T) Float.valueOf(split[(split.length-1)]);
+            }
+            case "I": {
+                return (T) Integer.valueOf(split[(split.length-1)]);
+            }
+            case "L": {
+                return (T) Long.valueOf(split[(split.length-1)]);
+            }
+            case "S": {
+                String string = "";
+                int i = 0;
+                for(String str : split) {
+                    if (i == 0) { i++; continue; }
+
+                    string += str;
+                }
+                return (T) string;
+            }
+            default: {
+                return null;
+            }
+        }
+    }
+
+    static public String typeOf(String value) {
+        String[] split = value.split("x");
+
+        switch (split[0]) {
+            case "B": {
+                return "Boolean";
+            }
+            case "D": {
+                return "Double";
+            }
+            case "F": {
+                return "Float";
+            }
+            case "I": {
+                return "Integer";
+            }
+            case "L": {
+                return "Long";
+            }
+            case "S": {
+                return "String";
+            }
+            default: {
+                return "=!UNKNOWN!=";
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionDeserializer.java

@@ -0,0 +1,33 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonDeserializationContext;
+import com.google.gson.JsonDeserializer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParseException;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+
+import java.lang.reflect.Type;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class ActionDeserializer implements JsonDeserializer<Action> {
+    @Override
+    public Action deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
+        JsonObject jsonObject = json.getAsJsonObject();
+
+        final String name = jsonObject.get("name").getAsString();
+        final String comment = jsonObject.get("comment").getAsString();
+        final boolean enabled = jsonObject.get("enabled").getAsBoolean();
+        Variable[] variablesArray = context.deserialize(jsonObject.get("variables"), Variable[].class);
+
+        List<Variable> variablesList = Arrays.asList(variablesArray);
+        ArrayList<Variable> variables = new ArrayList<>(variablesList);
+
+        return new Action(name, comment, enabled, variables);
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionSerializer.java

@@ -0,0 +1,27 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonPrimitive;
+import com.google.gson.JsonSerializationContext;
+import com.google.gson.JsonSerializer;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+
+import java.lang.reflect.Type;
+
+public class ActionSerializer implements JsonSerializer<Action> {
+    @Override
+    public JsonElement serialize(Action action, Type type, JsonSerializationContext context) {
+        JsonObject container = new JsonObject();
+
+        container.add("name", new JsonPrimitive(action.name));
+        container.add("comment", new JsonPrimitive(action.comment));
+        container.add("enabled", new JsonPrimitive(action.enabled));
+        container.add("variables", context.serialize(action.getVariables().toArray(), Variable[].class));
+
+        return container;
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigDeserializer.java

@@ -0,0 +1,34 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonDeserializationContext;
+import com.google.gson.JsonDeserializer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParseException;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
+
+import java.lang.reflect.Type;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class ConfigDeserializer implements JsonDeserializer<Config> {
+    @Override
+    public Config deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
+        JsonObject jsonObject = json.getAsJsonObject();
+        JsonObject data = jsonObject.get("data").getAsJsonObject();
+
+        Configuration configuration = context.deserialize(jsonObject.get("config"), Configuration.class);
+        Group[] groupsArray = context.deserialize(data.get("groups"), Group[].class);
+        List<Group> groupsList = Arrays.asList(groupsArray);
+        ArrayList<Group> groups = new ArrayList<>(groupsList);
+
+        Presets presets = context.deserialize(data.get("presets"), Presets.class);
+
+        return new Config(configuration, groups, presets);
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigSerializer.java

@@ -0,0 +1,32 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonSerializationContext;
+import com.google.gson.JsonSerializer;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+
+import java.lang.reflect.Type;
+
+public class ConfigSerializer implements JsonSerializer<Config> {
+    @Override
+    public JsonElement serialize(Config config, Type type, JsonSerializationContext context) {
+        JsonObject container = new JsonObject();
+        JsonObject result = new JsonObject();
+        JsonObject presets = new JsonObject();
+        container.add("config", context.serialize(config.getConfiguration(), Configuration.class));
+        result.add("groups", context.serialize(config.getGroups().toArray(), Group[].class));
+
+        presets.add("groups", context.serialize(config.getPresets().getGroups().toArray(), Group[].class));
+        presets.add("actions", context.serialize(config.getPresets().getActions().toArray(), Action[].class));
+
+        result.add("presets", presets);
+        container.add("data", result);
+
+        return container;
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationDeserializer.java

@@ -0,0 +1,36 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonDeserializationContext;
+import com.google.gson.JsonDeserializer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParseException;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
+
+import java.lang.reflect.Type;
+import java.text.ParseException;
+import java.text.SimpleDateFormat;
+import java.util.Date;
+
+public class ConfigurationDeserializer implements JsonDeserializer<Configuration> {
+    @Override
+    public Configuration deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
+        JsonObject config = json.getAsJsonObject();
+
+        SimpleDateFormat dateFormat = new SimpleDateFormat(Configuration.DATE_FORMAT);
+        Date createdAt = new Date();
+        Date updatedAt = new Date();
+        try {
+            createdAt = dateFormat.parse(config.get("created_at").getAsString());
+            updatedAt = dateFormat.parse(config.get("updated_at").getAsString());
+        } catch (ParseException e) {
+            e.printStackTrace();
+        }
+
+        final int spec_version = config.get("spec_version").getAsInt();
+        final int revision = config.get("revision").getAsInt();
+
+        return new Configuration(createdAt, updatedAt, spec_version, revision);
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationSerializer.java

@@ -0,0 +1,29 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonPrimitive;
+import com.google.gson.JsonSerializationContext;
+import com.google.gson.JsonSerializer;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+
+import java.lang.reflect.Type;
+import java.text.SimpleDateFormat;
+
+public class ConfigurationSerializer implements JsonSerializer<Configuration> {
+    @Override
+    public JsonElement serialize(Configuration configuration, Type type, JsonSerializationContext context) {
+        JsonObject container = new JsonObject();
+
+        SimpleDateFormat dateFormat = new SimpleDateFormat(Configuration.DATE_FORMAT);
+
+        container.add("created_at", new JsonPrimitive(dateFormat.format(configuration.createdAt)));
+        container.add("updated_at", new JsonPrimitive(dateFormat.format(configuration.updatedAt)));
+        container.add("spec_version", new JsonPrimitive(configuration.getSpecVersion()));
+        container.add("revision", new JsonPrimitive(configuration.revision));
+
+        return container;
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupDeserializer.java

@@ -0,0 +1,33 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonDeserializationContext;
+import com.google.gson.JsonDeserializer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParseException;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
+
+import java.lang.reflect.Type;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class GroupDeserializer implements JsonDeserializer<Group> {
+    @Override
+    public Group deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
+        JsonObject jsonObject = json.getAsJsonObject();
+
+        final String name = jsonObject.get("name").getAsString();
+        Action[] actionsArray = context.deserialize(jsonObject.get("actions"), Action[].class);
+
+        List<Action> actionsList = Arrays.asList(actionsArray);
+        ArrayList<Action> actions = new ArrayList<>(actionsList);
+
+        return new Group(name, actions);
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupSerializer.java

@@ -0,0 +1,26 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonPrimitive;
+import com.google.gson.JsonSerializationContext;
+import com.google.gson.JsonSerializer;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+
+import java.lang.reflect.Type;
+
+public class GroupSerializer implements JsonSerializer<Group> {
+    @Override
+    public JsonElement serialize(Group group, Type type, JsonSerializationContext context) {
+        JsonObject container = new JsonObject();
+
+        container.add("name", new JsonPrimitive(group.name));
+        container.add("actions", context.serialize(group.getActions().toArray(), Action[].class));
+
+        return container;
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsDeserializer.java

@@ -0,0 +1,33 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonDeserializationContext;
+import com.google.gson.JsonDeserializer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParseException;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
+
+import java.lang.reflect.Type;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class PresetsDeserializer implements JsonDeserializer<Presets> {
+    @Override
+    public Presets deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
+        JsonObject jsonObject = json.getAsJsonObject();
+
+        Group[] GroupsArray = context.deserialize(jsonObject.get("groups"), Group[].class);
+        Action[] actionsArray = context.deserialize(jsonObject.get("actions"), Action[].class);
+
+        List<Group> groupsList = Arrays.asList(GroupsArray);
+        ArrayList<Group> groups = new ArrayList<>(groupsList);
+        List<Action> actionsList = Arrays.asList(actionsArray);
+        ArrayList<Action> actions = new ArrayList<>(actionsList);
+
+        return new Presets(groups, actions);
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsSerializer.java

@@ -0,0 +1,25 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonPrimitive;
+import com.google.gson.JsonSerializationContext;
+import com.google.gson.JsonSerializer;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
+
+import java.lang.reflect.Type;
+
+public class PresetsSerializer implements JsonSerializer<Presets> {
+    @Override
+    public JsonElement serialize(Presets presets, Type type, JsonSerializationContext context) {
+        JsonObject container = new JsonObject();
+
+        container.add("groups", context.serialize(presets.getGroups().toArray(), Group[].class));
+        container.add("actions", context.serialize(presets.getActions().toArray(), Action[].class));
+
+        return container;
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsDeserializer.java

@@ -0,0 +1,25 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonDeserializationContext;
+import com.google.gson.JsonDeserializer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParseException;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Settings;
+
+import java.lang.reflect.Type;
+
+public class SettingsDeserializer implements JsonDeserializer<Settings> {
+    @Override
+    public Settings deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
+        JsonObject jsonObject = json.getAsJsonObject();
+        JsonObject data = jsonObject.get("data").getAsJsonObject();
+
+        return new Settings(
+                    data.get("hostname").getAsString(),
+                    data.get("port").getAsInt(),
+                    data.get("config").getAsString()
+                );
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsSerializer.java

@@ -0,0 +1,30 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonDeserializationContext;
+import com.google.gson.JsonDeserializer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParseException;
+import com.google.gson.JsonPrimitive;
+import com.google.gson.JsonSerializationContext;
+import com.google.gson.JsonSerializer;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Settings;
+
+import java.lang.reflect.Type;
+
+public class SettingsSerializer implements JsonSerializer<Settings> {
+    @Override
+    public JsonElement serialize(Settings settings, Type type, JsonSerializationContext context) {
+        JsonObject container = new JsonObject();
+        JsonObject result = new JsonObject();
+        result.add("hostname", new JsonPrimitive(settings.hostname));
+        result.add("port", new JsonPrimitive(settings.port));
+        result.add("config", new JsonPrimitive(settings.config));
+
+        container.add("data", result);
+
+        return container;
+    }
+}
+

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableDeserializer.java

@@ -0,0 +1,28 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonDeserializationContext;
+import com.google.gson.JsonDeserializer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParseException;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+
+import java.lang.reflect.Type;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class VariableDeserializer implements JsonDeserializer<Variable> {
+    @Override
+    public Variable deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
+        JsonObject jsonObject = json.getAsJsonObject();
+
+        final String name = jsonObject.get("name").getAsString();
+        final String value = jsonObject.get("value").getAsString();
+
+        return new Variable(name, value);
+    }
+}

TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableSerializer.java

@@ -0,0 +1,23 @@
+package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
+
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonPrimitive;
+import com.google.gson.JsonSerializationContext;
+import com.google.gson.JsonSerializer;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+
+import java.lang.reflect.Type;
+
+public class VariableSerializer implements JsonSerializer<Variable> {
+    @Override
+    public JsonElement serialize(Variable variable, Type type, JsonSerializationContext context) {
+        JsonObject container = new JsonObject();
+
+        container.add("name", new JsonPrimitive(variable.name));
+        container.add("value", new JsonPrimitive(variable.rawValue()));
+
+        return container;
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrateRingBeltLoop.java

@@ -0,0 +1,58 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class CalibrateRingBeltLoop extends CyberarmState {
+
+    private Robot robot;
+    private float startingTick;
+    private int currentTick;
+    private double ticksPerLoop;
+    private boolean limit;
+    private boolean hasSeenLimit;
+    private boolean limitPrevious;
+
+    public CalibrateRingBeltLoop(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+
+        currentTick = robot.ringBeltMotor.getCurrentPosition();
+        limit = robot.magnetSensor.isPressed();
+
+        if (engine.gamepad1.x || (engine.gamepad1.a && !limit)) {
+            robot.ringBeltMotor.setPower(0.5);
+        } else {
+           robot.ringBeltMotor.setPower(0);
+        }
+
+        if (engine.gamepad1.y) {
+            robot.ringBeltMotor.setPower(-0.1);
+        } else {
+            robot.ringBeltMotor.setPower(0);
+        }
+
+        if (engine.gamepad1.b) {
+            startingTick = currentTick;
+        }
+
+        if (limit && !limitPrevious){
+            if (hasSeenLimit) {
+                ticksPerLoop = currentTick - startingTick;
+            }
+            startingTick = currentTick;
+            hasSeenLimit = true;
+        }
+        limitPrevious = limit;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("ticks", currentTick - startingTick);
+        engine.telemetry.addData("limit swich", limit );
+        engine.telemetry.addData("Clockwise", ticksPerLoop);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java

@@ -0,0 +1,25 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+@Disabled
+@TeleOp (name = "Calibration", group = "test")
+public class CalibrationEngine extends CyberarmEngine {
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        super.init();
+    }
+
+
+    @Override
+    public void setup() {
+        addState(new OdometryCalibration(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java

@@ -0,0 +1,82 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+
+public class MechanumBiasCalibrator extends CyberarmState {
+
+    private Robot robot;
+    private ArrayList<double[]> Powers = new ArrayList<>();
+    private double BiasFR;
+    private double BiasFL;
+    private double BiasBR;
+    private double BiasBL;
+    private boolean hasCalculated;
+
+
+    public MechanumBiasCalibrator(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        if (engine.gamepad1.x) {
+            double[] mecanumPowers = robot.getMecanumPowers(-45, 0.5, 0);
+
+            
+            robot.driveFrontLeft.setPower(mecanumPowers[0]);
+            robot.driveFrontRight.setPower(mecanumPowers[1]);
+            robot.driveBackLeft.setPower(mecanumPowers[2]);
+            robot.driveBackRight.setPower(mecanumPowers[3]);
+
+            Powers.add(mecanumPowers);
+        } else {
+            robot.setDrivePower(0,0,0,0 );
+        }
+
+        if (engine.gamepad1.y && !hasCalculated) {
+            hasCalculated = true;
+
+            double sumFR = 0;
+            double sumFL = 0;
+            double sumBR = 0;
+            double sumBL = 0;
+
+            for (double[] powers : Powers) {
+                sumFL+= Math.abs(powers[0]);
+                sumFR+= Math.abs(powers[1]);
+                sumBL+= Math.abs(powers[2]);
+                sumBR+= Math.abs(powers[3]);
+            }
+
+            int length = Powers.size();
+            BiasFR = sumFR / length;
+            BiasFL = sumFL / length;
+            BiasBR = sumBR / length;
+            BiasBL = sumBL / length;
+
+            double max = Math.max(Math.max(BiasFL,BiasFR),Math.max(BiasBL,BiasBR));
+
+            BiasFL = BiasFL /max;
+            BiasFR = BiasFR /max;
+            BiasBL = BiasBL /max;
+            BiasBR = BiasBR /max;
+
+        } else if (!engine.gamepad1.y) {
+            hasCalculated = false;
+        }
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("FrontLeft", BiasFL);
+        engine.telemetry.addData("FrontRight", BiasFR);
+        engine.telemetry.addData("BackLeft", BiasBL);
+        engine.telemetry.addData("BackRight", BiasBR);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MotorCheck.java

@@ -0,0 +1,59 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Engagable;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.DriveMotor;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class MotorCheck extends CyberarmState {
+
+    private Robot robot;
+    private double[] Powers;
+    private DcMotor[] Motors;
+    private int currentMotor = 0;
+    private double currentPower = 0.001;
+
+    private boolean yPrevious;
+
+
+    public MotorCheck(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
+    }
+
+    @Override
+    public void start() {
+        Motors[currentMotor].setPower(0.2);
+    }
+
+    @Override
+    public void exec() {
+
+        if (currentMotor <= 3) {
+
+            boolean y = engine.gamepad1.y;
+            if (y && !yPrevious) {
+
+                Motors[currentMotor].setPower(0);
+                currentMotor += 1;
+                Motors[currentMotor].setPower(0.2);
+                sleep(20);
+            }
+            yPrevious = y;
+        }
+
+    }
+
+    @Override
+    public void telemetry() {
+
+        engine.telemetry.addData("current Motor", Motors[currentMotor].toString());
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java

@@ -0,0 +1,74 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class OdometryCalibration extends CyberarmState {
+
+    private Robot robot;
+    private float rotation = 0;
+    private float rotationPrev = 0;
+    private int currentTick;
+    private double ticksPerDegreeClockwise;
+    private double ticksPerDegreeCounterClockwise;
+    private long timePrevious;
+    private long timeChange;
+
+    public OdometryCalibration(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        long timeCurrent = System.currentTimeMillis();
+        timeChange = timeCurrent - timePrevious;
+
+        if (timeChange >= 1200) {
+            timePrevious = timeCurrent;
+
+
+            double power = 0.25;
+            float imu = -robot.imu.getAngularOrientation().firstAngle;
+            rotation += robot.getRelativeAngle(imu, rotationPrev);
+            rotationPrev = imu;
+
+            currentTick = (robot.encoderRight.getCurrentPosition() - robot.encoderLeft.getCurrentPosition()) / 2;
+
+            if (engine.gamepad1.x) {
+                robot.setDrivePower(power, -power, power, -power);
+                ticksPerDegreeClockwise = currentTick / rotation;
+            } else if (engine.gamepad1.y) {
+                robot.setDrivePower(-power, power, -power, power);
+                ticksPerDegreeCounterClockwise = currentTick / rotation;
+            } else {
+                robot.setDrivePower(0, 0, 0, 0);
+            }
+        }
+
+        if (engine.gamepad1.b) {
+            robot.record("Clockwise : "+ticksPerDegreeClockwise);
+            robot.record("Counter Clockwise : "+ticksPerDegreeCounterClockwise);
+            robot.saveRecording();
+            setHasFinished(true);
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");
+        engine.telemetry.addData("Rotation ", robot.getRotation());
+//
+//        engine.telemetry.addData("degrees", rotation);
+        engine.telemetry.addData("ticks", robot.encoderRight.getCurrentPosition() );
+        engine.telemetry.addData("Clockwise", ticksPerDegreeClockwise);
+        engine.telemetry.addData("CounterClockwise", ticksPerDegreeCounterClockwise);
+//        engine.telemetry.addData("forward", robot.forwardVector);
+//        engine.telemetry.addData("sideways", robot.sidewaysVector);
+    }
+
+    private float round(double number,double unit) {
+        return (float) (Math.floor(number/unit) * unit);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java

@@ -0,0 +1,32 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+
+public class PerformanceTest extends CyberarmState {
+
+    private Robot robot;
+    private ArrayList<Float> angles = new ArrayList<Float>();
+
+
+    public PerformanceTest(Robot robot) {
+        this.robot = robot;
+    }
+
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+//        robot.record();
+
+        if (engine.gamepad1.x) {
+            double[] powers = robot.getMecanumPowers(0, 1, 0);
+            robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
+        } else {
+            robot.setDrivePower(0,0,0,0);
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java

@@ -0,0 +1,122 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.Pause;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.WobbleArm;
+import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
+
+import java.util.ArrayList;
+
+@Disabled
+@Autonomous(name = "Autonomous")
+public class AutoEngine extends CyberarmEngine {
+
+    private Robot robot;
+    private TensorFlowCheck tensorFlowCheck;
+
+    private double launchTolerance;
+    private double launchPower;
+    private long launchBrakeTime;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        robot.webCamServo.setPosition(Robot.CAM_SERVO_DOWN);
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+        robot.wobbleArmMotor.setTargetPosition(0);
+        robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+        // since we've preloaded three rings, the ring belt stage is set to account for this;
+        robot.ringBeltStage = 3;
+
+        //Autonomous specific Configuration Variables
+        float rotation = robot.stateConfiguration.variable("system", "startPos", "direction").value();
+        double locationX = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "x").value());
+        double locationY = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "y").value());
+        robot.setLocalization(rotation,locationX,locationY);
+
+        launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
+        launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
+        launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value();
+
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        //drive to view
+        addState(new DriveToCoordinates(robot, "auto", "01_0"));
+
+        //face ring
+        addState(new Face(robot, "auto", "01_1"));
+
+        //select scoring area
+        tensorFlowCheck = new TensorFlowCheck(robot, "auto", "02_0");
+        addState(tensorFlowCheck);
+
+        //drive around ring stack
+        addState(new DriveToCoordinates(robot, "auto", "03_0"));
+
+        //drive to launch position
+        addState(new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime));
+
+        //aligns to goal
+        addState(new Face(robot, "auto", "04_1"));
+
+        //launch rings and drive to scoreing area. LaunchDriveControl allows makes the robot begin
+        //driving while the belt is resetting
+        CyberarmState launchState = new Launch(robot, "auto", "04_2");
+
+        CyberarmState driveState = new DriveToCoordinates(robot, "auto", "05_0", true);
+        addState(new LaunchDriveControl(robot,launchState,driveState));
+
+        //turn arm towards scoreing area.
+        ArrayList<CyberarmState> threadStates0 = new ArrayList<>();
+         threadStates0.add(new Face(robot, "auto", "05_1"));
+        threadStates0.add(new WobbleArm(robot, "auto", "05_2",robot.wobbleDownPos));
+        addState(new ThreadStates(threadStates0));
+
+        //open the wobble grab arm
+        addState(new WobbleGrab(robot, "auto", "06_0", true));
+
+        //drive to second wobble
+        addState(new DriveToCoordinates(robot, "auto","06_1"));
+
+        addState(new DriveToCoordinates(robot, "auto", "07_0"));
+        addState(new Face(robot,"auto","07_1"));
+
+        addState(new FindWobbleGoal(robot, "auto", "08_0"));
+        addState(new Pause(robot,"auto","09_0"));
+
+        //drive to scoring area
+        ArrayList<CyberarmState> threadStates1 = new ArrayList<>();
+//        threadStates1.add(new Face(robot, "auto", "09_0"));
+        threadStates1.add(new WobbleArm(robot, "auto", "09_1",robot.wobbleUpPos));
+        threadStates1.add(new DriveToCoordinates(robot, "auto", "10_0", true));
+        addState(new ThreadStates(threadStates1));
+
+
+
+        ArrayList<CyberarmState> threadStates2 = new ArrayList<>();
+        threadStates2.add(new Face(robot, "auto", "11_0"));
+        threadStates2.add(new WobbleArm(robot, "auto", "11_1",robot.wobbleDownPos));
+        addState(new ThreadStates(threadStates2));
+
+        //release wobble goal 2
+        addState(new WobbleGrab(robot, "auto", "12_0", true));
+
+        //drive to park
+        ArrayList<CyberarmState> threadStates3 = new ArrayList<>();
+        threadStates3.add(new WobbleArm(robot, "auto", "12_2", 0));
+        threadStates3.add(new Park(robot,"auto","13_0"));
+        addState(new ThreadStates(threadStates3));
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java

@@ -0,0 +1,141 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class DriveToCoordinates extends CyberarmState {
+
+    private Robot robot;
+    private String groupName = "manual";
+    private String actionName;
+    private double xTarget;
+    private double yTarget;
+    private float faceAngle;
+    private double tolerancePos;
+    private double basePower;
+    private boolean braking;
+    private long breakStartTime;
+    private long brakeTime;
+    private boolean autoFace;
+    private double decelRange;
+    private double decelMin;
+    private boolean scoringArea;
+
+    public DriveToCoordinates(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    public DriveToCoordinates(Robot robot, String groupName, String actionName, boolean scoringArea) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+        this.scoringArea = scoringArea;
+    }
+
+    public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime) {
+        this.robot = robot;
+        this.xTarget = xTarget;
+        this.yTarget = yTarget;
+        this.faceAngle = faceAngle;
+        this.tolerancePos = tolerance;
+        this.basePower = power;
+        this.brakeTime = brakeTime;
+    }
+
+    public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime, double  decelRange, double decelMin) {
+        this.robot = robot;
+        this.xTarget = xTarget;
+        this.yTarget = yTarget;
+        this.faceAngle = faceAngle;
+        this.tolerancePos = tolerance;
+        this.basePower = power;
+        this.brakeTime = brakeTime;
+        this.decelRange = decelRange;
+        this.decelMin = decelMin;
+    }
+
+    @Override
+    public void init() {
+        if (!groupName.equals("manual")) {
+            xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
+            yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
+            basePower = robot.stateConfiguration.variable(groupName, actionName, "power").value();
+            tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
+            brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
+
+            try {
+                faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
+            } catch (RuntimeException  e) {
+                autoFace = true;
+            }
+
+            try {
+                decelRange = robot.stateConfiguration.variable(groupName, actionName, "decelR").value();
+                decelMin = robot.stateConfiguration.variable(groupName, actionName, "decelM").value();
+            } catch (RuntimeException e) {
+                decelRange = 0;
+            }
+        }
+
+        if (decelRange > 0) {
+            decelRange = robot.inchesToTicks(decelRange);
+        }
+
+    }
+
+    @Override
+    public void start() {
+        if (!groupName.equals("manual")) {
+            setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
+
+            //used to navigate towards the randomly generated scoreing area. the original target
+            //becomes an offset of the scoring area position.
+            if (scoringArea) {
+                xTarget += robot.wobbleScoreX;
+                yTarget += robot.wobbleScoreY;
+            }
+
+            if (faceAngle == 360) {
+                faceAngle = robot.getRelativeAngle(180, robot.getAngleToPosition(xTarget,yTarget));
+            }
+        }
+
+        if (autoFace) {
+            faceAngle = robot.getAngleToPosition(xTarget,yTarget);
+        }
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY());
+
+        // deceleration
+        double power = basePower;
+        if (distanceToTarget < decelRange) {
+            power = ((distanceToTarget / decelRange) * (basePower - decelMin)) + decelMin;
+        }
+
+        double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
+        robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
+
+        //Once the robot is within the acceptable range,
+        if (distanceToTarget > tolerancePos) {
+            braking = false;
+        } else {
+            long currentTime = System.currentTimeMillis();
+            if (!braking) {
+                breakStartTime = currentTime;
+                braking = true;
+            }
+            if (currentTime - breakStartTime >= brakeTime) {
+                robot.setDrivePower(0,0,0,0);
+                setHasFinished(true);
+            }
+        }
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java

@@ -0,0 +1,46 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class DriveWithColorSensor extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private double power;
+    private double minimum;
+    private double maximum;
+
+    public DriveWithColorSensor(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    @Override
+    public void init() {
+        power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
+        minimum = robot.stateConfiguration.variable(groupName,actionName,"min").value();
+        maximum = robot.stateConfiguration.variable(groupName,actionName,"max").value();
+    }
+
+    @Override
+    public void start() {
+        setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled);
+    }
+
+    @Override
+    public void exec() {
+        double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
+        if (sensorValue < minimum) {
+            robot.getMecanumPowers(180,power,0);
+        } else if (sensorValue > maximum) {
+            robot.getMecanumPowers(0,power,0);
+        } else {
+            robot.setDrivePower(0,0,0,0);
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java

@@ -0,0 +1,95 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class Face extends CyberarmState {
+
+    private Robot robot;
+    private String groupName = "manual";
+    private String actionName;
+    private float faceAngle;
+    private float toleranceFace;
+    private double power;
+    private boolean braking;
+    private long breakStartTime;
+    private long breakTime;
+    private boolean autoFace;
+
+    public Face(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    public Face(Robot robot, float faceAngle, float toleranceFace, double power, long breakTime) {
+        this.robot = robot;
+        this.faceAngle = faceAngle;
+        this.toleranceFace = toleranceFace;
+        this.power = power;
+        this.breakTime = breakTime;
+    }
+
+    @Override
+    public void init() {
+
+        if (!groupName.equals("manual")) {
+            power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
+            toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value();
+            breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
+
+            try {
+                faceAngle = robot.stateConfiguration.variable(groupName, actionName, "faceAngle").value();
+            } catch (RuntimeException e) {
+                autoFace = true;
+            }
+        }
+    }
+
+    @Override
+    public void start() {
+
+        if (!groupName.equals("manual")) {
+            setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
+
+
+            if (autoFace) {
+                double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value());
+                double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value());
+                faceAngle = robot.getAngleToPosition(x, y);
+            }
+
+            if (faceAngle == 360) {
+                double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value());
+                double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value());
+                faceAngle = robot.getRelativeAngle(180,robot.getAngleToPosition(x, y));
+            }
+        }
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+        double[] powers = robot.getFacePowers(faceAngle,power);
+        robot.setDrivePower(powers[0], powers[1],powers[0],powers[1]);
+        if (Math.abs(robot.getRelativeAngle(faceAngle,robot.getRotation())) > toleranceFace) {
+            braking = false;
+        } else {
+            long currentTime = System.currentTimeMillis();
+            if (!braking) {
+                breakStartTime = currentTime;
+                braking = true;
+            }
+            if (currentTime - breakStartTime >= breakTime) {
+                setHasFinished(true);
+                robot.setDrivePower(0,0,0,0);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("target angle", faceAngle);
+        engine.telemetry.addData("current angle", robot.getRotation());
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FaceAndDrive.java

@@ -0,0 +1,56 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+
+public class FaceAndDrive extends CyberarmState {
+
+    private Robot robot;
+    private String groupName = "manual";
+    private String actionName;
+    private double xTarget;
+    private double yTarget;
+    private float faceAngle;
+    private double tolerancePos;
+    private double power;
+    private long brakeTime;
+    private float toleranceFace;
+
+    public FaceAndDrive(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    @Override
+    public void init() {
+
+            xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
+            yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
+            power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
+            tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
+            toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value();
+            brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
+
+            try {
+                faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
+            } catch (NullPointerException e) {
+                faceAngle = robot.getAngleToPosition(xTarget,yTarget);
+            }
+
+
+    }
+
+    @Override
+    public void start() {
+        addState(new Face(robot, faceAngle, toleranceFace, power, brakeTime));
+        addState(new DriveToCoordinates(robot,xTarget,yTarget,faceAngle,tolerancePos,power,brakeTime));
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(true);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java

@@ -0,0 +1,109 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+/*
+The FindWobbleGoal state is used in teleOp and Autonomous to aid in capturing the wobble goal.
+*/
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class FindWobbleGoal extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private double power;
+    private double turnCheck;
+    private double driveCheck;
+    private double range;
+    private boolean cCheckPrev;
+    private boolean ccCheckPrev;
+    private float startRotation;
+    private boolean foundGoalRotation;
+    private float wobbleGoalRotation;
+    private long timeLimit;
+
+    public FindWobbleGoal(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    @Override
+    public void init() {
+        power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
+        turnCheck = robot.stateConfiguration.variable(groupName,actionName,"max").value();
+        driveCheck = robot.stateConfiguration.variable(groupName,actionName,"min").value();
+        range = robot.stateConfiguration.variable(groupName,actionName,"r").value();
+        timeLimit = robot.stateConfiguration.variable(groupName,actionName,"limit").value();
+    }
+
+    @Override
+    public void start() {
+        setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled);
+        startRotation = robot.getRotation();
+        robot.setDrivePower(power,-power,power,-power);
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+        double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
+
+        //Stage 1: scan back and forth untile the sensor is in line with the wobble goal.
+        if (sensorValue > turnCheck) {
+
+            float rotation = robot.getRelativeAngle(startRotation,robot.getRotation());
+
+            boolean cCheck = rotation > range;
+            boolean ccCheck = rotation < -range;
+
+            if (cCheck && !cCheckPrev) {
+                robot.setDrivePower(-power, power, -power, power);
+            }
+
+            if (ccCheck && !ccCheckPrev) {
+                robot.setDrivePower(power, -power, power, -power);
+            }
+            cCheckPrev = cCheck;
+            ccCheckPrev = ccCheck;
+
+        } else {
+            //Stage 2: drive toward wobble goal until it's close enough to grab
+             if (sensorValue > driveCheck) {
+                 if (!foundGoalRotation) {
+                     foundGoalRotation = true;
+                     wobbleGoalRotation = robot.getRotation();
+                 }
+                 double[] powers = robot.getMecanumPowers(
+                         wobbleGoalRotation - 90,
+                         power*2 , wobbleGoalRotation);
+                 robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
+             } else {
+                 //stage 3: grab the wobble goal && finish the state
+                 endSearch();
+             }
+        }
+
+        //if the search takes too long, the robot grabs and finishes the state
+        if (runTime() > timeLimit) {
+            endSearch();
+        }
+    }
+
+    private void endSearch() {
+        robot.setDrivePower(0,0,0,0);
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+        setHasFinished(true);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("sensor", robot.wobbleColorSensor.getDistance(DistanceUnit.MM));
+        engine.telemetry.addData("red", robot.wobbleColorSensor.red());
+        engine.telemetry.addData("green", robot.wobbleColorSensor.green());
+        engine.telemetry.addData("blue", robot.wobbleColorSensor.blue());
+    }
+}
+

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/LaunchDriveControl.java

@@ -0,0 +1,41 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class LaunchDriveControl extends CyberarmState {
+
+    private Robot robot;
+    private CyberarmState launchState;
+    private CyberarmState driveState;
+    private boolean checkConditionPrev;
+
+    public LaunchDriveControl(Robot robot, CyberarmState launchState, CyberarmState driveState) {
+        this.robot = robot;
+        this.launchState = launchState;
+        this.driveState = driveState;
+    }
+
+    @Override
+    public void start() {
+        addParallelState(launchState);
+    }
+
+    @Override
+    public void exec() {
+        boolean checkCondition = (robot.ringBeltStage > 3);
+        if (checkCondition && !checkConditionPrev) {
+            addParallelState(driveState);
+        }
+        checkConditionPrev = checkCondition;
+
+        setHasFinished(childrenHaveFinished());
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("ring belt stage", robot.ringBeltStage);
+        engine.telemetry.addData("check prev", checkConditionPrev);
+        engine.telemetry.addData("drive  prev", checkConditionPrev);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java

@@ -0,0 +1,50 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+
+public class Park extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    double parkY;
+    float parkFaceAngle;
+    double parkTolerance;
+    double parkPower;
+    long parkBrakeTime;
+    double  parkDecelRange;
+    double parkDecelMin;
+
+    public Park(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    @Override
+    public void init() {
+        parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"yPos").value());
+        parkFaceAngle = robot.stateConfiguration.variable(groupName,actionName,"face").value();
+        parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto",actionName,"tolPos").value());
+        parkPower = robot.stateConfiguration.variable(groupName,actionName,"power").value();
+        parkBrakeTime = robot.stateConfiguration.variable(groupName,actionName,"brakeMS").value();
+        parkDecelMin = robot.stateConfiguration.variable(groupName,actionName,"decelM").value();
+        parkDecelRange = robot.stateConfiguration.variable(groupName,actionName,"decelR").value();
+    }
+
+    @Override
+    public void start() {
+        if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8)) {
+            addParallelState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower, parkBrakeTime,parkDecelRange,parkDecelMin));
+        }
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(childrenHaveFinished());
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java

@@ -0,0 +1,94 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class TensorFlowCheck extends CyberarmState {
+
+    private Robot robot;
+    private String group;
+    private String action;
+    private List<Recognition> recognitions;
+    private int path = 0;
+    private long checkTime;
+    public double wobblePosX;
+    public double wobblePosY;
+    private int manualPath;
+    private String status;
+
+    public TensorFlowCheck(Robot robot, String group, String action) {
+        this.robot = robot;
+        this.group = group;
+        this.action = action;
+    }
+
+    @Override
+    public void init() {
+        checkTime = robot.stateConfiguration.variable(group,action,"time").value();
+        manualPath = robot.stateConfiguration.variable(group,action,"path").value();
+    }
+
+    @Override
+    public void start() {
+        if (!robot.stateConfiguration.action(group,action).enabled) {
+            manualPath = -1;
+        } else {
+            robot.tfObjectDetector.activate();
+        }
+    }
+
+    @Override
+    public void exec() {
+
+        if (runTime() < checkTime) {
+            if (manualPath != -1) {
+                recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
+
+                if (recognitions != null) {
+                    if (recognitions.size() == 1) {
+                        String label = recognitions.get(0).getLabel();
+                        if (label.equals("Single")) {
+                            path = 1;
+                        } else if (label.equals("Quad")) {
+                            path = 2;
+                        }
+                    } else if (recognitions.size() > 1) {
+                        path = 1;
+                    }
+                }
+
+            } else {
+                path = manualPath;
+            }
+        } else {
+            robot.tfObjectDetector.deactivate();
+
+            if (path == 0) {
+                robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","x").value());
+                robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","y").value());
+            }
+            if (path == 1) {
+                robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","x").value());
+                robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","y").value());
+            }
+             if (path == 2) {
+                 robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","x").value());
+                 robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","y").value());
+             }
+             // make the servo look up once we're done using tensorFlow
+             robot.webCamServo.setPosition(0);
+             robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+
+             setHasFinished(true);
+         }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Chosen Path", path);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/ThreadStates.java

@@ -0,0 +1,44 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+import java.util.ArrayList;
+
+public class ThreadStates extends CyberarmState {
+
+    private ArrayList<CyberarmState> states = new ArrayList<CyberarmState>();
+
+    public ThreadStates(ArrayList<CyberarmState> states) {
+        this.states = states;
+    }
+
+    @Override
+    public void start() {
+        for (CyberarmState state : states) {
+            addParallelState(state);
+        }
+    }
+
+    @Override
+    public void exec() {
+        int finishedStates = 0;
+        for (CyberarmState state : states) {
+            if (state.getHasFinished()) {
+                finishedStates += 1;
+            }
+        }
+        setHasFinished(finishedStates == states.size());
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Threaded States");
+        for (CyberarmState child: children) {
+            engine.telemetry.addLine(""+child.getClass());
+            if (child.getHasFinished()) {
+                engine.telemetry.addLine("finished");
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java

@@ -0,0 +1,36 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.TeleOp.TeleOpState;
+
+@Disabled
+@TeleOp (name = "Demo: Game",group = "demo")
+public class DemoEngine extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+        robot.webCamServo.setPosition(0);
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        addState(new DemoState(robot));
+    }
+
+    @Override
+    public void stop() {
+        robot.deactivateVuforia();
+        robot.saveRecording();
+        super.stop();
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngineTank.java

@@ -0,0 +1,35 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+@Disabled
+@TeleOp (name = "Demo: Tank",group = "demo")
+public class DemoEngineTank extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+        robot.webCamServo.setPosition(0);
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        addState(new DemoStateTank(robot));
+    }
+
+    @Override
+    public void stop() {
+        robot.deactivateVuforia();
+        robot.saveRecording();
+        super.stop();
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoState.java

@@ -0,0 +1,194 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+/*
+The Player1 state has all the controls for player one. The Player One and Player Two controls are
+kept in separate states so that the childrenHaveFinished() method can be used to easily stop things
+from running at the same time that shouldn't be.
+*/
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl;
+
+public class DemoState extends CyberarmState {
+    private Robot robot;
+
+    //normal drive control
+    private float leftJoystickDegrees;
+    private double leftJoystickMagnitude;
+    private float rightJoystickDegrees;
+    private double rightJoystickMagnitude;
+    private double rightJoystickMagnitudePrevious;
+
+    private double faceControlThreshold;
+    private float cardinalSnapping;
+    private float pairSnapping;
+
+    private float faceDirection = 0;
+    private double[] powers = {0,0,0,0};
+    private double drivePower = 1;
+    private boolean lbPrev;
+
+    private double refinePower;
+
+    public DemoState(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        cardinalSnapping = robot.stateConfiguration.variable(
+                "tele","control", "cardinalSnapping").value();
+        pairSnapping = robot.stateConfiguration.variable(
+                "tele","control", "pairSnapping").value();
+        faceControlThreshold = robot.stateConfiguration.variable(
+                "tele","control", "faceControlT").value();
+        refinePower = robot.stateConfiguration.variable(
+                "tele","control", "refPower").value();
+
+    }
+
+    @Override
+    public void start() {
+        faceDirection = robot.getRotation();
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        Gamepad gamepad = engine.gamepad1;
+        if (engine.gamepad2.right_trigger != 0) {
+            gamepad = engine.gamepad2;
+        }
+
+        //toggle for drive speed
+        boolean lb = engine.gamepad2.left_stick_button;
+        if (lb && !lbPrev) {
+            if (drivePower == 1) {
+                drivePower = 0.5;
+            } else {
+                drivePower = 1;
+            }
+        }
+        lbPrev = lb;
+
+        //Calculate Joystick Vector
+        double leftJoystickX = gamepad.left_stick_x;
+        double leftJoystickY = gamepad.left_stick_y;
+
+        leftJoystickDegrees = robot.getRelativeAngle(0,
+                (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
+        leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
+
+        double rightJoystickX = gamepad.right_stick_x;
+        double rightJoystickY = gamepad.right_stick_y;
+
+        rightJoystickDegrees = robot.getRelativeAngle(0,
+                (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
+        rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
+
+        //allows the the driver to indicate which direction the robot is currently looking
+        //so that the controller and robot can be re-synced in the event of a bad initial
+        //rotation.
+        if (gamepad.back) {
+            float newRotation = 0;
+
+            if (rightJoystickMagnitude != 0) {
+                newRotation = rightJoystickDegrees;
+            }
+
+            robot.setLocalization(newRotation, robot.getLocationX(), robot.getLocationY());
+            faceDirection = newRotation;
+        }
+
+        //if the driver is letting go of the face joystick, the robot should maintain it's
+        //current face direction.
+        if (rightJoystickMagnitude > faceControlThreshold ||
+                rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) {
+
+            //if the joystick is close to one of the cardinal directions, it is set to exactly
+            // the cardinal direction
+            faceDirection = snapToCardinal(rightJoystickDegrees, cardinalSnapping, 0);
+        }
+        rightJoystickMagnitudePrevious = rightJoystickMagnitude;
+
+        //The D-pad is used if the drivers need to get a more precise angle than they can get
+        //using the face joystick
+        if (gamepad.dpad_right) {
+            powers = new double[]{refinePower, -refinePower, refinePower, -refinePower};
+            faceDirection = robot.getRotation();
+        } else if (gamepad.dpad_left) {
+            powers = new double[]{-refinePower, refinePower, -refinePower, refinePower};
+            faceDirection = robot.getRotation();
+        } else if (leftJoystickMagnitude == 0) {
+            double[] facePowers = robot.getFacePowers(faceDirection, 0.4);
+            powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
+        } else {
+            //drives the robot in the direction of the move joystick while facing the direction
+            //of the look joystick. if the move direction is almost aligned/perpendicular to the
+            //look joystick,
+            powers = robot.getMecanumPowers(
+                    snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection),
+                    drivePower, faceDirection);
+        }
+
+        robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
+
+
+        //LED feedback control
+        double ringBeltPower = robot.ringBeltMotor.getPower();
+        if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() -
+                robot.ringBeltMotor.getCurrentPosition()) > 10) {
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE );
+        } else if (ringBeltPower < 0) {
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED);
+        }  else {
+            if (drivePower == 1) {
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
+            } else {
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GOLD);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Controler Directions");
+        engine.telemetry.addData("Right", rightJoystickDegrees);
+        engine.telemetry.addData("Left", leftJoystickDegrees);
+
+        engine.telemetry.addData("face", faceDirection);
+
+        engine.telemetry.addData("Player 1 children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+
+    //This just checks if the wobble arm runmode is already the desired mode before setting it.
+    //I don't know if this is actually helpful
+    private void setArmMode(DcMotor.RunMode runMode) {
+        if (robot.wobbleArmMotor.getMode() != runMode) {
+            robot.wobbleArmMotor.setMode(runMode);
+        }
+    }
+
+    private float snapToCardinal(float angle, float snapping, float offset) {
+        int o = (int) offset + 180;
+        o %= 90;
+        for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) {
+            if (angle >= cardinal-snapping && angle <= cardinal+snapping) {
+                angle = cardinal;
+            }
+        }
+        return angle;
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoStateTank.java

@@ -0,0 +1,55 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+/*
+The Player1 state has all the controls for player one. The Player One and Player Two controls are
+kept in separate states so that the childrenHaveFinished() method can be used to easily stop things
+from running at the same time that shouldn't be.
+*/
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl;
+
+public class DemoStateTank extends CyberarmState {
+    private Robot robot;
+
+    private double drivePower = 1;
+    private boolean lbPrev;
+
+
+    public DemoStateTank(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        Gamepad gamepad = engine.gamepad1;
+        if (engine.gamepad2.right_bumper) {
+            gamepad = engine.gamepad2;
+        }
+
+        //toggle for drive speed
+        boolean lb = engine.gamepad2.left_stick_button;
+        if (lb && !lbPrev) {
+            if (drivePower == 1) {
+                drivePower = 0.5;
+            } else {
+                drivePower = 1;
+            }
+        }
+        lbPrev = lb;
+
+        double left = -drivePower * gamepad.left_stick_y;
+        double right = -drivePower * gamepad.right_stick_y;
+        robot.setDrivePower(left,right,left,right);
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java

@@ -0,0 +1,45 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+public class DriveMotor {
+
+    public DcMotor motor;
+    private int brakePosition;
+    private MotorConfigurationType motorType;
+    private DcMotorSimple.Direction direction;
+    static final double FINE_CORRECTION = 0.05;
+    static final double LARGE_CORRECTION = 0.03;
+
+    public DriveMotor(DcMotor motor, MotorConfigurationType motorType, DcMotorSimple.Direction direction) {
+        this.motor = motor;
+        this.motorType = motorType;
+        this.direction = direction;
+    }
+
+    public void init() {
+//        motor.setMotorType(motorType);
+//        motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+//        motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        motor.setDirection(direction);
+    }
+
+    public void setPower(double power) {
+        motor.setPower(power);
+    }
+
+    public void setBrakePosition() {
+        brakePosition = motor.getCurrentPosition();
+    }
+
+    public void brake() {
+        double currentPosition = motor.getCurrentPosition();
+        double relativePosition = brakePosition - currentPosition;
+        double breakPower = Math.pow(LARGE_CORRECTION * relativePosition, 3) + FINE_CORRECTION * relativePosition;
+
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java

@@ -0,0 +1,104 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+/*
+The Launch state is used in teleOp and Autonomous. In addition to launching any rings by cycling the
+ring belt, this state returns the ring belt to the starting position
+*/
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class Launch extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    boolean hasCycled;
+    boolean detectedPass;
+    private boolean reduceConditionPrev;
+    private double reducePos;
+    private long stuckStartTime;
+
+    public Launch(Robot robot) {
+        this.robot = robot;
+    }
+
+    public Launch(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    @Override
+    public void start() {
+        try {
+            if (robot.stateConfiguration.action(groupName, actionName).enabled) {
+                robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
+
+            } else {
+                setHasFinished(true);
+            }
+        } catch (NullPointerException e){
+            robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
+        }
+
+    }
+
+    @Override
+    public void exec() {
+        //jam counter measures
+        if (robot.beltIsStuck() && childrenHaveFinished()) {
+            long currentTime = System.currentTimeMillis();
+            if (stuckStartTime == 0) {
+                stuckStartTime = currentTime;
+            } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) {
+                addParallelState(new UnstickRingBelt(robot));
+            }
+        } else {
+            stuckStartTime = 0;
+        }
+
+        //detect when limit switch is initially triggered
+        boolean detectingPass = robot.magnetSensor.isPressed();
+        int beltPos = robot.ringBeltMotor.getCurrentPosition();
+        if (detectingPass && !detectedPass) {
+            //finish once the ring belt has cycled all the way through and then returned to
+            //the first receiving position.
+
+            if (hasCycled) {
+                robot.ringBeltStage = 0;
+                robot.ringBeltMotor.setTargetPosition(beltPos);
+                robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+                if (!robot.initLauncher) {
+                    robot.launchMotor.setPower(0);
+                }
+
+                setHasFinished(true);
+            } else {
+                hasCycled = true;
+                reducePos = beltPos + (robot.reduceLaunchPos);
+            }
+        }
+        detectedPass = detectingPass;
+
+        boolean reduceCondition = (hasCycled && beltPos > reducePos);
+        if (reduceCondition && !reduceConditionPrev){
+            robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER);
+
+            //the ring belt stage lets other states know that the robot has finished launching all three rings
+            //and is now returning to loading position.
+
+            robot.ringBeltStage += 1;
+        }
+        reduceConditionPrev = reduceCondition;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("hasCycled", hasCycled);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java

@@ -0,0 +1,50 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class Pause extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private long waitTime = -1;
+    private boolean enabled;
+
+    public Pause(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    public Pause(Robot robot,  long waitTime) {
+        this.robot = robot;
+        this.waitTime = waitTime;
+    }
+
+    @Override
+    public void init() {
+        if (waitTime == -1) {
+            enabled = robot.stateConfiguration.action(groupName, actionName).enabled;
+            waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
+        }
+    }
+
+    @Override
+    public void start() {
+        if (!enabled) {
+            setHasFinished(true);
+        }
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(runTime() > waitTime);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("runTime", runTime());
+        engine.telemetry.addData("wait", waitTime);
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java

@@ -0,0 +1,26 @@
+package org.timecrafters.UltimateGoal.Competition.PreInit;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class FindLimitSwitch extends CyberarmState {
+
+    private Robot robot;
+
+    public FindLimitSwitch(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER);
+    }
+
+    @Override
+    public void exec() {
+        if (robot.magnetSensor.isPressed()) {
+            robot.ringBeltMotor.setPower(0);
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java

@@ -0,0 +1,35 @@
+package org.timecrafters.UltimateGoal.Competition.PreInit;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class LoadRings extends CyberarmState {
+
+    private Robot robot;
+    private ProgressRingBelt ringBeltState;
+    private int ringCount = 0;
+
+    public LoadRings(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        robot.collectionMotor.setPower(1);
+    }
+
+    @Override
+    public void exec() {
+        ringBeltState = new ProgressRingBelt(robot);
+        if (engine.gamepad1.x && childrenHaveFinished()) {
+            addParallelState(ringBeltState);
+
+        }
+
+        if (robot.ringBeltStage > 2) {
+            robot.launchMotor.setPower(0);
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java

@@ -0,0 +1,26 @@
+package org.timecrafters.UltimateGoal.Competition.PreInit;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+@Disabled
+@Autonomous (name = "Load Rings")
+public class PreInitEngine extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        addState(new FindLimitSwitch(robot));
+        addState(new LoadRings(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java

@@ -0,0 +1,82 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+/*
+The ProgressRingBelt state is used in teleOp to automatically move the ring belt.
+*/
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class ProgressRingBelt extends CyberarmState {
+
+    private Robot robot;
+    private int targetPos;
+    private boolean prepLaunch;
+    private long stuckStartTime;
+
+    public ProgressRingBelt(Robot robot) {
+        this.robot = robot;
+    }
+
+    private void prep(){
+        robot.ringBeltMotor.setTargetPosition(targetPos);
+        robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
+        robot.ringBeltStage += 1;
+        robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
+    }
+
+    @Override
+    public void start() {
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
+
+        //The first two progressions should move to preprare for another ring.
+        if (robot.ringBeltStage < 2) {
+            targetPos = currentPos + robot.ringBeltGap;
+            prep();
+
+            //The third progression needs to only move forward enought to block the ring from
+            //falling out
+        } else if (robot.ringBeltStage == 2) {
+            targetPos = currentPos + 150;
+            prep();
+            prepLaunch = !robot.initLauncher;
+
+            //If the ring belt is already full, It does  allow another progression
+        } else if (robot.ringBeltStage > 2) {
+            setHasFinished(true);
+        }
+
+
+    }
+
+    @Override
+    public void exec() {
+
+        //finished state when the target position is reached
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
+        if (currentPos >= targetPos) {
+            if(prepLaunch) {
+                robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+            }
+
+            setHasFinished(true);
+        }
+
+        //belt jam mitigation
+        if (robot.beltIsStuck() && childrenHaveFinished()) {
+            long currentTime = System.currentTimeMillis();
+            if (stuckStartTime == 0) {
+                stuckStartTime = currentTime;
+
+            } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) {
+
+                addParallelState(new UnstickRingBelt(robot));
+            }
+        } else {
+            stuckStartTime = 0;
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java

@@ -0,0 +1,57 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class ResetRingBelt extends CyberarmState {
+
+    private Robot robot;
+    boolean detectedPass;
+    int reducePos;
+    boolean reduceConditionPrev;
+
+    public ResetRingBelt(Robot robot) {
+        this.robot = robot;
+    }
+
+
+    @Override
+    public void start() {
+        robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
+        reducePos = robot.ringBeltMotor.getCurrentPosition() + robot.reduceLaunchPos;
+    }
+
+    @Override
+    public void exec() {
+        //detect when limit switch is initially triggered
+        boolean detectingPass = robot.magnetSensor.isPressed();
+        int beltPos = robot.ringBeltMotor.getCurrentPosition();
+
+        if (detectingPass && !detectedPass) {
+            //finish once the ring belt has cycled all the way through and then returned to
+            //the first receiving position.
+
+
+            robot.ringBeltStage = 0;
+            robot.ringBeltMotor.setTargetPosition(beltPos);
+            robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+            robot.launchMotor.setPower(0);
+            setHasFinished(true);
+        }
+        detectedPass = detectingPass;
+
+        boolean reduceCondition = (beltPos > reducePos);
+        if (reduceCondition && !reduceConditionPrev){
+            robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER);
+
+            //the ring belt stage lets other states know that the robot has finished launching all three rings
+            //and is now returning to loading position.
+
+        }
+        reduceConditionPrev = reduceCondition;
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java

@@ -0,0 +1,757 @@
+ package org.timecrafters.UltimateGoal.Competition;
+
+/*
+The robot object contains all the hardware and functions that are used in both teleOp and
+Autonomous. This includes drive functions, localization functions, shared constants, and a few
+general calculations and debugging tools.
+*/
+
+import android.os.Environment;
+import android.util.Log;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.hardware.rev.RevColorSensorV3;
+import com.qualcomm.hardware.rev.RevTouchSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.ClassFactory;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
+import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
+import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+
+import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
+
+public class Robot {
+
+    private HardwareMap hardwareMap;
+
+    public Robot(HardwareMap hardwareMap) {
+        this.hardwareMap = hardwareMap;
+    }
+
+    //The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit
+    //variables saved on the phone, without having to re-download the whole program. This is
+    //especially useful for autonomous route tuning
+    public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
+
+    //We use the IMU to get reliable rotation and angular velocity information. Experimentation has
+    //demonstrated that the accelerometer and related integrations aren't as accurate.
+    public BNO055IMU imu;
+
+    //The LEDs are used to provide driver feedback and for looking beautiful
+    public RevBlinkinLedDriver ledDriver;
+
+    //drive and dead-wheal hardware
+    public DcMotor driveFrontLeft;
+    public DcMotor driveBackLeft;
+    public DcMotor driveFrontRight;
+    public DcMotor driveBackRight;
+
+    public DcMotor encoderLeft;
+    public DcMotor encoderRight;
+    public DcMotor encoderBack;
+
+    //Motion Constants
+
+    //related graphs
+    //https://www.desmos.com/calculator/gndnkjndu9
+    //https://www.desmos.com/calculator/w0rebnftvg
+    //https://www.desmos.com/calculator/qxa1rq8hrv
+
+    static final double CUBIC_CORRECTION = 0.035;
+    static final double FACE_CUBIC_CORRECTION = 0.025;
+    static final double LINEAR_CORRECTION = 0.055;
+    static final double FACE_MIN_CORRECTION = 0.2;
+    static final double FACE_LINEAR_CORRECTION = 0.025;
+    static final double MOMENTUM_CORRECTION = 1.05;
+    static final double MOMENTUM_MAX_CORRECTION = 1.4;
+    static final double MOMENTUM_HORIZONTAL_CORRECTION =
+            -(Math.log10(MOMENTUM_MAX_CORRECTION-1) / Math.log10(MOMENTUM_CORRECTION));
+    static final double FACE_MOMENTUM_MAX_CORRECTION = 1.1;
+    static final double FACE_MOMENTUM_CORRECTION = 1.06;
+    static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION =
+            -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1) / Math.log10(FACE_MOMENTUM_CORRECTION));
+    static final double ZERO_POWER_THRESHOLD = 0.25;
+
+    //Unit Conversion Constants
+    static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
+    static final int COUNTS_PER_REVOLUTION = 8192;
+    static final float mmPerInch = 25.4f;
+    static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD = 12.3;
+    static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD = 18.8;
+    static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4;
+    static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6;
+
+    //Robot Localization
+    private static double locationX;
+    private static double locationY;
+    private static float rotation;
+
+    private int encoderLeftPrevious = 0;
+    private int encoderBackPrevious = 0;
+    private int encoderRightPrevious = 0;
+    private float rotationPrevious = 0;
+    public float angularVelocity;
+
+    //vuforia && tensorFlow Stuff
+    private WebcamName webcam;
+    private VuforiaLocalizer vuforia;
+
+    // Inches Forward of axis of rotation
+    static final float CAMERA_FORWARD_DISPLACEMENT  = 8f;
+    // Inches above Ground
+    static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f;
+    // Inches Left of axis of rotation
+    static final float CAMERA_LEFT_DISPLACEMENT = 4f;
+
+    static final double CAMERA_DISPLACEMENT_MAG =
+            Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT);
+    static final float CAMERA_DISPLACEMENT_DIRECTION =
+            (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
+
+    public boolean trackableVisible;
+    private VuforiaTrackables targetsUltimateGoal;
+    private List<VuforiaTrackable> trackables = new ArrayList<VuforiaTrackable>();
+    private OpenGLMatrix lastConfirmendLocation;
+
+    private long timeStartZeroVelocity = 0;
+    private long minCheckDurationMs = 500;
+    private int minCheckVelocity = 1;
+    private float vuforiaRotationCull;
+
+    //The servo mount for our camera allows us to look down for ideal TensorFlow and look up for
+    //ideal Vuforia Navigation
+    public Servo webCamServo;
+    public static final double CAM_SERVO_DOWN = 0.15;
+
+    //TensorFlow Object Detection
+    public TFObjectDetector tfObjectDetector;
+    private static final float MINIMUM_CONFIDENCE = 0.8f;
+
+    //Launcher
+    public DcMotor launchMotor;
+    public static final double LAUNCH_POWER = 0.715;
+
+    private static final long LAUNCH_ACCEL_TIME = 500;
+    //These variables were originally going to be used in both autonomous and teleOp
+    public double launchPositionX;
+    public double launchPositionY;
+    public float launchRotation;
+    public int reduceLaunchPos;
+
+    public boolean initLauncher;
+
+    //Ring Intake
+    public DcMotor collectionMotor;
+
+    //Ring Belt
+    public DcMotor ringBeltMotor;
+    public RevTouchSensor magnetSensor;
+    public int ringBeltStage;
+    public int ringBeltGap = 700;
+    public static final double RING_BELT_SLOW_POWER = 0.2;
+    public static final double RING_BELT_NORMAL_POWER = 0.6;
+    private int ringBeltPrev;
+    public long beltMaxStopTime;
+    public int beltReverseTicks;
+    public int beltMaxStopTicks;
+
+    //Wobble Goal Arm & Grabber
+    public DcMotor  wobbleArmMotor;
+    public Servo  wobbleGrabServo;
+    public int wobbleDownPos;
+    public int wobbleUpPos;
+    public int wobbleDropPos;
+    public static final double WOBBLE_SERVO_OPEN = 0;
+    public static final double WOBBLE_SERVO_CLOSED = 1;
+    public RevColorSensorV3 wobbleColorSensor;
+    public double wobbleScoreX;
+    public double wobbleScoreY;
+    public RevTouchSensor wobbleTouchSensor;
+
+    //Debugging
+    public double totalV;
+    private String TestingRecord = "Raw IMU, Delta, Saved";
+    public double forwardVector;
+    public double sidewaysVector;
+    public double traveledForward = 0;
+    public DcMotorEx motorAmpsTest;
+
+    //All our hardware initialization in one place, for everything that is the same in TeleOp and
+    //Autonomous
+    public void initHardware() {
+
+        //drive motors
+        driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
+        driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
+        driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
+        driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
+
+        driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
+        driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        //Localization encoders
+        encoderRight = hardwareMap.dcMotor.get("driveFrontRight");
+        encoderBack = hardwareMap.dcMotor.get("driveFrontLeft");
+        encoderLeft = hardwareMap.dcMotor.get("collect");
+
+        encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        encoderBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        encoderBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        //init wobble arm
+        wobbleArmMotor = hardwareMap.dcMotor.get("wobbleArm");
+        wobbleArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        wobbleArmMotor.setTargetPosition(0);
+        wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+        wobbleUpPos = stateConfiguration.variable(
+                "system","arm", "up").value();
+        wobbleDownPos = stateConfiguration.variable(
+                "system","arm", "down").value();
+        wobbleDropPos = stateConfiguration.variable(
+                "system","arm", "drop").value();
+
+        wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
+
+        wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
+        wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
+
+
+        //init collection motor
+        collectionMotor = hardwareMap.dcMotor.get("collect");
+        collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        //init ring belt
+        ringBeltMotor = hardwareMap.dcMotor.get("belt");
+        ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        magnetSensor = hardwareMap.get(RevTouchSensor.class, "magLim");
+
+        beltMaxStopTime = stateConfiguration.variable(
+                "system","belt", "maxStopTime").value();
+        beltMaxStopTicks = stateConfiguration.variable(
+                "system","belt", "maxStopTicks").value();
+        beltReverseTicks = stateConfiguration.variable(
+                "system","belt", "reverseTicks").value();
+        ringBeltGap = stateConfiguration.variable(
+                "system","belt","gap").value();
+
+        //init IMU
+        imu  = hardwareMap.get(BNO055IMU.class, "imu");
+
+        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+
+        parameters.mode = BNO055IMU.SensorMode.IMU;
+        parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+        parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+        parameters.loggingEnabled = false;
+
+        imu.initialize(parameters);
+
+        Velocity startVelocity = new Velocity();
+        startVelocity.xVeloc = 0;
+        startVelocity.yVeloc = 0;
+        startVelocity.zVeloc = 0;
+
+        Position startPosition = new Position();
+        startPosition.x = 0;
+        startPosition.y = 0;
+        startPosition.z = 0;
+
+        imu.startAccelerationIntegration(startPosition,startVelocity, 10);
+
+        //Init Localization
+        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+        initVuforia();
+        webCamServo = hardwareMap.servo.get("look");
+        webCamServo.setDirection(Servo.Direction.REVERSE );
+
+        minCheckVelocity =stateConfiguration.variable(
+                "system", "camera", "minCheckV").value();
+        vuforiaRotationCull = stateConfiguration.variable(
+                "system", "camera", "rCull").value();
+        minCheckDurationMs =stateConfiguration.variable(
+                "system", "camera", "minCheckMS").value();
+
+        //Init Launch Motor
+        DcMotor launcher = hardwareMap.dcMotor.get("launcher");
+
+        MotorConfigurationType launchMotorType = launcher.getMotorType();
+        launchMotorType.setGearing(3);
+        launchMotorType.setTicksPerRev(84);
+        launchMotorType.setMaxRPM(2400);
+
+        launchMotor = launcher;
+        launchMotor.setMotorType(launchMotorType);
+        launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+        //This is a debugging option that automatically turns on the launch wheel during init.
+        //This can be disabled using a variable in the TimeCraftersConfiguration
+        initLauncher = stateConfiguration.action(
+                "system","initLauncher").enabled;
+        reduceLaunchPos = stateConfiguration.variable(
+                "system", "launchPos", "reducePower").value();
+
+        if (initLauncher) {
+            double launcherPower = 0;
+            long launchAccelStart = System.currentTimeMillis();
+            while (launcherPower < LAUNCH_POWER) {
+                launcherPower = (double) ((System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME) * LAUNCH_POWER;
+                launchMotor.setPower(launcherPower);
+            }
+        }
+
+        launchPositionX = inchesToTicks((double) stateConfiguration.variable(
+                "system", "launchPos","x").value());
+        launchPositionY = inchesToTicks((double) stateConfiguration.variable(
+                "system", "launchPos","y").value());
+        launchRotation = stateConfiguration.variable(
+                "system", "launchPos","rot").value();
+
+        initTensorFlow();
+
+        ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds");
+    }
+
+    private void initVuforia() {
+
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
+                "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        VuforiaLocalizer.Parameters parameters =
+                new VuforiaLocalizer.Parameters(cameraMonitorViewId);
+
+        parameters.vuforiaLicenseKey = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
+        parameters.cameraName = webcam;
+        parameters.useExtendedTracking = false;
+
+
+        vuforia = ClassFactory.getInstance().createVuforia(parameters);
+
+        targetsUltimateGoal = this.vuforia.loadTrackablesFromAsset("UltimateGoal");
+        VuforiaTrackable redTowerGoalTarget = targetsUltimateGoal.get(1);
+        redTowerGoalTarget.setName("Red Tower Goal Target");
+        VuforiaTrackable redAllianceTarget = targetsUltimateGoal.get(2);
+        redAllianceTarget.setName("Red Alliance Target");
+        VuforiaTrackable frontWallTarget = targetsUltimateGoal.get(4);
+        frontWallTarget.setName("Front Wall Target");
+
+        trackables.addAll(targetsUltimateGoal);
+
+        final float mmTargetHeight   = (6) * mmPerInch;
+
+        final float halfField = 72 * mmPerInch;
+        final float quadField  = 36 * mmPerInch;
+
+        redAllianceTarget.setLocation(OpenGLMatrix
+                .translation(0, -halfField, mmTargetHeight)
+                .multiplied(Orientation.getRotationMatrix(
+                        EXTRINSIC, XYZ, DEGREES, 90, 0, 180)));
+        frontWallTarget.setLocation(OpenGLMatrix
+                .translation(-halfField, 0, mmTargetHeight)
+                .multiplied(Orientation.getRotationMatrix(
+                        EXTRINSIC, XYZ, DEGREES, 90, 0 , 90)));
+        redTowerGoalTarget.setLocation(OpenGLMatrix
+                .translation(halfField, -quadField, mmTargetHeight)
+                .multiplied(Orientation.getRotationMatrix(
+                        EXTRINSIC, XYZ, DEGREES, 90, 0, -90)));
+
+        OpenGLMatrix robotFromCamera = OpenGLMatrix
+                .translation(CAMERA_FORWARD_DISPLACEMENT * mmPerInch,
+                        CAMERA_LEFT_DISPLACEMENT * mmPerInch,
+                        CAMERA_VERTICAL_DISPLACEMENT * mmPerInch)
+                .multiplied(Orientation.getRotationMatrix(
+                        EXTRINSIC, YZX, DEGREES, -90, 0, 0));
+
+        for (VuforiaTrackable trackable : trackables) {
+            ((VuforiaTrackableDefaultListener) trackable.getListener())
+                    .setPhoneInformation(robotFromCamera, parameters.cameraDirection);
+        }
+
+        targetsUltimateGoal.activate();
+    }
+
+    public void deactivateVuforia() {
+        targetsUltimateGoal.deactivate();
+    }
+
+    private void initTensorFlow() {
+        int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
+                "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
+        parameters.minResultConfidence = stateConfiguration.variable(
+                "system", "camera", "minConfidence").value();
+        tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
+        tfObjectDetector.loadModelFromAsset(
+                "UltimateGoal.tflite", "Quad", "Single");
+    }
+
+    //Localization Function! This function is represented in a flow diagram, earlier in the
+    //software section
+    //run this in every exec to track the robot's location.
+    public void updateLocation(){
+
+        // orientation is inverted to have clockwise be positive.
+        float imuAngle = -imu.getAngularOrientation().firstAngle;
+        double rotationChange = getRelativeAngle(rotationPrevious, imuAngle);
+
+        int encoderLeftCurrent = -encoderLeft.getCurrentPosition();
+        int encoderRightCurrent = encoderRight.getCurrentPosition();
+        int encoderBackCurrent = encoderBack.getCurrentPosition();
+
+        double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
+        double encoderRightChange = encoderRightCurrent - encoderRightPrevious;
+        double encoderBackChange = encoderBackCurrent - encoderBackPrevious;
+
+        encoderLeftPrevious = encoderLeftCurrent;
+        encoderRightPrevious = encoderRightCurrent;
+        encoderBackPrevious = encoderBackCurrent;
+        rotationPrevious = imuAngle;
+
+        //The forward Vector has the luxury of having an odometer on both sides of the robot.
+        //This allows us to reduce the unwanted influence of turning the robot by averaging
+        //the two. unfortunatly we the current positioning of the odometers
+
+        //Since there isn't a second wheel to remove the influence of robot rotation, we have to
+        //instead do this by approximating the number of ticks that were removed due to rotation
+        //based on a separate calibration program.
+
+        double ticksPerDegreeForward;
+        double ticksPerDegreeSideways;
+
+        if (rotationChange < 0) {
+            ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
+            ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD;
+        } else {
+            ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
+            ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD;
+        }
+
+        forwardVector = ((encoderLeftChange+encoderRightChange)/2) -
+                (rotationChange* ticksPerDegreeForward);
+
+        traveledForward += forwardVector;
+        sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways);
+
+        double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
+        double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) +
+                Math.atan2(sidewaysVector,forwardVector);
+
+        double xChange = magnitude * (Math.sin(direction));
+        double yChange = magnitude * (Math.cos(direction));
+
+        locationX += xChange;
+        locationY += yChange;
+
+        Robot.rotation += rotationChange;
+        Robot.rotation = scaleAngleRange(Robot.rotation);
+
+        totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) +
+                Math.abs(encoderBackChange);
+
+//        record(""+System.currentTimeMillis()+", "+imuAngle);
+    }
+
+    //Experimentation has demonstrated that Vuforia dosen't provide good data while the camera
+    //is moving or rotating. This function detects if conditions are appropriate to run vuforia and
+    //get more accurate results
+    public void syncIfStationary() {
+        if (totalV < minCheckVelocity) {
+            long timeCurrent = System.currentTimeMillis();
+
+            if (timeStartZeroVelocity == 0) {
+                timeStartZeroVelocity = timeCurrent;
+            } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs ) {
+                syncWithVuforia();
+            }
+
+        } else {
+            timeStartZeroVelocity = 0;
+        }
+
+    }
+
+    public void syncWithVuforia() {
+        trackableVisible = false;
+        for (VuforiaTrackable trackable : trackables) {
+            if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
+                OpenGLMatrix robotLocation =
+                        ((VuforiaTrackableDefaultListener)trackable.getListener())
+                                .getUpdatedRobotLocation();
+
+                //this is used for debugging purposes.
+                trackableVisible = true;
+
+                if (robotLocation != null) {
+                    lastConfirmendLocation = robotLocation;
+                }
+
+                //For our tournament, it makes sense to make zero degrees towards the goal.
+                //Orientation is inverted to have clockwise be positive.
+                Orientation orientation =
+                        Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
+                float vuforiaRotation = 90-orientation.thirdAngle;
+
+                if (vuforiaRotation > 180) {
+                    vuforiaRotation -= -180;
+                }
+
+                if (Math.abs(rotation - vuforiaRotation) < vuforiaRotationCull) {
+                    rotation = vuforiaRotation;
+
+                    VectorF translation = lastConfirmendLocation.getTranslation();
+                    double camX = -translation.get(1) / mmPerInch;
+                    double camY = translation.get(0) / mmPerInch;
+
+                    double displaceX = CAMERA_DISPLACEMENT_MAG *
+                            Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+                    double displaceY = CAMERA_DISPLACEMENT_MAG *
+                            Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+
+                    locationX = inchesToTicks(camX - displaceX);
+                    locationY = inchesToTicks(camY - displaceY);
+
+                    break;
+                }
+            }
+        }
+    }
+
+    public float getRotation() {
+        return Robot.rotation;
+    }
+
+    public double getLocationX() {
+        return Robot.locationX;
+    }
+
+    public double getLocationY() {
+        return Robot.locationY;
+    }
+
+    //This is meant to only be used to indicate starting positions and to reorient the robot.
+    public void setLocalization(float rotation, double x, double y) {
+        Robot.rotation = rotation;
+        Robot.locationX = x;
+        Robot.locationY = y;
+    }
+
+
+    //returns the angle from the robot's current position to the given target position.
+    public float getAngleToPosition (double x, double y) {
+        double differenceX = x- getLocationX();
+        double differenceY = y- getLocationY();
+        double angle = Math.toDegrees(Math.atan2(differenceX, differenceY ));
+
+        return (float) angle;
+
+    }
+
+    //Unit conversions
+    public double ticksToInches(double ticks) {
+        return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
+    }
+
+    public double inchesToTicks(double inches) {
+        return inches * (COUNTS_PER_REVOLUTION / ENCODER_CIRCUMFERENCE);
+
+    }
+
+    //Returns the shortest angle between two directions, with positive angles indicating that the
+    // reference is to the right (clockwise) of the current. Negative angles indicate that the
+    // reference is to the left.
+    public float getRelativeAngle(float reference, float current) {
+        return scaleAngleRange(current - reference);
+    }
+
+    //Drive Functions
+    public void setDrivePower(double powerFrontLeft, double powerFrontRight,
+                              double powerBackLeft, double powerBackRight){
+        driveFrontLeft.setPower(powerFrontLeft);
+        driveFrontRight.setPower(powerFrontRight);
+        driveBackLeft.setPower(powerBackLeft);
+        driveBackRight.setPower(powerBackRight);
+    }
+
+    //returns an array of the powers necessary to execute the provided motion.
+    //"degreesDirectionMotion" is the angle relative to the field that the robot should drive at.
+    //"degreesDirectionFace" is the angle the robot should face relative to the field. The order of
+    //the output powers is ForwardLeft, ForwardRight, BackLeft, BackRight
+    public double[] getMecanumPowers(float degreesDirectionMotion, double scalar,
+                                     float degreesDirectionFace) {
+        angularVelocity = imu.getAngularVelocity().xRotationRate;
+
+        //calculating the base mecanum powers so that the robot drives along the degreesDirectionMotion
+        //once it is pointed towards the degreesDirectionFace
+        double rad = Math.toRadians(getRelativeAngle(degreesDirectionFace,degreesDirectionMotion));
+        double y = Math.cos(rad);
+        double x = Math.sin(rad);
+
+        double p = y + x;
+        double q = y - x;
+
+
+
+        //calculating correction needed to steer the robot towards the degreesDirectionFace
+        float relativeRotation =
+                getRelativeAngle(degreesDirectionFace, Robot.rotation);
+        double turnCorrection =
+                Math.pow(CUBIC_CORRECTION * relativeRotation, 3) +
+                        LINEAR_CORRECTION * relativeRotation;
+
+        if (relativeRotation != 0) {
+            double momentumRelative =  angularVelocity *
+                    (relativeRotation / Math.abs(relativeRotation));
+            double exponential =
+                    Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
+            double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
+            //reduces concern for  momentum when the angle is far away from target
+            turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) *
+                    (1  - momentumCorrection)) / 180 );
+        }
+
+        double powerForwardRight = scalar * (q + turnCorrection);
+        double powerForwardLeft = scalar * (p - turnCorrection);
+        double powerBackRight = scalar * (p + turnCorrection);
+        double powerBackLeft = scalar * (q - turnCorrection);
+
+
+        // The "extreme" is the power value that is furthest from zero. When this values exceed the
+        // -1 to 1 power range, dividing the powers by the "extreme" scales everything back into the
+        // workable range without altering the final motion vector.
+
+        double extreme = Math.max(
+                Math.max(Math.abs(powerForwardRight),Math.abs(powerForwardLeft)),
+                Math.max(Math.abs(powerBackRight),Math.abs(powerBackLeft)));
+
+        if (extreme > 1) {
+            powerForwardRight = powerForwardRight/extreme;
+            powerForwardLeft = powerForwardLeft/extreme;
+            powerBackRight = powerBackRight/extreme;
+            powerBackLeft = powerBackLeft/extreme;
+        }
+
+        double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight};
+
+        double totalPower = Math.abs(powerForwardLeft) +
+                Math.abs(powerForwardRight) +
+                Math.abs(powerBackLeft) +
+                Math.abs(powerBackRight);
+        if (totalPower < ZERO_POWER_THRESHOLD) {
+            powers = new double[] {0,0,0,0};
+        }
+
+
+        return powers;
+    }
+
+    //Outputs the power necessary to turn and face a provided direction
+    public double[] getFacePowers(float direction, double power) {
+        angularVelocity = imu.getAngularVelocity().xRotationRate;
+        double relativeAngle = getRelativeAngle(direction, Robot.rotation);
+        double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) +
+                FACE_LINEAR_CORRECTION * relativeAngle;
+
+        if (relativeAngle > 0.5) {
+            scaler += FACE_MIN_CORRECTION;
+        } else if (relativeAngle < -0.5) {
+            scaler -= FACE_MIN_CORRECTION;
+        }
+
+        if (relativeAngle != 0) {
+            double momentumRelative =  angularVelocity * (relativeAngle / Math.abs(relativeAngle));
+            double exponential = Math.pow(FACE_MOMENTUM_CORRECTION,
+                    FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
+            double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
+
+            scaler *= momentumCorrection;
+        }
+
+        double left = -power * scaler;
+        double right = power *scaler;
+
+        double[] powers = {left,right};
+
+        double totalPower = 2 * (Math.abs(left) + Math.abs(right));
+        if (totalPower < ZERO_POWER_THRESHOLD) {
+            powers = new double[] {0,0};
+        }
+
+        return powers;
+    }
+
+    //Used to for automated jam countermeasures
+    public boolean beltIsStuck() {
+        int ringBeltPos = ringBeltMotor.getCurrentPosition();
+        boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks);
+        ringBeltPrev = ringBeltPos;
+        return notMoved;
+    }
+
+    public float scaleAngleRange(float angle) {
+        if (angle < -180) {
+            angle += 360;
+        }
+        if (angle > 180) {
+            angle -= 360;
+        }
+        return angle;
+    }
+
+    //Debugging tools
+    public void record(String record) {
+        TestingRecord+="\n"+record;
+    }
+
+    public void saveRecording() {
+        writeToFile(Environment.getExternalStorageDirectory().getAbsolutePath()+File.separator+"TimeCrafters_TestingRecord"+File.separator+"RobotTestingRecord.txt", TestingRecord);
+    }
+
+    public boolean writeToFile(String filePath, String content) {
+        try {
+
+            FileWriter writer = new FileWriter(filePath);
+            writer.write(content);
+            writer.close();
+
+            return true;
+
+        } catch (IOException e) {
+            Log.e("RecordTest", e.getLocalizedMessage());
+            return false;
+        }
+    }
+
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java

@@ -0,0 +1,57 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
+import org.timecrafters.UltimateGoal.Competition.ResetRingBelt;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class LaunchControl extends CyberarmState {
+
+    private Robot robot;
+    private boolean ranLaunch;
+
+    public LaunchControl(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+
+        if (robot.ringBeltStage > 2) {
+            if (robot.ringBeltStage > 4) {
+                addParallelState(new ResetRingBelt(robot));
+            } else {
+                addParallelState(new LaunchSingle(robot));
+            }
+            ranLaunch = true;
+        }
+    }
+
+    @Override
+    public void exec() {
+        if (childrenHaveFinished()) {
+            if (!ranLaunch) {
+                if (robot.ringBeltStage <= 2) {
+                    addParallelState(new ProgressRingBelt(robot));
+                } else {
+                    addParallelState(new LaunchSingle(robot));
+                    ranLaunch = true;
+                }
+            } else {
+                setHasFinished(true);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+
+        engine.telemetry.addData("Launch Control children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchSingle.java

@@ -0,0 +1,52 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.UnstickRingBelt;
+
+public class LaunchSingle extends CyberarmState {
+
+    private Robot robot;
+    boolean hasCycled;
+    boolean detectedPass;
+    private int targetPos;
+    private boolean reduceConditionPrev;
+    private double reducePos;
+    private long stuckStartTime;
+
+    public LaunchSingle(Robot robot) {
+        this.robot = robot;
+    }
+
+
+    @Override
+    public void start() {
+        robot.ringBeltStage +=1;
+        robot.ringBeltMotor.setTargetPosition(robot.ringBeltMotor.getCurrentPosition() + robot.ringBeltGap);
+        robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
+    }
+
+    @Override
+    public void exec() {
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
+        if (currentPos >= targetPos) {
+            setHasFinished(true);
+        }
+
+        if (robot.beltIsStuck() && childrenHaveFinished()) {
+            long currentTime = System.currentTimeMillis();
+            if (stuckStartTime == 0) {
+                stuckStartTime = currentTime;
+            } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) {
+                addParallelState(new UnstickRingBelt(robot));
+            }
+        } else {
+            stuckStartTime = 0;
+        }
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java

@@ -0,0 +1,260 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+/*
+The Player1 state has all the controls for player one. The Player One and Player Two controls are
+kept in separate states so that the childrenHaveFinished() method can be used to easily stop things
+from running at the same time that shouldn't be.
+*/
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class Player1 extends CyberarmState {
+    private Robot robot;
+
+    //normal drive control
+    private float leftJoystickDegrees;
+    private double leftJoystickMagnitude;
+    private float rightJoystickDegrees;
+    private double rightJoystickMagnitude;
+    private double rightJoystickMagnitudePrevious;
+
+    private double faceControlThreshold;
+    private float cardinalSnapping;
+    private float pairSnapping;
+
+    private float faceDirection = 0;
+    private double[] powers = {0,0,0,0};
+    private double drivePower = 1;
+    private boolean lbPrev;
+
+    //find wobble goal control
+    private FindWobbleGoal findWobbleGoal;
+    private boolean runNextFindWobble;
+    private boolean findWobbleInputPrev;
+    private boolean aPrev;
+
+    //Drive to launch control
+    private powerShotsControl powerShots;
+    private boolean runNextDriveToLaunch;
+    private boolean driveToLaunchInputPrev;
+
+    private double endGameX;
+    private double endGameY;
+    private float endGameRot;
+    private int loopCount;
+
+    private double refinePower;
+
+    public Player1(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        cardinalSnapping = robot.stateConfiguration.variable(
+                "tele","control", "cardinalSnapping").value();
+        pairSnapping = robot.stateConfiguration.variable(
+                "tele","control", "pairSnapping").value();
+        faceControlThreshold = robot.stateConfiguration.variable(
+                "tele","control", "faceControlT").value();
+        refinePower = robot.stateConfiguration.variable(
+                "tele","control", "refPower").value();
+
+        endGameX = robot.stateConfiguration.variable(
+                "tele","_endGameStart","x").value();
+        endGameY = robot.stateConfiguration.variable(
+                "tele","_endGameStart","y").value();
+        endGameRot = robot.stateConfiguration.variable(
+                "tele","_endGameStart", "r").value();
+    }
+
+    @Override
+    public void start() {
+        faceDirection = robot.getRotation();
+    }
+
+    @Override
+    public void exec() {
+        loopCount+=1;
+        robot.record(""+runTime()+", "+loopCount+", "+robot.getRotation());
+        robot.updateLocation();
+
+        //toggle for drive speed
+        boolean lb = engine.gamepad1.left_stick_button;
+        if (lb && !lbPrev) {
+            if (drivePower == 1) {
+                drivePower = 0.5;
+            } else {
+                drivePower = 1;
+            }
+        }
+        lbPrev = lb;
+
+        //This Runs the FindWobbleGoal state as long as the button is pressed. When it's released,
+        //the state is interrupted. If the state finishes while the button is still pressed,
+        //it waits until the button is released before running the state again
+        runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
+
+        boolean findWobbleInput = engine.gamepad1.x;
+        if (findWobbleInput) {
+            //if the claw is open, run FindWobbleGoal
+            if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) {
+
+                faceDirection = robot.getRotation();
+
+                if (runNextFindWobble && !findWobbleInputPrev) {
+                    findWobbleGoal =
+                            new FindWobbleGoal(robot, "auto", "08_0");
+                    addParallelState(findWobbleGoal);
+                }
+                //if the claw is closed, open the claw.
+            } else if (!findWobbleInputPrev) {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
+            }
+            //if the button is released cancel the search
+        } else if (!runNextFindWobble) {
+            findWobbleGoal.setHasFinished(true);
+        }
+        findWobbleInputPrev = findWobbleInput;
+
+        //toggles wobble grabber open and closed
+        boolean a = engine.gamepad1.a;
+        if (a && !aPrev) {
+            if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+            } else {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
+            }
+        }
+        aPrev = a;
+
+        //This logic works the same as the stuff above, accept instead of autonomous wobble grab,
+        //it
+        runNextDriveToLaunch = (powerShots == null || powerShots.getHasFinished());
+
+        boolean driveToLaunchInput = engine.gamepad1.y && !findWobbleInput;
+        if (driveToLaunchInput) {
+            if (runNextDriveToLaunch && !driveToLaunchInputPrev) {
+                powerShots = new powerShotsControl(robot);
+                addParallelState(powerShots);
+            }
+            faceDirection = robot.getRotation();
+        } else if (!runNextDriveToLaunch) {
+            powerShots.setHasFinished(true);
+        }
+        driveToLaunchInputPrev = driveToLaunchInput;
+
+        //Normal Driver Controls
+        if (childrenHaveFinished()) {
+
+
+            double leftJoystickX = engine.gamepad1.left_stick_x;
+            double leftJoystickY = engine.gamepad1.left_stick_y;
+
+            leftJoystickDegrees = robot.getRelativeAngle(90,
+                    (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
+            leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
+
+            double rightJoystickX = engine.gamepad1.right_stick_x;
+            double rightJoystickY = engine.gamepad1.right_stick_y;
+
+            rightJoystickDegrees = robot.getRelativeAngle(90,
+                    (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
+            rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
+
+            //allows the the driver to indicate which direction the robot is currently looking
+            //so that the controller and robot can be re-synced in the event of a bad initial
+            //rotation.
+            if (engine.gamepad1.back) {
+                float newRotation = 0;
+
+                if (rightJoystickMagnitude != 0) {
+                    newRotation = rightJoystickDegrees;
+                }
+
+                robot.setLocalization(newRotation, robot.getLocationX(), robot.getLocationY());
+                faceDirection = newRotation;
+            }
+
+            //if the driver is letting go of the face joystick, the robot should maintain it's
+            //current face direction.
+            if (rightJoystickMagnitude > faceControlThreshold ||
+                    rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) {
+
+                //if the joystick is close to one of the cardinal directions, it is set to exactly
+                // the cardinal direction
+                faceDirection = snapToCardinal(rightJoystickDegrees, cardinalSnapping, 0);
+            }
+            rightJoystickMagnitudePrevious = rightJoystickMagnitude;
+
+            //The D-pad is used if the drivers need to get a more precise angle than they can get
+            //using the face joystick
+            if (engine.gamepad1.dpad_right) {
+                powers = new double[]{refinePower, -refinePower, refinePower, -refinePower};
+                faceDirection = robot.getRotation();
+            } else if (engine.gamepad1.dpad_left) {
+                powers = new double[]{-refinePower, refinePower, -refinePower, refinePower};
+                faceDirection = robot.getRotation();
+            } else if (leftJoystickMagnitude == 0) {
+                double[] facePowers = robot.getFacePowers(faceDirection, 0.4);
+                powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
+            } else {
+                //drives the robot in the direction of the move joystick while facing the direction
+                //of the look joystick. if the move direction is almost aligned/perpendicular to the
+                //look joystick,
+                powers = robot.getMecanumPowers(
+                        snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection),
+                        drivePower, faceDirection);
+            }
+
+            robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
+        }
+
+        //LED feedback control
+        double ringBeltPower = robot.ringBeltMotor.getPower();
+        if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() -
+                robot.ringBeltMotor.getCurrentPosition()) > 10) {
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE );
+        } else if (ringBeltPower < 0) {
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED);
+        }  else {
+            if (drivePower == 1) {
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
+            } else {
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GOLD);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Controler Directions");
+        engine.telemetry.addData("Right", rightJoystickDegrees);
+        engine.telemetry.addData("Left", leftJoystickDegrees);
+
+        engine.telemetry.addData("face", faceDirection);
+
+        engine.telemetry.addData("Player 1 children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+
+    private float snapToCardinal(float angle, float snapping, float offset) {
+        int o = (int) offset + 180;
+        o %= 90;
+        for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) {
+            if (angle >= cardinal-snapping && angle <= cardinal+snapping) {
+                angle = cardinal;
+            }
+        }
+        return angle;
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java

@@ -0,0 +1,189 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+/*
+The Player2 state has all the controls for player two. This includes a lot of automation and
+emergency features, for if the driver wants to take control unexpectedly
+*/
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class Player2 extends CyberarmState {
+    private Robot robot;
+
+    private boolean rbPrev;
+    private boolean yPrev;
+    private boolean xPrev;
+    private boolean bPrev;
+    private boolean wobbleArmUp = false;
+    private boolean wobbleGrabOpen = false;
+    private boolean aPrev;
+    private double beltPowerPrev;
+    private DcMotor.RunMode runModePrev;
+    private boolean lbPrev;
+    private boolean manualArmHold;
+    private int loops = 0;
+
+
+
+    private boolean launchInput = false;
+
+    public Player2(Robot robot) {
+        this.robot = robot;
+    }
+
+
+    @Override
+    public void init() {
+        robot.wobbleArmMotor.setTargetPosition(0);
+        robot.wobbleArmMotor.setPower(0.5);
+    }
+
+    @Override
+    public void start() {
+        robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
+    }
+
+    @Override
+    public void exec() {
+//        loops+=1;
+//        robot.record(""+runTime()+", "+loops+", "+robot.getRotation());
+
+        //Collector control
+        double rt = engine.gamepad2.right_trigger;
+        double lt = engine.gamepad2.left_trigger;
+        if (rt < lt) {
+            robot.collectionMotor.setPower(-lt);
+        } else if (childrenHaveFinished()) {
+            robot.collectionMotor.setPower(rt);
+        } else {
+            robot.collectionMotor.setPower(0);
+        }
+
+        //The childrenHaveFinished() method tracks if there are parallel states that are still
+        //running.
+        if (childrenHaveFinished()) {
+            //belt progression control
+            boolean rb = engine.gamepad2.right_bumper;
+            if (rb && !rbPrev) {
+                addParallelState(new ProgressRingBelt(robot));
+            }
+            rbPrev = rb;
+
+            //main launch sequence control
+            boolean y2 = engine.gamepad2.y;
+            if (y2 && !yPrev) {
+                addParallelState(new Launch(robot));
+            }
+            yPrev = y2;
+
+
+            //special launch sequence for single shots
+            boolean x = engine.gamepad2.x;
+            if (x && !bPrev) {
+                addParallelState(new LaunchControl(robot));
+            }
+            bPrev = x;
+        }
+
+        //manually control the wobble arm for when it's initialized in an unexpected position.
+        double leftStickY = engine.gamepad2.left_stick_y;
+
+        if (engine.gamepad2.dpad_right) {
+            if (!robot.wobbleTouchSensor.isPressed()) {
+                setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.wobbleArmMotor.setPower(-0.2);
+                robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
+            } else {
+                setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                robot.wobbleArmMotor.setTargetPosition(0);
+                setArmMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+        } else if (leftStickY != 0 ) {
+            setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            robot.wobbleArmMotor.setPower(0.15 * leftStickY);
+            robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
+        } else {
+            setArmMode(DcMotor.RunMode.RUN_TO_POSITION);
+            robot.wobbleArmMotor.setPower(0.5);
+            if (engine.gamepad2.dpad_up) {
+                robot.wobbleArmMotor.setTargetPosition(robot.wobbleUpPos);
+            } else if (engine.gamepad2.dpad_down) {
+                robot.wobbleArmMotor.setTargetPosition(robot.wobbleDownPos);
+            } else if (engine.gamepad2.dpad_left) {
+                robot.wobbleArmMotor.setTargetPosition(robot.wobbleDropPos );
+            }
+        }
+
+        //manually toggle the launch wheel for emergencies
+        boolean a = engine.gamepad2.a;
+        if (a && !aPrev) {
+            if (robot.launchMotor.getPower() == 0) {
+                robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+            } else {
+                robot.launchMotor.setPower(0);
+            }
+        }
+        aPrev = a;
+
+
+
+
+        //allows the driver to revers the belt in the event of a jam
+        boolean lb = engine.gamepad2.left_bumper;
+        if (lb && !lbPrev) {
+            runModePrev = robot.ringBeltMotor.getMode();
+            beltPowerPrev = robot.ringBeltMotor.getPower();
+            robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER);
+        }
+
+        if (!lb && lbPrev) {
+            robot.ringBeltMotor.setPower(beltPowerPrev);
+            robot.ringBeltMotor.setMode(runModePrev);
+        }
+        lbPrev = lb;
+
+        if (engine.gamepad2.back) {
+            for (CyberarmState state : children) {
+                engine.stopState(state);
+            }
+        }
+
+    }
+
+
+    //This just checks if the wobble arm runmode is already the desired mode before setting it.
+    //I don't know if this is actually helpful
+    private void setArmMode(DcMotor.RunMode runMode) {
+        if (robot.wobbleArmMotor.getMode() != runMode) {
+             robot.wobbleArmMotor.setMode(runMode);
+        }
+    }
+
+
+    @Override
+    public void telemetry() {
+//        engine.telemetry.addLine("belt");
+//        engine.telemetry.addData("power", robot.ringBeltMotor.getPower());
+//        engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition());
+//        engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition());
+//
+//        engine.telemetry.addData("ring belt stage", robot.ringBeltStage);
+//        engine.telemetry.addData("Touch Sensor Pressed", robot.wobbleTouchSensor.isPressed());
+//        engine.telemetry.addData("  Sensor value", robot.wobbleTouchSensor.getValue());
+        engine.telemetry.addData("Player 2 children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java

@@ -0,0 +1,38 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+@Disabled
+@TeleOp (name = "TeleOp",group = "comp")
+public class TeleOpEngine extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+        robot.webCamServo.setPosition(0);
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        addState(new TeleOpState(robot));
+
+
+    }
+
+    @Override
+    public void stop() {
+        robot.deactivateVuforia();
+        robot.saveRecording();
+        super.stop();
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java

@@ -0,0 +1,43 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
+import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class TeleOpState extends CyberarmState {
+
+    private Robot robot;
+
+    public TeleOpState(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        addParallelState(new Player1(robot));
+        addParallelState(new Player2(robot));
+    }
+
+    @Override
+    public void exec() {
+//        setHasFinished(childrenHaveFinished());
+    }
+
+    @Override
+    public void telemetry() {
+
+        engine.telemetry.addLine("Location");
+        engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");
+        engine.telemetry.addData("Rotation ", robot.getRotation());
+    }
+
+    private float round(double number,double unit) {
+        return (float) (Math.floor(number/unit) * unit);
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java

@@ -0,0 +1,80 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
+import org.timecrafters.UltimateGoal.Competition.Pause;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+
+public class powerShotsControl extends CyberarmState {
+
+    private Robot robot;
+
+    private double endGameX;
+    private double endGameY;
+    private float endGameRot;
+
+    private double endGamePower;
+
+    private int nextState = 0;
+
+    private ArrayList<CyberarmState> states = new ArrayList<CyberarmState>();
+
+    public powerShotsControl(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value();
+        endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value();
+        endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value();
+        endGamePower = robot.stateConfiguration.variable("tele","_endGameStart", "power").value();
+
+        endGameX = robot.inchesToTicks(endGameX);
+        endGameY = robot.inchesToTicks(endGameY);
+        endGameRot = (float) robot.inchesToTicks(endGameRot);
+
+        states.add(new Pause(robot, "tele","_endGameStart"));
+        states.add(new DriveToCoordinates(robot, "tele", "_pow1"));
+        states.add(new Face(robot,"tele","_faceZero"));
+        states.add(new LaunchControl(robot));
+        states.add(new DriveToCoordinates(robot, "tele", "_pow2"));
+        states.add(new Face(robot,"tele","_faceZero"));
+        states.add(new LaunchControl(robot));
+        states.add(new DriveToCoordinates(robot, "tele", "_pow3"));
+        states.add(new Face(robot,"tele","_faceZero"));
+        states.add(new LaunchControl(robot));
+    }
+
+    @Override
+    public void start() {
+        robot.setLocalization(endGameRot,endGameX,endGameY);
+        robot.launchMotor.setPower(endGamePower);
+    }
+
+    @Override
+    public void exec() {
+        if (childrenHaveFinished()) {
+            if (nextState < states.size()) {
+                addParallelState(states.get(nextState));
+                nextState += 1;
+            } else {
+                robot.launchMotor.setPower(0);
+                setHasFinished(true);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Power shots children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java

@@ -0,0 +1,37 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class UnstickRingBelt extends CyberarmState {
+
+    private Robot robot;
+    private int targetPos;
+    private double lastPower;
+    private DcMotor.RunMode lastRunMode;
+
+    public UnstickRingBelt(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        lastPower = robot.ringBeltMotor.getPower();
+        lastRunMode = robot.ringBeltMotor.getMode();
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
+        targetPos = currentPos - robot.beltReverseTicks;
+        robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER);
+    }
+
+    @Override
+    public void exec() {
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
+
+        if (currentPos < targetPos) {
+            robot.ringBeltMotor.setPower(lastPower);
+            setHasFinished(true);
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java

@@ -0,0 +1,55 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class WobbleArm extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private int suggestedPos;
+    private int targetPos = -1;
+    private long waitTime = -1;
+
+    public WobbleArm(Robot robot, String groupName, String actionName, int targetPos) {
+        this.robot = robot;
+        this.groupName  = groupName;
+        this.actionName = actionName;
+        this.suggestedPos = targetPos;
+    }
+
+    public WobbleArm(Robot robot, int targetPos, long waitTime) {
+        this.robot = robot;
+        this.targetPos = targetPos;
+        this.waitTime = waitTime;
+    }
+
+    @Override
+    public void init() {
+        if (waitTime == -1) {
+            waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
+            targetPos = robot.stateConfiguration.variable(groupName,actionName,"armPos").value();
+            if (targetPos == -1) {
+                targetPos = suggestedPos;
+            }
+        }
+    }
+
+    @Override
+    public void start() {
+        robot.wobbleArmMotor.setPower(0.5);
+        robot.wobbleArmMotor.setTargetPosition(targetPos);
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(runTime() > waitTime);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("runTime", runTime());
+        engine.telemetry.addData("target", targetPos);
+        engine.telemetry.addData("wait", waitTime);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java

@@ -0,0 +1,59 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class WobbleGrab extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private boolean open;
+    private long waitTime = -1;
+    private boolean enabled;
+
+    public WobbleGrab(Robot robot, String groupName, String actionName, boolean open) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+        this.open = open;
+    }
+
+    public WobbleGrab(Robot robot, boolean open, long waitTime) {
+        this.robot = robot;
+        this.open = open;
+        this.waitTime = waitTime;
+    }
+
+    @Override
+    public void init() {
+        if (waitTime == -1) {
+            enabled = robot.stateConfiguration.action(groupName, actionName).enabled;
+            waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
+        }
+    }
+
+    @Override
+    public void start() {
+        if (enabled) {
+            if (open) {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
+            } else {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+            }
+        } else {
+            setHasFinished(true);
+        }
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(runTime() > waitTime);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("runTime", runTime());
+        engine.telemetry.addData("wait", waitTime);
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java

@@ -0,0 +1,76 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+
+public class ControlHubTest extends CyberarmState {
+
+    private Robot robot;
+
+    private Acceleration acceleration;
+    private double Vx = 0;
+    private double Vy = 0;
+    private double Vz = 0;
+    private double aVx = 0;
+    private double aVy = 0;
+    private double aVz = 0;
+
+
+    public ControlHubTest(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+
+        Velocity startVelocity = new Velocity();
+        startVelocity.xVeloc = 0;
+        startVelocity.yVeloc = 0;
+        startVelocity.zVeloc = 0;
+
+        Position startPosition = new Position();
+        startPosition.x = 0;
+        startPosition.y = 0;
+        startPosition.z = 0;
+
+        robot.imu.startAccelerationIntegration(startPosition,startVelocity, 200);
+    }
+
+    @Override
+    public void exec() {
+        acceleration = robot.imu.getAcceleration();
+
+        Vx = robot.imu.getVelocity().xVeloc;
+        Vy = robot.imu.getVelocity().yVeloc;
+        Vz = robot.imu.getVelocity().zVeloc;
+
+        aVx = robot.imu.getAngularVelocity().xRotationRate;
+        aVy = robot.imu.getAngularVelocity().yRotationRate;
+        aVz = robot.imu.getAngularVelocity().zRotationRate;
+
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Acceleration");
+        engine.telemetry.addData("x", acceleration.xAccel);
+        engine.telemetry.addData("y", acceleration.yAccel);
+        engine.telemetry.addData("z", acceleration.zAccel);
+
+        engine.telemetry.addLine("Velocity");
+        engine.telemetry.addData("X", Vx);
+        engine.telemetry.addData("Y", Vy);
+        engine.telemetry.addData("Z", Vz);
+
+        engine.telemetry.addLine("Angular Velocity");
+        engine.telemetry.addData("X", aVx);
+        engine.telemetry.addData("Y", aVy);
+        engine.telemetry.addData("Z", aVz);
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java

@@ -0,0 +1,37 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
+import com.qualcomm.robotcore.hardware.ColorRangeSensor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+
+public class DistanceSensorTest extends CyberarmState {
+
+
+    private Rev2mDistanceSensor distanceSensor;
+//    private RevColorSensorV3 colorSensor;
+    private ColorRangeSensor colorSensor;
+    private double distance = 0;
+    private double color = 0;
+
+    @Override
+    public void init() {
+        distanceSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "distance");
+        colorSensor = engine.hardwareMap.get(ColorRangeSensor.class, "color");
+//        colorSensor = engine.hardwareMap.get(RevColorSensorV3.class, "color");
+    }
+
+    @Override
+    public void exec() {
+        distance = distanceSensor.getDistance(DistanceUnit.MM);
+        color = colorSensor.getDistance(DistanceUnit.MM);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("distance", distance);
+        engine.telemetry.addData("color", color);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LEDTest.java

@@ -0,0 +1,28 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+
+public class LEDTest extends CyberarmState {
+
+    RevBlinkinLedDriver leds;
+
+    @Override
+    public void init() {
+        leds = engine.hardwareMap.get(RevBlinkinLedDriver.class, "leds");
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(false);
+       if (engine.gamepad1.x) {
+           leds.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED);
+       } else {
+           leds.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN );
+       }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java

@@ -0,0 +1,137 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+
+public class MecanumFunctionTest extends CyberarmState {
+
+    private Robot robot;
+    private float leftJoystickDegrees;
+    private double leftJoystickMagnitude;
+    private float rightJoystickDegrees;
+    private double rightJoystickMagnitude;
+    private double powerFrontLeft=0;
+    private double powerFrontRight=0;
+    private double powerBackLeft=0;
+    private double powerBackRight=0;
+
+    private double aVx = 0;
+    private double aVy = 0;
+    private double aVz = 0;
+
+    private int powerStep = 5;
+    private double POWER_SCALE = 0.5 ;
+    private boolean toggleSpeedInput = false;
+
+    public MecanumFunctionTest(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        Velocity startVelocity = new Velocity();
+        startVelocity.xVeloc = 0;
+        startVelocity.yVeloc = 0;
+        startVelocity.zVeloc = 0;
+
+        Position startPosition = new Position();
+        startPosition.x = 0;
+        startPosition.y = 0;
+        startPosition.z = 0;
+
+        robot.imu.startAccelerationIntegration(startPosition,startVelocity, 10);
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+//        robot.record();/////////////
+
+        AngularVelocity angularVelocity = robot.imu.getAngularVelocity();
+
+        aVx = angularVelocity.xRotationRate;
+        aVy = angularVelocity.yRotationRate;
+        aVz = angularVelocity.zRotationRate;
+
+        double leftJoystickX = engine.gamepad1.left_stick_x;
+        double leftJoystickY = engine.gamepad1.left_stick_y;
+
+        leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY));
+        leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
+
+        double rightJoystickX = engine.gamepad1.right_stick_x;
+        double rightJoystickY = engine.gamepad1.right_stick_y;
+
+        rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
+        rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
+
+        powerFrontLeft = 0;
+        powerFrontRight = 0;
+        powerBackLeft = 0;
+        powerBackRight = 0;
+
+        boolean a = engine.gamepad1.a;
+        boolean b = engine.gamepad1.b;
+
+        if (a && !toggleSpeedInput && POWER_SCALE < 1) {
+            powerStep += 1;
+            POWER_SCALE = powerStep * 0.1;
+        }
+
+        if (b && !toggleSpeedInput && POWER_SCALE > 0.1) {
+            powerStep -= 1;
+            POWER_SCALE = powerStep * 0.1;
+        }
+
+        toggleSpeedInput = a || b;
+
+        if (rightJoystickMagnitude == 0) {
+            if (leftJoystickMagnitude !=0) {
+                double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, leftJoystickDegrees);
+                powerFrontLeft = powers[0];
+                powerFrontRight = powers[1];
+                powerBackLeft = powers[2];
+                powerBackRight = powers[3];
+
+            }
+        } else {
+            if (leftJoystickMagnitude == 0) {
+                double[] powers =  robot.getFacePowers(rightJoystickDegrees, POWER_SCALE * rightJoystickMagnitude);
+                powerFrontLeft = powers[0];
+                powerFrontRight = powers[1];
+                powerBackLeft = powers[0];
+                powerBackRight = powers[1];
+            } else {
+                double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, rightJoystickDegrees);
+                powerFrontLeft = powers[0];
+                powerFrontRight = powers[1];
+                powerBackLeft = powers[2];
+                powerBackRight = powers[3];
+            }
+        }
+
+        robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight);
+
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Scale", POWER_SCALE);
+
+        engine.telemetry.addLine("Angular Velocity");
+        engine.telemetry.addData("X", aVx);
+        engine.telemetry.addData("Y", aVy);
+        engine.telemetry.addData("Z", aVz);
+
+        engine.telemetry.addLine("Powers");
+        engine.telemetry.addData("FL", robot.driveFrontLeft.getPower());
+        engine.telemetry.addData("FR", robot.driveFrontRight.getPower());
+        engine.telemetry.addData("BL", robot.driveBackLeft.getPower());
+        engine.telemetry.addData("BR", robot.driveBackRight.getPower());
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java

@@ -0,0 +1,35 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+
+public class ServoPosTest extends CyberarmState {
+
+    private Servo servo;
+    private double servoPos;
+
+    public ServoPosTest() {
+    }
+
+    @Override
+    public void init() {
+        servo = engine.hardwareMap.servo.get("look");
+    }
+
+    @Override
+    public void exec() {
+        servoPos = engine.gamepad1.right_stick_y;
+        servo.setPosition(servoPos);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Position", servoPos);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java

@@ -0,0 +1,19 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+@TeleOp (name = "Hardware test", group = "test")
+public class TestingEngine extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void setup() {
+        addState(new ServoPosTest());
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WobbleArmTest.java

@@ -0,0 +1,30 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+
+public class WobbleArmTest extends CyberarmState {
+
+    private int currentPos;
+    private DcMotor motor;
+
+    @Override
+    public void init() {
+        motor = engine.hardwareMap.dcMotor.get("wobbleArm");
+        motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    @Override
+    public void exec() {
+        currentPos = motor.getCurrentPosition();
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Position", currentPos);
+    }
+}